From 33844b24a38dd0b518910d85d0f76a16ca92ed9b Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Thu, 6 Jun 2019 16:50:28 +0300
Subject: [PATCH] camera chaining

---
 .../reconstruct/include/ftl/registration.hpp  |  45 ++++-
 applications/reconstruct/src/main.cpp         |   2 +-
 applications/reconstruct/src/registration.cpp | 180 +++++++++++++-----
 config/config.jsonc                           |   3 +-
 4 files changed, 178 insertions(+), 52 deletions(-)

diff --git a/applications/reconstruct/include/ftl/registration.hpp b/applications/reconstruct/include/ftl/registration.hpp
index c73b71012..500616b53 100644
--- a/applications/reconstruct/include/ftl/registration.hpp
+++ b/applications/reconstruct/include/ftl/registration.hpp
@@ -126,19 +126,43 @@ protected:
 	 */
 	virtual bool findFeatures(ftl::rgbd::RGBDSource* source, size_t idx)=0;
 
+	std::vector<std::vector<bool>> visibility_; /*< Adjacency matrix for sources (feature visibility). */
+
 private:
 	std::optional<std::string> target_source_; /*< Reference coordinate system for transformations. */
 	std::vector<ftl::rgbd::RGBDSource*> sources_;
-	std::vector<std::vector<bool>> visibility_; /*< Adjacency matrix for sources (feature visibility). */
 };
 
 /**
  * @brief	Registration using chessboard calibration pattern
- * @todo	Document chessboard parameters
+ * 
+ * Parameters from configuration:
+ * 
+ * 		patternsize: required
+ * 			Chessboard pattern size, inner corners.
+ * 
+ * 		maxerror: default +inf
+ * 			Maximum allowed error value for pattern detection. MSE error between
+ * 			estimated plane and points captured from input.
+ * 
+ * 		delay:	default 500
+ * 			Milliseconds between captured images.
+ * 
+ * 		chain: default false
+ * 			Enabling allows camera chaining. In chain mode, pattern is not
+ * 			required to be visible in every source. In default (chain: false)
+ * 			mode, pattern visibility is required for every source.
+ * 
+ * 		iter: default 10
+ * 			Number of iterations for capturing calibration samples. In
+ * 			non-chaining mode, each iteration consists of images where patterns
+ * 			were detected on every input. In chaining mode each iteration only
+ * 			requires camera visibility to be connected.
  */
 class ChessboardRegistration : public Registration {
 public:
 	ChessboardRegistration(nlohmann::json &config);
+	static ChessboardRegistration* create(nlohmann::json &config);
 
 	void run() override;
 	bool findTransformations(std::vector<Eigen::Matrix4f> &data) override;
@@ -146,16 +170,29 @@ public:
 protected:
 	bool findFeatures(ftl::rgbd::RGBDSource* source, size_t idx) override;
 	bool processData() override;
-
-private:
 	cv::Size pattern_size_;
 	std::vector<std::vector<std::optional<pcl::PointCloud<pcl::PointXYZ>::Ptr>>> data_;
+	std::vector<Eigen::Matrix4f> T_;
 	float error_threshold_;
 	uint delay_;
 	uint iter_;
 	uint iter_remaining_;
 };
 
+/**
+ * @brief Chain registration. Finds visibility and then runs pairwise registration.
+ */
+class ChessboardRegistrationChain : public ChessboardRegistration {
+public:
+	ChessboardRegistrationChain(nlohmann::json &config) : ChessboardRegistration(config) {}
+	bool findTransformations(std::vector<Eigen::Matrix4f> &data) override;
+
+protected:
+	bool processData() override;
+	std::vector<Eigen::Matrix4f> T_;
+	std::vector<std::vector<std::pair<size_t, size_t>>> edges_;
+};
+
 }
 }
 
diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index ca4630ecf..4ebc6fbd3 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -124,7 +124,7 @@ static void run(ftl::Configurable *root) {
 		if ((*merge)["register"]) {
 			LOG(INFO) << "Registration requested";
 
-			ftl::registration::Registration *reg = new ftl::registration::ChessboardRegistration(*merge);
+			ftl::registration::Registration *reg = ftl::registration::ChessboardRegistration::create(*merge);
 			for (auto &input : inputs) { 
 				while(!input.source->isReady()) { std::this_thread::sleep_for(std::chrono::milliseconds(50)); }
 				reg->addSource(input.source);
diff --git a/applications/reconstruct/src/registration.cpp b/applications/reconstruct/src/registration.cpp
index 55891d57f..abe5f95b2 100644
--- a/applications/reconstruct/src/registration.cpp
+++ b/applications/reconstruct/src/registration.cpp
@@ -30,6 +30,7 @@ using ftl::rgbd::RGBDSource;
 
 using std::string;
 using std::vector;
+using std::pair;
 using std::map;
 using std::optional;
 
@@ -295,25 +296,67 @@ void Registration::addSource(RGBDSource *source) {
 	sources_.push_back(source);
 }
 
-bool Registration::connectedVisibility() {
-	vector<bool> visited(visibility_.size(), false);
-	std::queue<size_t> next;
-
-	visited[0] = true;
+/**
+ * @param	adjacency matrix
+ * @param	index of starting vertex
+ * @param	(out) edges connecting each level
+ * @returns	true if graph connected (all vertices visited), otherwise false
+ * 
+ * Breadth First Search
+ */
+bool isConnected(vector<vector<bool>> matrix, size_t start_idx, vector<vector<pair<size_t, size_t>>> &edges) {
+vector<bool> visited(matrix.size(), false);
+	DCHECK(start_idx < matrix.size());
+
+	edges.clear();
+	vector<size_t> level { start_idx };
+
+	visited[start_idx] = true;
 	size_t visited_count = 1;
-	next.push(0);
 	
-	while(next.size() > 0) {
-		size_t current = next.front(); next.pop();
-		for (size_t i = 0; i < visibility_.size(); ++i) {
-			if (visibility_[current][i] && !visited[i]) {
+	while(level.size() != 0) {
+		vector<size_t> level_prev = level;
+		level = {};
+
+		vector<pair<size_t, size_t>> new_edges;
+
+		for (size_t current : level_prev) {
+		for (size_t i = 0; i < matrix.size(); ++i) {
+			if (matrix[current][i] && !visited[i]) {
 				visited[i] = true;
 				visited_count += 1;
-				next.push(i);
+				level.push_back(i);
+				// could also save each level's vertices
+
+				new_edges.push_back(pair(current, i));
 			}
-		}
+		}}
+		edges.push_back(new_edges);
 	}
-	return visited_count == visibility_.size();
+
+	return visited_count == matrix.size();
+}
+
+bool isConnected(vector<vector<bool>> matrix, size_t start_idx = 0) {
+	vector<vector<pair<size_t, size_t>>> edges;
+	return isConnected(matrix, start_idx, edges);
+}
+
+/**
+ * @param	Adjacency matrix
+ * @returns	Vector containing degree of each vertex
+*/
+vector<uint> verticleDegrees(vector<vector<bool>> matrix) {
+	vector<uint> res(matrix.size(), 0);
+	for (size_t i = 0; i < matrix.size(); ++i) {
+	for (size_t j = 0; j < matrix.size(); ++j) {
+		if (matrix[i][j]) res[i] = res[i] + 1;
+	}}
+	return res;
+}
+
+bool Registration::connectedVisibility() {
+	return isConnected(visibility_, getTargetSourceIdx());
 }
 
 void Registration::resetVisibility() {
@@ -352,6 +395,15 @@ bool Registration::findTransformations(map<string, Matrix4f> &data) {
 	return true;
 }
 
+ChessboardRegistration* ChessboardRegistration::create(nlohmann::json &config) {
+	if (config.value<bool>("chain", false)) {
+		return new ChessboardRegistrationChain(config);
+	}
+	else {
+		return new ChessboardRegistration(config);
+	}
+}
+
 ChessboardRegistration::ChessboardRegistration(nlohmann::json &config) :
 	Registration(config) {
 	
@@ -365,10 +417,36 @@ ChessboardRegistration::ChessboardRegistration(nlohmann::json &config) :
 	if (!delay) { LOG(INFO) << "delay not set in configuration"; }
 	auto iter = get<int>("iterations");
 	if (!iter) { LOG(INFO) << "iterations not set in configuration"; }
-	
+	auto chain = get<bool>("chain");
+	if (!chain) { LOG(INFO) << "input chaining disabled"; }
+	else { LOG(INFO) << "Input chaining enabled"; }
+
 	error_threshold_ = maxerror ? *maxerror : std::numeric_limits<float>::infinity();
 	iter_ = iter ? *iter : 10;
-	delay_ = delay ? *delay : 50; 
+	delay_ = delay ? *delay : 50;
+}
+
+void ChessboardRegistration::run() {
+	if (!isTargetSourceFound()) {
+		LOG(WARNING) << "targetsource not found in sources";
+	}
+
+	if (data_.size() != getSourcesCount()) {
+		data_ = vector<vector<optional<PointCloud<PointXYZ>::Ptr>>>(getSourcesCount());
+	}
+	iter_remaining_ = iter_;
+
+	// TODO: Move GUI elsewhere. Also applies to processData() and findFeatures()
+	for (size_t i = 0; i < getSourcesCount(); ++i) { 
+		cv::namedWindow("Registration: " + getSource(i)->getURI(),
+						cv::WINDOW_KEEPRATIO|cv::WINDOW_NORMAL);
+	}
+
+	Registration::run();
+
+	for (size_t i = 0; i < getSourcesCount(); ++i) { 
+		cv::destroyWindow("Registration: " + getSource(i)->getURI());
+	}
 }
 
 bool ChessboardRegistration::findFeatures(RGBDSource *source, size_t idx) {
@@ -390,10 +468,6 @@ bool ChessboardRegistration::findFeatures(RGBDSource *source, size_t idx) {
 }
 
 bool ChessboardRegistration::processData() {
-	// TODO:	Check there is enough samples for each input source.
-	// TODO:	With multiple samples which are not totally connected, find the
-	//			smallest subset which is connected.
-
 	bool retval = connectedVisibility();
 	resetVisibility();
 
@@ -401,43 +475,19 @@ bool ChessboardRegistration::processData() {
 		iter_remaining_--;
 	}
 	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(); }
 	}
-	
+
 	//std::this_thread::sleep_for(std::chrono::milliseconds(delay_));
 	cv::waitKey(delay_); // OpenCV GUI doesn't show otherwise
 
 	return iter_remaining_ > 0;
 }
 
-void ChessboardRegistration::run() {
-	if (!isTargetSourceFound()) {
-		LOG(WARNING) << "targetsource not found in sources";
-	}
-
-	if (data_.size() != getSourcesCount()) {
-		data_ = vector<vector<optional<PointCloud<PointXYZ>::Ptr>>>(getSourcesCount());
-	}
-	iter_remaining_ = iter_;
-
-	// TODO: Move GUI elsewhere. Also applies to processData() and findFeatures()
-	for (size_t i = 0; i < getSourcesCount(); ++i) { 
-		cv::namedWindow("Registration: " + getSource(i)->getURI(),
-						cv::WINDOW_KEEPRATIO|cv::WINDOW_NORMAL);
-	}
-
-	Registration::run();
-
-	for (size_t i = 0; i < getSourcesCount(); ++i) { 
-		cv::destroyWindow("Registration: " + getSource(i)->getURI());
-	}
-}
-
 bool ChessboardRegistration::findTransformations(vector<Matrix4f> &data) {
 	data.clear();
+	vector<bool> status(getSourcesCount(), false);
 	size_t idx_target = getTargetSourceIdx();
 
 	for (size_t idx = 0; idx < getSourcesCount(); ++idx) {
@@ -461,12 +511,50 @@ bool ChessboardRegistration::findTransformations(vector<Matrix4f> &data) {
 			}
 			T = findTransformation(d, d_target); 
 		}
-
 		data.push_back(T);
 	}
 	return true;
 }
 
+bool ChessboardRegistrationChain::processData() {
+	for (auto &sample : data_ ) { sample.clear(); }
+	bool retval = isConnected(visibility_, getTargetSourceIdx(), edges_);
+	
+	if (retval) {
+		LOG(INFO) << "Chain complete, depth: " << edges_.size();
+		return false;
+	}
+	else{
+		LOG(INFO) << "Chain not complete ";
+	}
+
+	return true;
+}
+
+bool ChessboardRegistrationChain::findTransformations(vector<Matrix4f> &data) {
+	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) {
+			nlohmann::json conf(config_);
+			conf["targetsource"] = getSource(edge.first)->getURI();
+			conf["chain"] = false;
+
+
+			vector<Matrix4f> result;
+			ChessboardRegistration reg(conf);
+			reg.addSource(getSource(edge.first));
+			reg.addSource(getSource(edge.second));
+			reg.run();
+			if (!reg.findTransformations(result)) { return false; }
+			data[edge.second] = data[edge.first] * result[1];
+		}
+	}
+	
+	return true;
+}
+
 } // namespace registration
 } // namespace ftl
 
diff --git a/config/config.jsonc b/config/config.jsonc
index 493333317..95fc7fde6 100644
--- a/config/config.jsonc
+++ b/config/config.jsonc
@@ -171,8 +171,9 @@
 		"virtual": { "$ref": "#virtual_cams/default" },
 		"voxelhash": { "$ref": "#hash_conf/default" },
 		"merge": {
-			"register": false,
 			"targetsource" : "ftl://utu.fi/node4#vision_default/source",
+			"register": false,
+			"chain": false,
 			"maxerror": 25,
 			"iterations" : 10,
 			"delay" : 500,
-- 
GitLab