various fixes; registration now works as before

applications/reconstruct/include/ftl/registration.hpp

@@ -57,7 +57,8 @@ public:
 
 	/**
 	 * @brief	Find registration transformations. run() must be called before
-	 * 			findTransformations(). Keys in map are source URIs.
+	 * 			findTransformations(). Transformations are in same order as
+	 * 			sources were added with addSource().
 	 * 
 	 * Transformations are calculated to targetsource if configured. If
 	 * targetsource is not configured or is not found in inputs, which target
@@ -66,7 +67,12 @@ public:
 	 * @param	Output parameter for transformations.
 	 * @return	True if transformations found, otherwise false.
 	 */
-	virtual bool findTransformations(std::map<std::string, Eigen::Matrix4f> &data)=0;
+	virtual bool findTransformations(std::vector<Eigen::Matrix4f> &data)=0;
+	
+	/**
+	 * @brief	Overload of findTransformations(). Map keys are source URIs.
+	 */
+	virtual bool findTransformations(std::map<std::string, Eigen::Matrix4f> &data);
 
 protected:
 	ftl::rgbd::RGBDSource* getSource(size_t idx);
@@ -135,7 +141,7 @@ public:
 	ChessboardRegistration(nlohmann::json &config);
 
 	void run() override;
-	bool findTransformations(std::map<std::string, Eigen::Matrix4f> &data) override;
+	bool findTransformations(std::vector<Eigen::Matrix4f> &data) override;
 
 protected:
 	bool findFeatures(ftl::rgbd::RGBDSource* source, size_t idx) override;

applications/reconstruct/src/main.cpp

@@ -124,15 +124,23 @@ static void run(ftl::Configurable *root) {
 		if ((*merge)["register"]) {
 			LOG(INFO) << "Registration requested";
 
-			auto reg = ftl::registration::ChessboardRegistration(*merge);
+			ftl::registration::Registration *reg = new ftl::registration::ChessboardRegistration(*merge);
 			for (auto &input : inputs) { 
 				while(!input.source->isReady()) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); }
-				reg.addSource(input.source);
+				reg->addSource(input.source);
 			}
 			
-			reg.run();
-			reg.findTransformations(transformations);
-			saveTransformations(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json", transformations);
+			reg->run();
+			if (reg->findTransformations(transformations)) {
+				if (!saveTransformations(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json", transformations)) {
+					LOG(ERROR) << "Error saving new registration";
+				};
+			}
+			else {
+				LOG(ERROR) << "Registration failed";
+			}
+
+			free(reg);
 		}
 		else {
 			if (loadTransformations(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json", transformations)) {

applications/reconstruct/src/registration.cpp

@@ -40,7 +40,7 @@ using pcl::PointXYZRGB;
 using cv::Mat;
 using Eigen::Matrix4f;
 
-void to_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
+void from_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
 	for (auto it = json.begin(); it != json.end(); ++it) {
 		Eigen::Matrix4f m;
 		auto data = m.data();
@@ -49,7 +49,7 @@ void to_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
 	}
 }
 
-void from_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
+void to_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
 	for (auto &item : transformations) {
 		auto val = nlohmann::json::array();
 		for(size_t i = 0; i < 16; i++) { val.push_back((float) item.second.data()[i]); }
@@ -80,7 +80,7 @@ bool saveTransformations(const string &path, map<string, Matrix4f> &data) {
 		return false;
 	}
 
-	file << data_json;
+	file << std::setw(4) << data_json;
 	return true;
 }
 
@@ -127,7 +127,7 @@ float fitPlaneError(PointCloud<PointXYZ>::Ptr cloud_in, float distance_threshold
 		score += d * d;
 	}
 	
-	return score / cloud_proj.size();
+	return (score / cloud_proj.size()) * 10000000.0f;
 }
 
 //template<typename T = PointXYZ> typename
@@ -321,22 +321,35 @@ void Registration::resetVisibility() {
 }
 
 void Registration::run() {
-	if (visibility_.size() != sources_.size()) { resetVisibility(); }
+	resetVisibility();
 
 	do {
 		vector<bool> visible(sources_.size(), false);
 
 		for (size_t i = 0; i < sources_.size(); ++i) {
 			bool retval = findFeatures(sources_[i], i);
-
 			visible[i] = retval;
+		}
+
+		for (size_t i = 0; i < visible.size(); ++i) {
 		for (size_t j = 0; j < visible.size(); ++j) {
-				visibility_[i][j] = retval;
-				visibility_[j][i] = retval;
+			bool val = visible[i] && visible[j];
+			visibility_[i][j] = val;
+			visibility_[j][i] = val;
+		}}
 	}
+	while(processData());
 }
+
+bool Registration::findTransformations(map<string, Matrix4f> &data) {
+	vector<Matrix4f> T;
+	data.clear();
+
+	if (!findTransformations(T)) return false;
+	for (size_t i = 0; i < sources_.size(); ++i) {
+		data[sources_[i]->getURI()] = T[i];
 	}
-	while(processData());
+	return true;
 }
 
 ChessboardRegistration::ChessboardRegistration(nlohmann::json &config) :
@@ -366,7 +379,9 @@ bool ChessboardRegistration::findFeatures(RGBDSource *source, size_t idx) {
 	source->getRGBD(rgb, depth);
 
 	bool retval = findChessboardCorners(rgb, depth, source->getParameters(), pattern_size_, cloud, error_threshold_);
-	if (retval) { result = cloud; }
+	if (retval) {
+		result.emplace(cloud);
+	}
 	data_[idx].push_back(result);
 	
 	cv::imshow("Registration: " + source->getURI(), rgb);
@@ -380,15 +395,16 @@ bool ChessboardRegistration::processData() {
 	//			smallest subset which is connected.
 
 	bool retval = connectedVisibility();
+	resetVisibility();
+
 	if (retval) {
 		iter_remaining_--;
-		resetVisibility();
 	}
 	else{
 		// remove samples and try again
 		//	TODO: decide what to do in findTransformations() instead
 		LOG(INFO) << "Pattern not visible in all inputs";
-		for (auto sample : data_) { sample.pop_back(); }
+		for (auto &sample : data_) { sample.pop_back(); }
 	}
 	
 	//std::this_thread::sleep_for(std::chrono::milliseconds(delay_));
@@ -420,7 +436,8 @@ void ChessboardRegistration::run() {
 	}
 }
 
-bool ChessboardRegistration::findTransformations(map<string, Matrix4f> &data) {
+bool ChessboardRegistration::findTransformations(vector<Matrix4f> &data) {
+	data.clear();
 	size_t idx_target = getTargetSourceIdx();
 
 	for (size_t idx = 0; idx < getSourcesCount(); ++idx) {
@@ -445,7 +462,7 @@ bool ChessboardRegistration::findTransformations(map<string, Matrix4f> &data) {
 			T = findTransformation(d, d_target); 
 		}
 
-		data[getSource(idx)->getURI()] = T;
+		data.push_back(T);
 	}
 	return true;
 }