diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index f510e213d516a5ed18cba761251d05091ff051a0..9b41a057235e39f12075bb9a9073d70728f05b2f 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -2,6 +2,8 @@
#include <nlohmann/json.hpp>
#include <random>
#include <chrono>
+#include <thread>
+#include <opencv2/xphoto.hpp>
using ftl::registration::Correspondances;
using std::string;
@@ -14,6 +16,7 @@ using pcl::PointXYZRGB;
using nlohmann::json;
using std::tuple;
using std::make_tuple;
+using cv::Mat;
Correspondances::Correspondances(Source *src, Source *targ)
@@ -86,16 +89,16 @@ static PointXYZ makePCL(Source *s, int x, int y) {
}
void Correspondances::clear() {
- targ_cloud_.clear();
- src_cloud_.clear();
- src_index_ = cv::Mat(rgb1.size(), CV_32SC1, cv::Scalar(-1));
- targ_index_ = cv::Mat(rgb2.size(), CV_32SC1, cv::Scalar(-1));
+ targ_cloud_->clear();
+ src_cloud_->clear();
+ src_index_ = cv::Mat(cv::Size(src_->parameters().width, src_->parameters().height), CV_32SC1, cv::Scalar(-1));
+ targ_index_ = cv::Mat(cv::Size(targ_->parameters().width, targ_->parameters().height), CV_32SC1, cv::Scalar(-1));
src_feat_.clear();
targ_feat_.clear();
log_.clear();
}
-void Correspondance::clearCorrespondances() {
+void Correspondances::clearCorrespondances() {
src_feat_.clear();
targ_feat_.clear();
log_.clear();
@@ -109,10 +112,10 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
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(rgb1, d1);
- current->target()->getFrames(rgb2, d2);
+ src_->grab();
+ targ_->grab();
+ src_->getFrames(rgb1, d1);
+ targ_->getFrames(rgb2, d2);
buffer[0][i] = d1;
buffer[1][i] = d2;
@@ -129,10 +132,11 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
cv::imshow("smooth d1", d1_v);
cv::imshow("smooth d2", d2_v);
- Ptr<xphoto::WhiteBalancer> wb;
- wb = xphoto::createSimpleWB();
+ // Should be done in RGB-Depth source class
+ /*cv::Ptr<cv::xphoto::WhiteBalancer> wb;
+ wb = cv::xphoto::createSimpleWB();
wb->balanceWhite(rgb1, rgb1);
- wb->balanceWhite(rgb2, rgb2);
+ wb->balanceWhite(rgb2, rgb2);*/
// Build point clouds
clear();
@@ -142,8 +146,8 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
for (int y=0; y<rgb1.rows; y++) {
//unsigned char *rgb1ptr = rgb1.ptr(y);
//unsigned char *rgb2ptr = rgb2.ptr(y);
- float *d1ptr = d1.ptr(y);
- float *d2ptr = d2.ptr(y);
+ float *d1ptr = (float*)d1.ptr(y);
+ float *d2ptr = (float*)d2.ptr(y);
for (int x=0; x<rgb1.cols; x++) {
float d1_value = d1ptr[x];
@@ -151,15 +155,23 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
if (d1_value < 39.0f) {
// Make PCL points with specific depth value
- pcl::PointXYZ p1 = src_->point(x,y,d1_value);
- src_cloud_.push_back(p1);
+ pcl::PointXYZ p1;
+ Eigen::Vector4f p1e = src_->point(x,y,d1_value);
+ p1.x = p1e[0];
+ p1.y = p1e[1];
+ p1.z = p1e[2];
+ src_cloud_->push_back(p1);
src_index_.at<int>(y,x) = six++;
}
if (d2_value < 39.0f) {
// Make PCL points with specific depth value
- pcl::PointXYZ p2 = src_->point(x,y,d2_value);
- targ_cloud_.push_back(p2);
+ pcl::PointXYZ p2;
+ Eigen::Vector4f p2e = src_->point(x,y,d2_value);
+ p2.x = p2e[0];
+ p2.y = p2e[1];
+ p2.z = p2e[3];
+ targ_cloud_->push_back(p2);
targ_index_.at<int>(y,x) = tix++;
}
}
@@ -188,19 +200,12 @@ bool Correspondances::add(int tx, int ty, int sx, int sy) {
void Correspondances::removeLast() {
uptodate_ = false;
- targ_feat_->erase(targ_feat_->end()-1);
- src_feat_->erase(src_feat_->end()-1);
+ targ_feat_.erase(targ_feat_.end()-1);
+ src_feat_.erase(src_feat_.end()-1);
log_.pop_back();
}
double Correspondances::estimateTransform(Eigen::Matrix4f &T) {
- //Eigen::Matrix4f transform = transform_ * T;
- 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;
@@ -209,8 +214,6 @@ double Correspondances::estimateTransform(Eigen::Matrix4f &T) {
}
double Correspondances::estimateTransform(Eigen::Matrix4f &T, const vector<int> &src_feat, const vector<int> &targ_feat) {
- uptodate_ = true;
-
pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
@@ -236,7 +239,7 @@ double Correspondances::findBestSubset(Eigen::Matrix4f &tr, int K, int N) {
Eigen::Matrix4f bestT;
std::mt19937 rng(std::chrono::steady_clock::now().time_since_epoch().count());
- std::uniform_int_distribution<int> distribution(0,p.size()-1);
+ std::uniform_int_distribution<int> distribution(0,src_feat_.size()-1);
//int dice_roll = distribution(generator);
auto dice = std::bind ( distribution, rng );
@@ -265,7 +268,6 @@ double Correspondances::findBestSubset(Eigen::Matrix4f &tr, int K, int N) {
if (scoreT < score) {
score = scoreT;
- best = ps;
bestT = T;
}
}
@@ -278,6 +280,6 @@ double Correspondances::findBestSubset(Eigen::Matrix4f &tr, int K, int N) {
}
Eigen::Matrix4f Correspondances::transform() {
- if (!uptodate_) estimateTransform();
+ if (!uptodate_) estimateTransform(transform_);
return (parent_) ? parent_->transform() * transform_ : transform_;
}
diff --git a/applications/registration/src/correspondances.hpp b/applications/registration/src/correspondances.hpp
index 6a58dfb18783a450f2c70acd869906afe7174cc6..5f4b397d70ea46a3861a17a2e20c44176c40220a 100644
--- a/applications/registration/src/correspondances.hpp
+++ b/applications/registration/src/correspondances.hpp
@@ -60,9 +60,10 @@ class Correspondances {
*
* @return Validation score of the transform.
*/
- double estimateTransform();
+ double estimateTransform(Eigen::Matrix4f &);
+ double estimateTransform(Eigen::Matrix4f &T, const std::vector<int> &src_feat, const std::vector<int> &targ_feat);
- double findBest(Eigen::Matrix4f &tr, const std::vector<std::tuple<int,int,int,int>> &p, int K, int N);
+ double findBestSubset(Eigen::Matrix4f &tr, 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);
@@ -83,7 +84,10 @@ class Correspondances {
Eigen::Matrix4f transform_;
bool uptodate_;
std::vector<std::tuple<int,int,int,int>> log_;
- cv::Mat &index_;
+ cv::Mat src_index_;
+ cv::Mat targ_index_;
+ std::vector<int> targ_feat_;
+ std::vector<int> src_feat_;
};
}
diff --git a/applications/registration/src/manual.cpp b/applications/registration/src/manual.cpp
index 98926415e3f4ed86d02bdc177ee4294104096161..543cb8ef6ede6ac172f8510a979dc01022fd95ff 100644
--- a/applications/registration/src/manual.cpp
+++ b/applications/registration/src/manual.cpp
@@ -14,7 +14,6 @@
#include <opencv2/imgcodecs.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/highgui.hpp>
-#include <opencv2/xphoto.hpp>
#include <map>
@@ -223,10 +222,10 @@ 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;
+ /*Ptr<xphoto::WhiteBalancer> wb;
wb = xphoto::createSimpleWB();
wb->balanceWhite(tsrgb, srgb);
- wb->balanceWhite(ttrgb, trgb);
+ wb->balanceWhite(ttrgb, trgb);*/
// Apply points and labels...
auto &points = current->screenPoints();
@@ -261,23 +260,24 @@ void ftl::registration::manual(ftl::Configurable *root) {
curtarget = key-48;
if (curtarget >= sources.size()) curtarget = 0;
current = corrs[sources[curtarget]->getURI()];
- feat.clear();
- pclfeat.clear();
lastScore = 1000.0;
} else if (key == 'd') {
depthtoggle = !depthtoggle;
} else if (key == 'c') {
- feat.clear();
- pclfeat.clear();
+ current->clear();
lastScore = 1000.0;
}else if (key == 'a') {
+ Eigen::Matrix4f t;
if (current->add(lastTX,lastTY,lastSX,lastSY)) {
lastTX = lastTY = lastSX = lastSY = 0;
- lastScore = current->estimateTransform();
+ lastScore = current->estimateTransform(t);
+ current->setTransform(t);
}
} else if (key == 'u') {
current->removeLast();
- lastScore = current->estimateTransform();
+ Eigen::Matrix4f t;
+ lastScore = current->estimateTransform(t);
+ current->setTransform(t);
} else if (key == 's') {
// Save
map<string, Eigen::Matrix4f> transforms;
@@ -301,13 +301,13 @@ void ftl::registration::manual(ftl::Configurable *root) {
insearch = true;
LOG(INFO) << "Features matched = " << feat.size();
- ftl::pool.push([&lastScore,&insearch,current,&feat,&pclfeat](int id) {
+ ftl::pool.push([&lastScore,&insearch,current](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;
+ float s = current->findBestSubset(tr, 20, 10);
+ LOG(INFO) << "SCORE = " << s;
if (s < lastScore) {
lastScore = s;
current->setTransform(tr);
@@ -326,21 +326,19 @@ void ftl::registration::manual(ftl::Configurable *root) {
insearch = false;
}
} else if (key == 'f') {
- Mat f1,f2;
- current->capture(f1,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));
+ if (!insearch) {
+ Mat f1,f2;
+ current->capture(f1,f2);
+
+ vector<tuple<int,int,int,int>> feat;
+ if (ftl::registration::featuresSIFT(f1, f2, feat, 2)) {
+ for (int j=0; j<feat.size(); j++) {
+ auto [sx,sy,tx,ty] = feat[j];
+ current->add(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);
+ LOG(INFO) << "Found " << current->screenPoints().size() << " features";
} else {
LOG(ERROR) << "Can't add features whilst searching...";
}