From 177ed8969c17a7454506b121ba352d79ef46dede Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Wed, 19 Jun 2019 17:51:51 +0300
Subject: [PATCH] ICP attempt
---
.../registration/src/correspondances.cpp | 60 +++++++++++++++----
.../registration/src/correspondances.hpp | 2 +
applications/registration/src/manual.cpp | 2 +
3 files changed, 53 insertions(+), 11 deletions(-)
diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index 9765a1530..91e3948a1 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -4,6 +4,7 @@
#include <chrono>
#include <thread>
#include <opencv2/xphoto.hpp>
+#include <pcl/registration/icp.h>
using ftl::registration::Correspondances;
using std::string;
@@ -70,11 +71,11 @@ static void averageDepth(vector<Mat> &in, Mat &out, float varThresh) {
//LOG(INFO) << "VAR " << var;
- if (var < varThresh) {
+ //if (var < varThresh) {
out.at<float>(i) = sum;
- } else {
- out.at<float>(i) = 41.0f;
- }
+ //} else {
+ // out.at<float>(i) = 41.0f;
+ //}
} else {
out.at<float>(i) = 41.0f;
}
@@ -215,6 +216,8 @@ double Correspondances::estimateTransform(Eigen::Matrix4f &T) {
pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
+ validate.setMaxRange(0.1);
+
svd.estimateRigidTransformation(*src_cloud_, src_feat_, *targ_cloud_, targ_feat_, T);
return validate.validateTransformation(src_cloud_, targ_cloud_, T);
}
@@ -223,19 +226,54 @@ double Correspondances::estimateTransform(Eigen::Matrix4f &T, const vector<int>
pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
- //pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- //pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
- //vector<int> idx;
+ vector<int> idx;
- /*for (int i=0; i<src_feat.size(); i++) {
+ 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_, src_feat, *targ_cloud_, targ_feat, T);
- return validate.validateTransformation(src_cloud_, targ_cloud_, T);
+ pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
+
+ validate.setMaxRange(0.1);
+
+ svd.estimateRigidTransformation(*tsrc_cloud, idx, *targ_cloud, idx, T);
+ T = T * transform_;
+ uptodate_ = true;
+ float score = validate.validateTransformation(src_cloud, targ_cloud, T);
+ return score;
+}
+
+double Correspondances::icp() {
+ //pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_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]));
+ }
+
+ pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
+
+ pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
+ icp.setInputSource(tsrc_cloud);
+ icp.setInputTarget(targ_cloud);
+ icp.align(*final_cloud);
+ LOG(INFO) << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore();
+ transform_ *= icp.getFinalTransformation();
+ return icp.getFitnessScore();
}
static std::default_random_engine generator;
diff --git a/applications/registration/src/correspondances.hpp b/applications/registration/src/correspondances.hpp
index 6174b6391..ab73b6aef 100644
--- a/applications/registration/src/correspondances.hpp
+++ b/applications/registration/src/correspondances.hpp
@@ -68,6 +68,8 @@ class Correspondances {
double findBestSubset(Eigen::Matrix4f &tr, int K, int N);
+ double icp();
+
void convertToPCL(const std::vector<std::tuple<int,int,int,int>> &p, std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &p2);
/**
diff --git a/applications/registration/src/manual.cpp b/applications/registration/src/manual.cpp
index b92fa0162..a9463fcf8 100644
--- a/applications/registration/src/manual.cpp
+++ b/applications/registration/src/manual.cpp
@@ -299,6 +299,8 @@ void ftl::registration::manual(ftl::Configurable *root) {
Eigen::Matrix4f t;
lastScore = current->estimateTransform(t);
current->setTransform(t);
+ } else if (key == 'i') {
+ current->icp();
} else if (key == 's') {
// Save
map<string, Eigen::Matrix4f> transforms;
--
GitLab