ICP attempt

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;

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);
 
 	/**

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;