From f84075fbbebea77b5df9bce90e0332d9b0cde497 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Mon, 24 Jun 2019 11:33:02 +0300
Subject: [PATCH] Use ICP with aruco result
---
.../reconstruct/src/scene_rep_hash_sdf.cu | 2 +-
applications/registration/src/aruco.cpp | 3 ++-
.../registration/src/correspondances.cpp | 25 ++++++++++++++++---
.../registration/src/correspondances.hpp | 2 +-
4 files changed, 26 insertions(+), 6 deletions(-)
diff --git a/applications/reconstruct/src/scene_rep_hash_sdf.cu b/applications/reconstruct/src/scene_rep_hash_sdf.cu
index 521a34601..9c568280d 100644
--- a/applications/reconstruct/src/scene_rep_hash_sdf.cu
+++ b/applications/reconstruct/src/scene_rep_hash_sdf.cu
@@ -624,7 +624,7 @@ __global__ void starveVoxelsKernel(HashData hashData) {
//is typically exectued only every n'th frame
int weight = hashData.d_SDFBlocks[entry.ptr + threadIdx.x].weight;
weight = max(0, weight-1);
- hashData.d_SDFBlocks[entry.ptr + threadIdx.x].weight = weight; //CHECK Remove to totally clear previous frame (Nick)
+ hashData.d_SDFBlocks[entry.ptr + threadIdx.x].weight = 1; //CHECK Remove to totally clear previous frame (Nick)
}
extern "C" void starveVoxelsKernelCUDA(HashData& hashData, const HashParams& hashParams)
diff --git a/applications/registration/src/aruco.cpp b/applications/registration/src/aruco.cpp
index 28cddabea..8ff848546 100644
--- a/applications/registration/src/aruco.cpp
+++ b/applications/registration/src/aruco.cpp
@@ -165,8 +165,9 @@ void ftl::registration::aruco(ftl::Configurable *root) {
}
Eigen::Matrix4d t;
- lastScore = current->estimateTransform(t, srcfeat, targfeat);
+ lastScore = current->estimateTransform(t, srcfeat, targfeat, true);
current->setTransform(t);
+ //lastScore = current->icp();
} else if (key == 'f') {
Mat f1,f2;
current->capture(f1,f2);
diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index f40eaca97..a05cddfa8 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -310,7 +310,7 @@ double Correspondances::estimateTransform(Eigen::Matrix4d &T) {
return validate.validateTransformation(src_cloud, targ_cloud, T);
}
-double Correspondances::estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat) {
+double Correspondances::estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat, bool doicp) {
pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ, double> validate;
pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ, double> svd;
@@ -337,8 +337,27 @@ double Correspondances::estimateTransform(Eigen::Matrix4d &T, const std::vector<
pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
- svd.estimateRigidTransformation(*src_cloud, idx, *targ_cloud, idx, T);
- return validate.validateTransformation(src_cloud, targ_cloud, T);
+ svd.estimateRigidTransformation(*tsrc_cloud, idx, *targ_cloud, idx, T);
+
+ if (doicp) {
+ pcl::transformPointCloud(*tsrc_cloud, *src_cloud, T);
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+ pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ, double> icp;
+
+ icp.setInputSource(src_cloud);
+ icp.setInputTarget(targ_cloud);
+ icp.align(*final_cloud);
+ LOG(INFO) << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore();
+ //transform_ *= icp.getFinalTransformation();
+
+ T = icp.getFinalTransformation() * T * transform_;
+ //uptodate_ = true;
+
+ return icp.getFitnessScore();
+ } else {
+ return validate.validateTransformation(src_cloud, targ_cloud, T);
+ }
}
double Correspondances::estimateTransform(Eigen::Matrix4d &T, const vector<int> &src_feat, const vector<int> &targ_feat) {
diff --git a/applications/registration/src/correspondances.hpp b/applications/registration/src/correspondances.hpp
index 53f50a155..72f39cf86 100644
--- a/applications/registration/src/correspondances.hpp
+++ b/applications/registration/src/correspondances.hpp
@@ -65,7 +65,7 @@ class Correspondances {
*/
double estimateTransform(Eigen::Matrix4d &);
double estimateTransform(Eigen::Matrix4d &T, const std::vector<int> &src_feat, const std::vector<int> &targ_feat);
- double estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat);
+ double estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat, bool doicp=false);
double findBestSubset(Eigen::Matrix4d &tr, int K, int N);
--
GitLab