chain works now (pairwise)

applications/reconstruct/include/ftl/registration.hpp

@@ -162,6 +162,11 @@ private:
 class ChessboardRegistration : public Registration {
 public:
 	ChessboardRegistration(nlohmann::json &config);
+	/** 
+	 * @brief	Creates new ChessboardRegistration or ChessboardRegistrationChain
+	 * 			object depending on chain option in config. User of the method
+	 * 			needs to free the memory.
+	 */
 	static ChessboardRegistration* create(nlohmann::json &config);
 
 	void run() override;
@@ -184,7 +189,8 @@ protected:
  */
 class ChessboardRegistrationChain : public ChessboardRegistration {
 public:
-	ChessboardRegistrationChain(nlohmann::json &config) : ChessboardRegistration(config) {}
+	ChessboardRegistrationChain(nlohmann::json &config);
+	
 	bool findTransformations(std::vector<Eigen::Matrix4f> &data) override;
 
 protected:

applications/reconstruct/src/registration.cpp

@@ -331,7 +331,7 @@ vector<bool> visited(matrix.size(), false);
 				new_edges.push_back(pair(current, i));
 			}
 		}}
-		edges.push_back(new_edges);
+		if (new_edges.size() > 0) edges.push_back(new_edges);
 	}
 
 	return visited_count == matrix.size();
@@ -377,8 +377,8 @@ void Registration::run() {
 		for (size_t i = 0; i < visible.size(); ++i) {
 		for (size_t j = 0; j < visible.size(); ++j) {
 			bool val = visible[i] && visible[j];
-			visibility_[i][j] = val;
-			visibility_[j][i] = val;
+			visibility_[i][j] = visibility_[i][j] || val;
+			visibility_[j][i] = visibility_[j][i] || val;
 		}}
 	}
 	while(processData());
@@ -516,6 +516,11 @@ bool ChessboardRegistration::findTransformations(vector<Matrix4f> &data) {
 	return true;
 }
 
+ChessboardRegistrationChain::ChessboardRegistrationChain(nlohmann::json &config) :
+							ChessboardRegistration(config) {
+		error_threshold_ = std::numeric_limits<float>::infinity();
+}
+
 bool ChessboardRegistrationChain::processData() {
 	for (auto &sample : data_ ) { sample.clear(); }
 	bool retval = isConnected(visibility_, getTargetSourceIdx(), edges_);
@@ -525,7 +530,7 @@ bool ChessboardRegistrationChain::processData() {
 		return false;
 	}
 	else{
-		LOG(INFO) << "Chain not complete ";
+		LOG(5) << "Chain not complete ";
 	}
 
 	return true;
@@ -534,11 +539,16 @@ bool ChessboardRegistrationChain::processData() {
 bool ChessboardRegistrationChain::findTransformations(vector<Matrix4f> &data) {
 	// TODO	Change to group registration: register all sources which have visibility
 	//		to the target source in chain.
+
 	LOG(INFO) << "Running pairwise registration";
 	data = vector<Matrix4f>(getSourcesCount(), Matrix4f::Identity());
 
 	for (vector<pair<size_t, size_t>> level : edges_) {
 		for (pair<size_t, size_t> edge : level) {
+			LOG(INFO) 	<< "Registering source "
+						<< getSource(edge.second)->getURI() << " to source"
+						<< getSource(edge.first)->getURI();
+			
 			nlohmann::json conf(config_);
 			conf["targetsource"] = getSource(edge.first)->getURI();
 			conf["chain"] = false;

components/rgbd-sources/include/ftl/rgbd_source.hpp

@@ -29,7 +29,7 @@ class RGBDSource : public ftl::Configurable {
 	void getRGBD(cv::Mat &rgb, cv::Mat &depth);
 
 	const CameraParameters &getParameters() { return params_; };
-	std::string getURI() const { return config_["uri"].get<std::string>(); }
+	std::string getURI() const { return config_["$id"].get<std::string>(); }
 
 	virtual void setPose(const Eigen::Matrix4f &pose) { pose_ = pose; };
 	const Eigen::Matrix4f &getPose() { return pose_; };