diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index 7cae10a166d037973e510724f6371ffcc7b0a2b4..33440af019a512bea3218b7250b970e65973a4af 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -123,8 +123,8 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
std::this_thread::sleep_for(std::chrono::milliseconds(50));
}
- averageDepth(buffer[0], d1, 0.00000005f);
- averageDepth(buffer[1], d2, 0.00000005f);
+ averageDepth(buffer[0], d1, 0.0000004f);
+ averageDepth(buffer[1], d2, 0.0000004f);
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);
@@ -163,7 +163,8 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
p1.y = p1e[1];
p1.z = p1e[2];
src_cloud_->push_back(p1);
- src_index_.at<int>(y,x) = six++;
+ src_index_.at<int>(y,x) = six;
+ six++;
}
if (d2_value < 39.0f) {
@@ -174,7 +175,8 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
p2.y = p2e[1];
p2.z = p2e[3];
targ_cloud_->push_back(p2);
- targ_index_.at<int>(y,x) = tix++;
+ targ_index_.at<int>(y,x) = tix;
+ tix++;
}
}
}
@@ -221,8 +223,19 @@ double Correspondances::estimateTransform(Eigen::Matrix4f &T, const vector<int>
pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
- svd.estimateRigidTransformation(*src_cloud_, src_feat, *targ_cloud_, targ_feat, T);
- return validate.validateTransformation(src_cloud_, targ_cloud_, T);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+ vector<int> idx;
+
+ for (int i=0; i<src_feat.size(); i++) {
+ idx.push_back(i);
+ src_cloud->push_back(src_cloud_->at(src_feat[i]));
+ targ_cloud->push_back(targ_cloud_->at(targ_feat[i]));
+ }
+
+ svd.estimateRigidTransformation(*src_cloud, idx, *targ_cloud, idx, T);
+ return validate.validateTransformation(src_cloud, targ_cloud, T);
}
static std::default_random_engine generator;
@@ -237,6 +250,42 @@ void Correspondances::convertToPCL(const std::vector<std::tuple<int,int,int,int>
}
}
+void Correspondances::getFeatures3D(std::vector<Eigen::Vector4f> &f) {
+ for (int i=0; i<src_feat_.size(); i++) {
+ Eigen::Vector4f p;
+ const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
+ p[0] = pp.x;
+ p[1] = pp.y;
+ p[2] = pp.z;
+ p[3] = 1.0f;
+ f.push_back(p);
+ }
+}
+
+void Correspondances::getTransformedFeatures(std::vector<Eigen::Vector4f> &f) {
+ for (int i=0; i<src_feat_.size(); i++) {
+ Eigen::Vector4f p;
+ const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
+ p[0] = pp.x;
+ p[1] = pp.y;
+ p[2] = pp.z;
+ p[3] = 1.0f;
+ f.push_back(transform_ * p);
+ }
+}
+
+void Correspondances::getTransformedFeatures2D(std::vector<Eigen::Vector2i> &f) {
+ for (int i=0; i<src_feat_.size(); i++) {
+ Eigen::Vector4f p;
+ const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
+ p[0] = pp.x;
+ p[1] = pp.y;
+ p[2] = pp.z;
+ p[3] = 1.0f;
+ f.push_back(src_->point(transform_ * p));
+ }
+}
+
double Correspondances::findBestSubset(Eigen::Matrix4f &tr, int K, int N) {
double score = 10000.0f;
vector<tuple<int,int,int,int>> best;
diff --git a/applications/registration/src/correspondances.hpp b/applications/registration/src/correspondances.hpp
index 5f4b397d70ea46a3861a17a2e20c44176c40220a..6174b63919e54d39262d98f5ce44fdaf5eace0dd 100644
--- a/applications/registration/src/correspondances.hpp
+++ b/applications/registration/src/correspondances.hpp
@@ -52,6 +52,9 @@ class Correspondances {
void removeLast();
const std::vector<std::tuple<int,int,int,int>> &screenPoints() const { return log_; }
+ void getFeatures3D(std::vector<Eigen::Vector4f> &f);
+ void getTransformedFeatures(std::vector<Eigen::Vector4f> &f);
+ void getTransformedFeatures2D(std::vector<Eigen::Vector2i> &f);
void setPoints(const std::vector<std::tuple<int,int,int,int>> &p) { log_ = p; }
diff --git a/applications/registration/src/manual.cpp b/applications/registration/src/manual.cpp
index f8182d5a3a4dc989e2509985e4b675b7abaff91d..cc31e7d2780490c66791195fbb948869c8aa5ef1 100644
--- a/applications/registration/src/manual.cpp
+++ b/applications/registration/src/manual.cpp
@@ -243,6 +243,14 @@ void ftl::registration::manual(ftl::Configurable *root) {
drawMarker(tdepth, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
}
+ vector<Eigen::Vector2i> tpoints;
+ current->getTransformedFeatures2D(tpoints);
+ for (int i=0; i<tpoints.size(); i++) {
+ Eigen::Vector2i p = tpoints[i];
+ drawMarker(trgb, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
+ drawMarker(tdepth, Point(p[0],p[1]), Scalar(255,0,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);
@@ -318,12 +326,12 @@ void ftl::registration::manual(ftl::Configurable *root) {
lastScore = 1000.0;
do {
Eigen::Matrix4f tr;
- float s = current->findBestSubset(tr, 6, 10);
+ float s = current->findBestSubset(tr, 20, 20);
LOG(INFO) << "SCORE = " << s;
if (s < lastScore) {
lastScore = s;
current->setTransform(tr);
- current->source()->setPose(current->transform());
+ //current->source()->setPose(current->transform());
}
} while (ftl::running && insearch && lastScore > 0.000001);
insearch = false;
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index 595fe7f9899d6850e09ef6f2e208bedefd5a9afb..89ec80e42869c78d9c86539dc4d34026b144a046 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp
@@ -139,6 +139,8 @@ class Source : public ftl::Configurable {
*/
Eigen::Vector4f point(uint x, uint y, float d);
+ Eigen::Vector2i point(const Eigen::Vector4f &p);
+
/**
* Force the internal implementation to be reconstructed.
*/
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index f2049b791a3c4a2e9a3c8160602806ff22776eb3..8e2864f22b61e5900f95e26d01a9dd32e56c4bd0 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -164,6 +164,15 @@ Eigen::Vector4f Source::point(uint ux, uint uy, float d) {
return Eigen::Vector4f(x*d,y*d,d,1.0);
}
+Eigen::Vector2i Source::point(const Eigen::Vector4f &p) {
+ const auto ¶ms = parameters();
+ float x = p[0] / p[2];
+ float y = p[1] / p[2];
+ x *= params.fx;
+ y *= params.fy;
+ return Eigen::Vector2i((int)(x - params.cx), (int)(y - params.cy));
+}
+
void Source::setPose(const Eigen::Matrix4f &pose) {
pose_ = pose;
if (impl_) impl_->setPose(pose);