Implements #82 registration app

CMakeLists.txt

@@ -186,6 +186,10 @@ if (BUILD_VISION)
 	add_subdirectory(applications/vision)
 endif()
 
+if (HAVE_PCL)
+	add_subdirectory(applications/registration)
+endif()
+
 if (BUILD_RECONSTRUCT AND HAVE_PCL)
 	add_subdirectory(applications/reconstruct)
 endif()

applications/reconstruct/src/registration.cpp

@@ -233,7 +233,7 @@ Eigen::Matrix4f findTransformation(vector<PointCloud<PointXYZ>::Ptr> clouds_sour
 		// score new transformation
 		double score = 0.0;
 		for (size_t j = 0; j < n_clouds; ++j) {
-			score += validate.validateTransformation(clouds_source[j], clouds_target[j], T);
+			score += validate.validateTransformation(clouds_source[j], clouds_target[j], T); // CHECK Is use of T here a mistake??
 		}
 		score /= n_clouds;
 		

applications/reconstruct/src/virtual_source.cpp

@@ -19,8 +19,8 @@ VirtualSource::VirtualSource(ftl::rgbd::Source *host)
 	params_.fy = params_.fx;
 	params_.width = rays_->value("width", 640);
 	params_.height = rays_->value("height", 480);
-	params_.cx = params_.width / 2;
-	params_.cy = params_.height / 2;
+	params_.cx =  -((double)params_.width / 2);
+	params_.cy = -((double)params_.height / 2);
 	params_.maxDepth = rays_->value("max_depth", 10.0f);
 	params_.minDepth = rays_->value("min_depth", 0.1f);
 
@@ -43,8 +43,8 @@ bool VirtualSource::grab() {
 		DepthCameraParams params;
 		params.fx = params_.fx;
 		params.fy = params_.fy;
-		params.mx = params_.cx;
-		params.my = params_.cy;
+		params.mx = -params_.cx;
+		params.my = -params_.cy;
 		params.m_imageWidth = params_.width;
 		params.m_imageHeight = params_.height;
 		params.m_sensorDepthWorldMin = params_.minDepth;

applications/registration/CMakeLists.txt

+set(REGSRC
+	src/main.cpp
+	src/manual.cpp
+	src/correspondances.cpp
+)
+
+add_executable(ftl-register ${REGSRC})
+
+target_include_directories(ftl-register PRIVATE src)
+
+target_link_libraries(ftl-register ftlcommon ftlnet ftlrgbd Threads::Threads ${OpenCV_LIBS})
+

applications/registration/src/correspondances.cpp

+#include "correspondances.hpp"
+#include <nlohmann/json.hpp>
+
+using ftl::registration::Correspondances;
+using std::string;
+using std::map;
+using std::vector;
+using ftl::rgbd::Source;
+using pcl::PointCloud;
+using pcl::PointXYZ;
+using pcl::PointXYZRGB;
+using nlohmann::json;
+using std::tuple;
+using std::make_tuple;
+
+
+Correspondances::Correspondances(Source *src, Source *targ)
+	:	parent_(nullptr), targ_(targ), src_(src),
+		targ_cloud_(new PointCloud<PointXYZ>),
+		src_cloud_(new PointCloud<PointXYZ>), uptodate_(true) {
+	transform_.setIdentity();
+}
+
+Correspondances::Correspondances(Correspondances *parent, Source *src)
+	:	parent_(parent), targ_(parent_->source()), src_(src),
+		targ_cloud_(new PointCloud<PointXYZ>),
+		src_cloud_(new PointCloud<PointXYZ>), uptodate_(true) {
+	transform_.setIdentity();
+}
+
+bool Correspondances::add(int tx, int ty, int sx, int sy) {
+	Eigen::Vector4f p1 = targ_->point(tx,ty);
+	Eigen::Vector4f p2 = src_->point(sx,sy);
+	PointXYZ pcl_p1;
+	pcl_p1.x = p1[0];
+	pcl_p1.y = p1[1];
+	pcl_p1.z = p1[2];
+
+	PointXYZ pcl_p2;
+	pcl_p2.x = p2[0];
+	pcl_p2.y = p2[1];
+	pcl_p2.z = p2[2];
+
+	if (pcl_p2.z >= 40.0f || pcl_p1.z >= 40.0f) {
+		LOG(WARNING) << "Bad point choosen";
+		return false;
+	}
+
+	targ_cloud_->push_back(pcl_p1);
+	src_cloud_->push_back(pcl_p2);
+	log_.push_back(make_tuple(tx,ty,sx,sy));
+
+	uptodate_ = false;
+	return true;
+}
+
+void Correspondances::removeLast() {
+	uptodate_ = false;
+	targ_cloud_->erase(targ_cloud_->end()-1);
+	src_cloud_->erase(src_cloud_->end()-1);
+	log_.pop_back();
+}
+
+double Correspondances::estimateTransform() {
+	uptodate_ = true;
+	int n_points = targ_cloud_->size();
+
+	vector<int> idx(n_points);
+	for (int i = 0; i < n_points; i++) { idx[i] = i; }
+
+	pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
+	pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
+
+	svd.estimateRigidTransformation(*src_cloud_, idx, *targ_cloud_, idx, transform_);
+
+	return validate.validateTransformation(src_cloud_, targ_cloud_, transform_);
+}
+
+Eigen::Matrix4f Correspondances::transform() {
+	if (!uptodate_) estimateTransform();
+	return (parent_) ? parent_->transform() * transform_ : transform_;
+}

applications/registration/src/correspondances.hpp

+#ifndef _FTL_REG_CORRESPONDANCES_HPP_
+#define _FTL_REG_CORRESPONDANCES_HPP_
+
+#include <ftl/rgbd/source.hpp>
+#include <nlohmann/json.hpp>
+
+#include <pcl/common/transforms.h>
+
+#include <pcl/registration/transformation_estimation_svd.h>
+#include <pcl/registration/transformation_estimation_svd_scale.h>
+#include <pcl/registration/transformation_validation.h>
+#include <pcl/registration/transformation_validation_euclidean.h>
+
+#include <vector>
+#include <string>
+#include <map>
+#include <tuple>
+
+namespace ftl {
+namespace registration {
+
+/**
+ * Manage the identified set of correspondances between two sources. There may
+ * also be a parent correspondances object to support the chaining of
+ * transforms.
+ */
+class Correspondances {
+	public:
+	Correspondances(ftl::rgbd::Source *src, ftl::rgbd::Source *targ);
+	Correspondances(Correspondances *parent, ftl::rgbd::Source *src);
+
+	ftl::rgbd::Source *source() { return src_; }
+	ftl::rgbd::Source *target() { return targ_; }
+
+	/**
+	 * Add a new correspondance point. The point will only be added if there is
+	 * a valid depth value at that location.
+	 * 
+	 * @param tx X-Coordinate in target image
+	 * @param ty Y-Coordinate in target image
+	 * @param sx X-Coordinate in source image
+	 * @param sy Y-Coordinate in source image
+	 * @return False if point was invalid and not added.
+	 */
+	bool add(int tx, int ty, int sx, int sy);
+
+	void removeLast();
+
+	const std::vector<std::tuple<int,int,int,int>> &screenPoints() const { return log_; }
+
+	/**
+	 * Calculate a transform using current set of correspondances.
+	 * 
+	 * @return Validation score of the transform.
+	 */
+	double estimateTransform();
+
+	/**
+	 * Get the estimated transform. This includes any parent transforms to make
+	 * it relative to root camera.
+	 */
+	Eigen::Matrix4f transform();
+
+	private:
+	Correspondances *parent_;
+	ftl::rgbd::Source *targ_;
+	ftl::rgbd::Source *src_;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud_;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud_;
+	Eigen::Matrix4f transform_;
+	bool uptodate_;
+	std::vector<std::tuple<int,int,int,int>> log_;
+};
+
+}
+}
+
+#endif  // _FTL_REG_CORRESPONDANCES_HPP_

applications/registration/src/main.cpp

+#include <loguru.hpp>
+#include <ftl/configuration.hpp>
+#include <string>
+
+#include "manual.hpp"
+
+using std::string;
+
+int main(int argc, char **argv) {
+	auto root = ftl::configure(argc, argv, "registration_default");
+
+	// TODO
+	ftl::registration::manual(root);
+	if (root->value("mode", string("manual") == "manual")) {
+		//ftl::registration::manual(root);
+	}
+
+	return 0;
+}

applications/registration/src/manual.cpp

+#include "manual.hpp"
+#include "correspondances.hpp"
+
+#include <loguru.hpp>
+
+#include <ftl/net/universe.hpp>
+#include <ftl/rgbd/source.hpp>
+
+#include <opencv2/core.hpp>
+#include <opencv2/core/utility.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/videoio.hpp>
+#include <opencv2/highgui.hpp>
+
+#include <map>
+
+using std::string;
+using std::vector;
+using std::pair;
+using std::map;
+
+using ftl::net::Universe;
+using ftl::rgbd::Source;
+
+using cv::Mat;
+using cv::Point;
+using cv::Scalar;
+
+using namespace ftl::registration;
+
+using MouseAction = std::function<void(int, int, int, int)>;
+
+static void setMouseAction(const std::string& winName, const MouseAction &action)
+{
+	cv::setMouseCallback(winName,
+						[] (int event, int x, int y, int flags, void* userdata) {
+	(*(MouseAction*)userdata)(event, x, y, flags);
+	}, (void*)&action);
+}
+
+static MouseAction tmouse;
+static MouseAction smouse;
+
+
+static void to_json(nlohmann::json &json, map<string, Eigen::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]); }
+		json[item.first] = val;
+	}
+}
+
+static bool saveTransformations(const string &path, map<string, Eigen::Matrix4f> &data) {
+	nlohmann::json data_json;
+	to_json(data_json, data);
+	std::ofstream file(path);
+
+	if (!file.is_open()) {
+		LOG(ERROR) << "Error writing transformations to file " << path;
+		return false;
+	}
+
+	file << std::setw(4) << data_json;
+	return true;
+}
+
+static void build_correspondances(const vector<Source*> &sources, map<string, Correspondances*> &cs, int origin) {
+	Correspondances *last = nullptr;
+
+	cs[sources[origin]->getURI()] = nullptr;
+
+	for (int i=origin-1; i>=0; i--) {
+		if (last == nullptr) {
+			auto *c = new Correspondances(sources[i], sources[origin]);
+			last = c;
+			cs[sources[i]->getURI()] = c;
+		} else {
+			auto *c = new Correspondances(last, sources[i]);
+			last = c;
+			cs[sources[i]->getURI()] = c;
+		}
+	}
+
+	last = nullptr;
+
+	for (int i=origin+1; i<sources.size(); i++) {
+		if (last == nullptr) {
+			auto *c = new Correspondances(sources[i], sources[origin]);
+			last = c;
+			cs[sources[i]->getURI()] = c;
+		} else {
+			auto *c = new Correspondances(last, sources[i]);
+			last = c;
+			cs[sources[i]->getURI()] = c;
+		}
+	}
+}
+
+void ftl::registration::manual(ftl::Configurable *root) {
+	using namespace cv;
+	
+	Universe *net = ftl::create<Universe>(root, "net");
+
+	net->start();
+	net->waitConnections();
+
+	auto sources = ftl::createArray<Source>(root, "sources", net);
+
+	int curtarget = 0;
+	bool freeze = false;
+
+	vector<Mat> rgb;
+	vector<Mat> depth;
+
+	if (sources.size() == 0) return;
+
+	rgb.resize(sources.size());
+	depth.resize(sources.size());
+
+	cv::namedWindow("Target", cv::WINDOW_KEEPRATIO);
+	cv::namedWindow("Source", cv::WINDOW_KEEPRATIO);
+
+	//Correspondances c(sources[targsrc], sources[cursrc]);
+	map<string, Correspondances*> corrs;
+	build_correspondances(sources, corrs, root->value("origin", 0));
+
+	int lastTX = 0;
+	int lastTY = 0;
+	int lastSX = 0;
+	int lastSY = 0;
+
+	tmouse = [&]( int event, int ux, int uy, int) {
+		if (event == 1) {   // click
+			lastTX = ux;
+			lastTY = uy;
+		}
+	};
+	setMouseAction("Target", tmouse);
+
+	smouse = [&]( int event, int ux, int uy, int) {
+		if (event == 1) {   // click
+			lastSX = ux;
+			lastSY = uy;
+		}
+	};
+	setMouseAction("Source", smouse);
+
+	bool depthtoggle = false;
+	double lastScore = 0.0;
+
+	Correspondances *current = corrs[sources[curtarget]->getURI()];
+
+	while (ftl::running) {
+		if (!freeze) {
+			// Grab the latest frames from sources
+			for (int i=0; i<sources.size(); i++) {
+				if (sources[i]->isReady()) {
+					sources[i]->grab();
+				}
+			}
+		}
+
+		// Get the raw rgb and depth frames from sources
+		Mat ttrgb, trgb, tdepth, ttdepth;
+		Mat tsrgb, srgb, sdepth, tsdepth;
+
+		if (current == nullptr) {
+			srgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
+			trgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
+
+			putText(srgb, string("No correspondance for ") + sources[curtarget]->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+
+			if (!trgb.empty()) imshow("Target", trgb);
+			if (!srgb.empty()) imshow("Source", srgb);
+		} else if (current->source()->isReady() && current->target()->isReady()) {
+			current->source()->getFrames(tsrgb, tsdepth);
+			current->target()->getFrames(ttrgb, ttdepth);
+			tsrgb.copyTo(srgb);
+			ttrgb.copyTo(trgb);
+			ttdepth.convertTo(tdepth, CV_8U, 255.0f / 10.0f);
+			applyColorMap(tdepth, tdepth, cv::COLORMAP_JET);
+			tsdepth.convertTo(sdepth, CV_8U, 255.0f / 10.0f);
+			applyColorMap(sdepth, sdepth, cv::COLORMAP_JET);
+
+			// Apply points and labels...
+			auto &points = current->screenPoints();
+			for (auto &p : points) {
+				auto [tx,ty,sx,sy] = p;
+				drawMarker(srgb, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
+				drawMarker(sdepth, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
+				drawMarker(trgb, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
+				drawMarker(tdepth, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
+			}
+
+			// Add most recent click position
+			drawMarker(srgb, Point(lastSX, lastSY), Scalar(0,0,255), MARKER_TILTED_CROSS);
+			drawMarker(trgb, Point(lastTX, lastTY), Scalar(0,0,255), MARKER_TILTED_CROSS);
+			drawMarker(sdepth, Point(lastSX, lastSY), Scalar(0,0,255), MARKER_TILTED_CROSS);
+			drawMarker(tdepth, Point(lastTX, lastTY), Scalar(0,0,255), MARKER_TILTED_CROSS);
+
+			putText(srgb, string("Source: ") + current->source()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+			putText(trgb, string("Target: ") + current->target()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+			putText(srgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+			putText(trgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+			if (freeze) putText(srgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+			if (freeze) putText(trgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
+
+			if (!trgb.empty()) imshow("Target", (depthtoggle) ? tdepth : trgb);
+			if (!srgb.empty()) imshow("Source", (depthtoggle) ? sdepth : srgb);
+		}
+	
+		int key = cv::waitKey(20);
+		if (key == 27) break;
+		else if (key >= 48 && key <= 57) {
+			curtarget = key-48;
+			if (curtarget >= sources.size()) curtarget = 0;
+			current = corrs[sources[curtarget]->getURI()];
+		} else if (key == 'd') depthtoggle = !depthtoggle;
+		else if (key == 'a') {
+			if (current->add(lastTX,lastTY,lastSX,lastSY)) {
+				lastTX = lastTY = lastSX = lastSY = 0;
+				lastScore = current->estimateTransform();
+			}
+		} else if (key == 'u') {
+			current->removeLast();
+			lastScore = current->estimateTransform();
+		} else if (key == 's') {
+			// Save
+			map<string, Eigen::Matrix4f> transforms;
+			//transforms[sources[targsrc]->getURI()] = Eigen::Matrix4f().setIdentity();
+			//transforms[sources[cursrc]->getURI()] = c.transform();
+
+			for (auto x : corrs) {
+				if (x.second == nullptr) {
+					transforms[x.first] = Eigen::Matrix4f().setIdentity();
+				} else {
+					transforms[x.first] = x.second->transform();
+				}
+			}
+
+			saveTransformations(root->value("output", string("./test.json")), transforms);
+			LOG(INFO) << "Saved!";
+		}
+		else if (key == 32) freeze = !freeze;
+	}
+
+	// store transformations in map<string Matrix4f>
+
+	// TODO:	(later) extract features and correspondencies from complete cloud
+	//			and do ICP to find best possible transformation
+
+	// saveTransformations(const string &path, map<string, Matrix4f> &data)
+}

applications/registration/src/manual.hpp

+#ifndef _FTL_REGISTRATION_MANUAL_HPP_
+#define _FTL_REGISTRATION_MANUAL_HPP_
+
+#include <ftl/configurable.hpp>
+
+namespace ftl {
+namespace registration {
+
+void manual(ftl::Configurable *root);
+
+}
+}
+
+#endif  // _FTL_REGISTRATION_MANUAL_HPP_

components/rgbd-sources/src/source.cpp

@@ -149,8 +149,8 @@ void Source::getFrames(cv::Mat &rgb, cv::Mat &depth) {
 
 Eigen::Vector4f Source::point(uint ux, uint uy) {
 	const auto &params = parameters();
-	const float x = ((float)ux-(float)params.cx) / (float)params.fx;
-	const float y = ((float)uy-(float)params.cy) / (float)params.fy;
+	const float x = ((float)ux+(float)params.cx) / (float)params.fx;
+	const float y = ((float)uy+(float)params.cy) / (float)params.fy;
 
 	SHARED_LOCK(mutex_,lk);
 	const float depth = depth_.at<float>(uy,ux);

components/rgbd-sources/src/streamer.cpp

@@ -266,7 +266,11 @@ void Streamer::_schedule() {
 		// Grab job
 		pool_.push([this,src](int id) {
 			//StreamSource *src = sources_[uri];
-			src->src->grab();
+			try {
+				src->src->grab();
+			} catch (...) {
+				LOG(ERROR) << "Exception when grabbing frame";
+			}
 
 			// CHECK (Nick) Can state be an atomic instead?
 			//UNIQUE_LOCK(src->mutex, lk);

config/config_nick.jsonc

@@ -171,11 +171,13 @@
 
 	"registration_default": {
 		"net": {
-			"peers": ["tcp://ftl-node-2:9001"]
+			"peers": []
 		},
 		"sources": [
-			{"uri":"ftl://utu.fi/node2#vision_default/source"}
-		]
+			{"uri":"file:///home/nick/Pictures/FTL/snap.tar.gz#0", "index": 0},
+			{"uri":"file:///home/nick/Pictures/FTL/snap.tar.gz#1", "index": 1}
+		],
+		"origin": 0
 	},
 
 	"reconstruction_rs": {