From 9ad27d104faa1645f85449400d1e5f0ea523cb67 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nicolas.pope@utu.fi>
Date: Tue, 18 Jun 2019 10:50:52 +0300
Subject: [PATCH] Add automated matching to registration app
---
applications/registration/CMakeLists.txt | 1 +
.../registration/src/correspondances.cpp | 80 +++++++++
.../registration/src/correspondances.hpp | 8 +-
applications/registration/src/manual.cpp | 131 +++++++++++++-
applications/registration/src/sfm.cpp | 164 ++++++++++++++++++
applications/registration/src/sfm.hpp | 44 +++++
components/rgbd-sources/src/local.cpp | 6 +
7 files changed, 430 insertions(+), 4 deletions(-)
create mode 100644 applications/registration/src/sfm.cpp
create mode 100644 applications/registration/src/sfm.hpp
diff --git a/applications/registration/CMakeLists.txt b/applications/registration/CMakeLists.txt
index f3bde2653..6597192ca 100644
--- a/applications/registration/CMakeLists.txt
+++ b/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})
diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index 58eb1c6e2..134b9088a 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -1,5 +1,7 @@
#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_;
diff --git a/applications/registration/src/correspondances.hpp b/applications/registration/src/correspondances.hpp
index ee519bbc1..bd08dd6dd 100644
--- a/applications/registration/src/correspondances.hpp
+++ b/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_;
diff --git a/applications/registration/src/manual.cpp b/applications/registration/src/manual.cpp
index 6269b0830..ca7f1cbf5 100644
--- a/applications/registration/src/manual.cpp
+++ b/applications/registration/src/manual.cpp
@@ -1,5 +1,6 @@
#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;
@@ -134,7 +163,7 @@ void ftl::registration::manual(ftl::Configurable *root) {
net->waitConnections();
auto sources = ftl::createArray<Source>(root, "sources", net);
-
+
int curtarget = 0;
bool freeze = false;
@@ -179,8 +208,13 @@ 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) {
@@ -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;
}
diff --git a/applications/registration/src/sfm.cpp b/applications/registration/src/sfm.cpp
new file mode 100644
index 000000000..5d5e7cc9a
--- /dev/null
+++ b/applications/registration/src/sfm.cpp
@@ -0,0 +1,164 @@
+#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;
+}
diff --git a/applications/registration/src/sfm.hpp b/applications/registration/src/sfm.hpp
new file mode 100644
index 000000000..97e27da10
--- /dev/null
+++ b/applications/registration/src/sfm.hpp
@@ -0,0 +1,44 @@
+#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_
diff --git a/components/rgbd-sources/src/local.cpp b/components/rgbd-sources/src/local.cpp
index 63257bad5..a0d6334b3 100644
--- a/components/rgbd-sources/src/local.cpp
+++ b/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);
--
GitLab