Merge branch 'feature/sfmreg' into 'master'

Add automated matching to registration app

See merge request nicolas.pope/ftl!41

applications/registration/CMakeLists.txt

@@ -2,6 +2,7 @@ set(REGSRC
 	src/main.cpp
 	src/manual.cpp
 	src/correspondances.cpp
+	src/sfm.cpp
 )
 
 add_executable(ftl-register ${REGSRC})

applications/registration/src/correspondances.cpp

 #include "correspondances.hpp"
 #include <nlohmann/json.hpp>
+#include <random>
+#include <chrono>
 
 using ftl::registration::Correspondances;
 using std::string;
@@ -54,6 +56,15 @@ bool Correspondances::add(int tx, int ty, int sx, int sy) {
 	return true;
 }
 
+PointXYZ makePCL(Source *s, int x, int y) {
+	Eigen::Vector4f p1 = s->point(x,y);
+	PointXYZ pcl_p1;
+	pcl_p1.x = p1[0];
+	pcl_p1.y = p1[1];
+	pcl_p1.z = p1[2];
+	return pcl_p1;
+}
+
 void Correspondances::removeLast() {
 	uptodate_ = false;
 	targ_cloud_->erase(targ_cloud_->end()-1);
@@ -76,6 +87,75 @@ double Correspondances::estimateTransform() {
 	return validate.validateTransformation(src_cloud_, targ_cloud_, transform_);
 }
 
+static std::default_random_engine generator;
+
+void Correspondances::convertToPCL(const std::vector<std::tuple<int,int,int,int>> &p, std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &pp) {
+	for (size_t i=0; i<p.size(); i++) {
+		auto [sx,sy,tx,ty] = p[i];
+		//LOG(INFO) << "POINT " << sx << "," << sy;
+		auto p1 = makePCL(src_, sx, sy);
+		auto p2 = makePCL(targ_, tx, ty);
+		pp.push_back(std::make_pair(p1,p2));
+	}
+}
+
+double Correspondances::findBest(Eigen::Matrix4f &tr, const std::vector<std::tuple<int,int,int,int>> &p, const std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &pp, int K, int N) {
+	double score = 10000.0f;
+	vector<tuple<int,int,int,int>> best;
+	Eigen::Matrix4f bestT;
+
+	pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
+	pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
+
+	std::mt19937 rng(std::chrono::steady_clock::now().time_since_epoch().count());
+	std::uniform_int_distribution<int> distribution(0,p.size()-1);
+	//int dice_roll = distribution(generator);
+	auto dice = std::bind ( distribution, rng );
+
+	vector<int> idx(K);
+	for (int i = 0; i < K; i++) { idx[i] = i; }
+
+	for (int n=0; n<N; n++) {
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_t(new PointCloud<PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_s(new PointCloud<PointXYZ>);
+
+		vector<tuple<int,int,int,int>> ps;
+
+		for (int k=0; k<K; k++) {
+			int r = dice();
+			//LOG(INFO) << "DICE " << r << "  -  " << K;
+			//ps.push_back(p[r]);
+			auto [sx,sy,tx,ty] = p[r];
+			//LOG(INFO) << "POINT " << sx << "," << sy;
+			//auto p1 = makePCL(src_, sx, sy);
+			//auto p2 = makePCL(targ_, tx, ty);
+
+			auto [p1,p2] = pp[r];
+
+			if (p1.z >= 30.0f || p2.z >= 30.0f) { k--; continue; }
+			ps.push_back(std::make_tuple(tx,ty,sx,sy));
+			cloud_s->push_back(p1);
+			cloud_t->push_back(p2);
+		}
+
+		Eigen::Matrix4f T;
+		svd.estimateRigidTransformation(*cloud_s, idx, *cloud_t, idx, T);
+		float scoreT = validate.validateTransformation(cloud_s, cloud_t, T);
+
+		if (scoreT < score) {
+			score = scoreT;
+			best = ps;
+			bestT = T;
+		}
+	}
+
+	// TODO Add these best points to actual clouds.
+	log_ = best;
+	tr = bestT;
+	//uptodate_ = true;
+	return score;
+}
+
 Eigen::Matrix4f Correspondances::transform() {
 	if (!uptodate_) estimateTransform();
 	return (parent_) ? parent_->transform() * transform_ : transform_;

applications/registration/src/correspondances.hpp

@@ -48,6 +48,8 @@ class Correspondances {
 
 	const std::vector<std::tuple<int,int,int,int>> &screenPoints() const { return log_; }
 
+	void setPoints(const std::vector<std::tuple<int,int,int,int>> &p) { log_ = p; }
+
 	/**
 	 * Calculate a transform using current set of correspondances.
 	 * 
@@ -55,13 +57,17 @@ class Correspondances {
 	 */
 	double estimateTransform();
 
+	double findBest(Eigen::Matrix4f &tr, const std::vector<std::tuple<int,int,int,int>> &p, const std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &p2, int K, int N);
+
+	void convertToPCL(const std::vector<std::tuple<int,int,int,int>> &p, std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &p2);
+
 	/**
 	 * Get the estimated transform. This includes any parent transforms to make
 	 * it relative to root camera.
 	 */
 	Eigen::Matrix4f transform();
 
-	void setTransform(Eigen::Matrix4f &t) { transform_ = t; }
+	void setTransform(Eigen::Matrix4f &t) { uptodate_ = true; transform_ = t; }
 
 	private:
 	Correspondances *parent_;

applications/registration/src/manual.cpp

 #include "manual.hpp"
 #include "correspondances.hpp"
+#include "sfm.hpp"
 
 #include <loguru.hpp>
 
@@ -13,6 +14,7 @@
 #include <opencv2/imgcodecs.hpp>
 #include <opencv2/videoio.hpp>
 #include <opencv2/highgui.hpp>
+#include <opencv2/xphoto.hpp>
 
 #include <map>
 
@@ -20,6 +22,7 @@ using std::string;
 using std::vector;
 using std::pair;
 using std::map;
+using std::tuple;
 
 using ftl::net::Universe;
 using ftl::rgbd::Source;
@@ -125,6 +128,32 @@ static void build_correspondances(const vector<Source*> &sources, map<string, Co
 	}
 }
 
+void averageDepth(vector<Mat> &in, Mat &out) {
+	const int rows = in[0].rows;
+	const int cols = in[0].cols;
+
+	// todo: create new only if out is empty (or wrong shape/type)
+	out = Mat(rows, cols, CV_32FC1);
+
+	for (int i = 0; i < rows * cols; ++i) {
+		float sum = 0;
+		int good_values = 0;
+		for (int i_in = 0; i_in < in.size(); ++i_in) {
+			float d = in[i_in].at<float>(i);
+			if (d < 40) {
+				good_values++;
+				sum += d;
+			}
+		}
+
+		if (good_values > 0) {
+			out.at<float>(i) = sum / (float) good_values;
+		} else {
+			out.at<float>(i) = 41.0f;
+		}
+	}
+}
+
 void ftl::registration::manual(ftl::Configurable *root) {
 	using namespace cv;
 	
@@ -179,9 +208,14 @@ void ftl::registration::manual(ftl::Configurable *root) {
 
 	bool depthtoggle = false;
 	double lastScore = 0.0;
+	bool insearch = false;
 
 	Correspondances *current = corrs[sources[curtarget]->getURI()];
 	
+	// Features for current frame...
+	vector<tuple<int,int,int,int>> feat;
+	vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> pclfeat;
+
 	while (ftl::running) {
 		if (!freeze) {
 			// Grab the latest frames from sources
@@ -207,6 +241,7 @@ void ftl::registration::manual(ftl::Configurable *root) {
 		} 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);
@@ -214,6 +249,11 @@ void ftl::registration::manual(ftl::Configurable *root) {
 			tsdepth.convertTo(sdepth, CV_8U, 255.0f / 10.0f);
 			applyColorMap(sdepth, sdepth, cv::COLORMAP_JET);
 
+			Ptr<xphoto::WhiteBalancer> wb;
+			wb = xphoto::createSimpleWB();
+			wb->balanceWhite(tsrgb, srgb);
+			wb->balanceWhite(ttrgb, trgb);
+
 			// Apply points and labels...
 			auto &points = current->screenPoints();
 			for (auto &p : points) {
@@ -247,8 +287,16 @@ void ftl::registration::manual(ftl::Configurable *root) {
 			curtarget = key-48;
 			if (curtarget >= sources.size()) curtarget = 0;
 			current = corrs[sources[curtarget]->getURI()];
-		} else if (key == 'd') depthtoggle = !depthtoggle;
-		else if (key == 'a') {
+			feat.clear();
+			pclfeat.clear();
+			lastScore = 1000.0;
+		} else if (key == 'd') {
+			depthtoggle = !depthtoggle;
+		} else if (key == 'c') {
+			feat.clear();
+			pclfeat.clear();
+			lastScore = 1000.0;
+		}else if (key == 'a') {
 			if (current->add(lastTX,lastTY,lastSX,lastSY)) {
 				lastTX = lastTY = lastSX = lastSY = 0;
 				lastScore = current->estimateTransform();
@@ -274,6 +322,83 @@ void ftl::registration::manual(ftl::Configurable *root) {
 			LOG(INFO) << "Saved!";
 		} else if (key == 't') {
 			current->source()->setPose(current->transform());
+		} else if (key == 'g') {
+			if (!insearch) {
+				insearch = true;
+				LOG(INFO) << "Features matched = " << feat.size();
+
+				ftl::pool.push([&lastScore,&insearch,current,&feat,&pclfeat](int id) {
+					LOG(INFO) << "START";
+					lastScore = 1000.0;
+					do {
+						Eigen::Matrix4f tr;
+						float s = current->findBest(tr, feat, pclfeat, 20, 100);
+						//LOG(INFO) << "SCORE = " << s;
+						if (s < lastScore) {
+							lastScore = s;
+							current->setTransform(tr);
+							current->source()->setPose(current->transform());
+						}
+					} while (ftl::running && insearch && lastScore > 0.000001);
+					insearch = false;
+					LOG(INFO) << "DONE: " << lastScore;
+				});
+				/*for (int i=0; i<feat.size(); i++) {
+					auto [sx,sy,tx,ty] = feat[i];
+					current->add(tx,ty,sx,sy);
+				}
+				lastScore = current->estimateTransform();*/
+			} else {
+				insearch = false;
+			}
+		} else if (key == 'f') {
+			Mat f1, d1, f2, d2;
+
+			size_t buffer_size = 10;
+			size_t buffer_i = 0;
+			vector<vector<Mat>> buffer(2, vector<Mat>(buffer_size));
+
+			for (size_t i = 0; i < buffer_size; ++i) {
+				current->source()->grab();
+				current->target()->grab();
+				current->source()->getFrames(f1, d1);
+				current->target()->getFrames(f2, d2);
+				buffer[0][i] = d1;
+				buffer[1][i] = d2;
+
+				std::this_thread::sleep_for(std::chrono::milliseconds(20));
+			}
+			averageDepth(buffer[0], d1);
+			averageDepth(buffer[1], d2);
+			Mat d1_v, d2_v;
+			d1.convertTo(d1_v, CV_8U, 255.0f / 10.0f);
+			d2.convertTo(d2_v, CV_8U, 255.0f / 10.0f);
+			applyColorMap(d1_v, d1_v, cv::COLORMAP_JET);
+			applyColorMap(d2_v, d2_v, cv::COLORMAP_JET);
+
+			cv::imshow("smooth d1", d1_v);
+			cv::imshow("smooth d2", d2_v);
+
+			Ptr<xphoto::WhiteBalancer> wb;
+			wb = xphoto::createSimpleWB();
+			wb->balanceWhite(f1, f1);
+			wb->balanceWhite(f2, f2);
+
+			vector<tuple<int,int,int,int>> tfeat;
+			if (!insearch && ftl::registration::featuresSIFT(f1, f2, tfeat, 10)) {
+				vector<tuple<int,int,int,int>> tfeat2;
+				for (int j=0; j<tfeat.size(); j++) {
+					auto [sx,sy,tx,ty] = tfeat[j];
+					tfeat2.push_back(std::make_tuple(tx,ty,sx,sy));
+				}
+				current->setPoints(tfeat2);
+				feat.insert(feat.end(), tfeat.begin(), tfeat.end());
+
+				//vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> tpclfeat;
+				current->convertToPCL(tfeat,pclfeat);
+			} else {
+				LOG(ERROR) << "Can't add features whilst searching...";
+			}
 		} else if (key == 32) freeze = !freeze;
 	}
 

applications/registration/src/sfm.cpp

+#include "sfm.hpp"
+#include <opencv2/sfm.hpp>
+#include <opencv2/viz.hpp>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/xfeatures2d.hpp>
+
+using std::vector;
+using std::tuple;
+
+using namespace cv;
+using namespace cv::sfm;
+using namespace cv::xfeatures2d;
+using ftl::registration::CalibrationChessboard;
+
+CalibrationChessboard::CalibrationChessboard(ftl::Configurable *root) {
+	pattern_size_ = Size(9, 6);
+	image_size_ = Size(1280, 720);
+	pattern_square_size_ = 36.0;//0.036;
+	// CALIB_CB_NORMALIZE_IMAfE | CALIB_CB_EXHAUSTIVE | CALIB_CB_ACCURACY 
+	chessboard_flags_ = cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_ACCURACY | cv::CALIB_CB_EXHAUSTIVE;
+}
+
+void CalibrationChessboard::objectPoints(vector<Vec3f> &out) {
+	out.reserve(pattern_size_.width * pattern_size_.height);
+	for (int row = 0; row < pattern_size_.height; ++row) {
+	for (int col = 0; col < pattern_size_.width; ++col) {
+		out.push_back(Vec3f(col * pattern_square_size_, row * pattern_square_size_, 0));
+	}}
+}
+
+bool CalibrationChessboard::findPoints(Mat &img, vector<Vec2f> &points) {
+	return cv::findChessboardCornersSB(img, pattern_size_, points, chessboard_flags_);
+}
+
+void CalibrationChessboard::drawPoints(Mat &img, const vector<Vec2f> &points) {
+	cv::drawChessboardCorners(img, pattern_size_, points, true);
+}
+
+static void get_tracks(const vector<vector<DMatch>> &matches, const vector<KeyPoint> &kp1,
+		const vector<KeyPoint> &kp2, vector<vector<Vec2d>> &tracks, int k) {
+		
+	for (size_t i=0; i<matches.size(); i++) {
+		// TODO Generalise to any number of keypoint frames
+		if (k >= matches[i].size()) continue;
+		Point2f point1 = kp1[matches[i][k].queryIdx].pt;
+    	Point2f point2 = kp2[matches[i][k].trainIdx].pt;
+    	
+    	vector<Vec2d> track;
+    	track.push_back(Vec2d(point1.x,point1.y));
+    	track.push_back(Vec2d(point2.x,point2.y));
+    	tracks.push_back(track);
+    }
+}
+
+static void get_tracks(const vector<DMatch> &matches, const vector<KeyPoint> &kp1,
+		const vector<KeyPoint> &kp2, vector<vector<Vec2d>> &tracks) {
+		
+	for (size_t i=0; i<matches.size(); i++) {
+		// TODO Generalise to any number of keypoint frames
+		//if (k >= matches[i].size()) continue;
+		Point2f point1 = kp1[matches[i].queryIdx].pt;
+    	Point2f point2 = kp2[matches[i].trainIdx].pt;
+    	
+    	vector<Vec2d> track;
+    	track.push_back(Vec2d(point1.x,point1.y));
+    	track.push_back(Vec2d(point2.x,point2.y));
+    	tracks.push_back(track);
+    }
+}
+
+static void convert_tracks(const vector<vector<Vec2d>> &tracks, vector<tuple<int,int,int,int>> &points2d) {
+	//int n_frames = 2;
+	int n_tracks = tracks.size();
+	
+	//for (int i = 0; i < n_frames; ++i)
+	//{
+		//Mat_<double> frame(2, n_tracks);
+		for (int j = 0; j < n_tracks; ++j)
+		{
+			//frame(0,j) = tracks[j][i][0];
+			//frame(1,j) = tracks[j][i][1];
+			points2d.push_back(std::make_tuple(tracks[j][0][0], tracks[j][0][1], tracks[j][1][0], tracks[j][1][1]));
+		}
+	//}
+}
+
+bool ftl::registration::featuresChess(cv::Mat &frame1, cv::Mat &frame2, std::vector<std::tuple<int,int,int,int>> &points) {
+	CalibrationChessboard cb(nullptr);
+
+	vector<Vec2f> points1, points2;
+	if (!cb.findPoints(frame1, points1)) {
+		LOG(ERROR) << "Could not find chessboard (1)";
+		return false;
+	}
+	if (!cb.findPoints(frame2, points2)) {
+		LOG(ERROR) << "Could not find chessboard (2)";
+		return false;
+	}
+
+	if (points1.size() != points2.size()) return false;
+
+	for (size_t i=0; i<points1.size(); i++) {
+		points.push_back(std::make_tuple(points1[i][0], points1[i][1], points2[i][0], points2[i][1]));
+	}
+	return true;
+}
+
+bool ftl::registration::featuresSIFT(cv::Mat &frame1, cv::Mat &frame2, std::vector<std::tuple<int,int,int,int>> &points, int K) {
+	int minHessian = 400;
+	Ptr<SIFT> detector = SIFT::create();
+	//detector->setHessianThreshold(minHessian);
+
+	vector<vector<KeyPoint>> keypoints;
+	vector<Mat> descriptors;
+	
+	{
+		vector<KeyPoint> kps;
+		Mat desc;
+		detector->detectAndCompute(frame1, Mat(), kps, desc);
+		keypoints.push_back(kps);
+		descriptors.push_back(desc);
+	}
+
+	{
+		vector<KeyPoint> kps;
+		Mat desc;
+		detector->detectAndCompute(frame2, Mat(), kps, desc);
+		keypoints.push_back(kps);
+		descriptors.push_back(desc);
+	}
+	
+	// TODO This can be done on node machines...
+	
+	//-- Step 2: Matching descriptor vectors using FLANN matcher
+	// Match between each sequential pair of images in the feed.
+	FlannBasedMatcher matcher;
+	std::vector<std::vector< DMatch >> matches;
+	matcher.knnMatch( descriptors[0], descriptors[1], matches, K );
+	
+	const float ratio_thresh = 0.7f;
+    std::vector<DMatch> good_matches;
+    for (size_t i = 0; i < matches.size(); i++)
+    {
+		for (int k=0; k<K-1; k++) {
+			if (matches[i][k].distance < ratio_thresh * matches[i][k+1].distance)
+			{
+				good_matches.push_back(matches[i][k]);
+			} else break;
+		}
+    }
+	
+	// --- SFM ---
+
+	//for (int i=0; i<K; i++) {
+		vector<vector<Vec2d>> tracks;
+		get_tracks(good_matches, keypoints[0], keypoints[1], tracks);
+		convert_tracks(tracks, points);
+	//}
+
+	//cout << "Tracks: " << tracks.size() << endl;
+	return true;
+}

applications/registration/src/sfm.hpp

+#ifndef _FTL_REGISTRATION_SFM_HPP_
+#define _FTL_REGISTRATION_SFM_HPP_
+
+#include <ftl/configurable.hpp>
+#include <opencv2/opencv.hpp>
+#include <vector>
+#include <tuple>
+
+namespace ftl {
+namespace registration {
+
+/**
+ * @brief	Chessboard calibration pattern. Uses OpenCV's
+ * 			findChessboardCornersSB function.
+ * @todo	Parameters hardcoded in constructor
+ *
+ * All parameters:
+ * 	- pattern size (inner corners)
+ * 	- square size, millimeters (TODO: meters)
+ * 	- image size, pixels
+ * 	- flags, see ChessboardCornersSB documentation
+ */
+class CalibrationChessboard {
+public:
+	CalibrationChessboard(ftl::Configurable *root);
+	void objectPoints(std::vector<cv::Vec3f> &out);
+	bool findPoints(cv::Mat &in, std::vector<cv::Vec2f> &out);
+	void drawPoints(cv::Mat &img, const std::vector<cv::Vec2f> &points);
+
+private:
+	int chessboard_flags_ = 0;
+	float pattern_square_size_;
+	cv::Size pattern_size_;
+	cv::Size image_size_;
+};
+
+bool featuresSIFT(cv::Mat &frame1, cv::Mat &frame2, std::vector<std::tuple<int,int,int,int>> &points, int);
+
+bool featuresChess(cv::Mat &frame1, cv::Mat &frame2, std::vector<std::tuple<int,int,int,int>> &points);
+
+}
+}
+
+#endif  // _FTL_REGISTRATION_SFM_HPP_

components/rgbd-sources/src/local.cpp

@@ -11,6 +11,7 @@
 #include "local.hpp"
 #include <opencv2/core.hpp>
 #include <opencv2/opencv.hpp>
+#include <opencv2/xphoto.hpp>
 
 using ftl::rgbd::detail::LocalSource;
 using cv::Mat;
@@ -269,6 +270,11 @@ bool LocalSource::get(cv::Mat &l, cv::Mat &r) {
 				0, 0, cv::INTER_LINEAR);
 	}
 
+	cv::Ptr<cv::xphoto::WhiteBalancer> wb;
+	wb = cv::xphoto::createSimpleWB();
+	wb->balanceWhite(l, l);
+	wb->balanceWhite(r, r);
+
 	if (flip_v_) {
 		Mat tl, tr;
 		cv::flip(l, tl, 0);