ICP on best estimate

applications/registration/src/correspondances.cpp

@@ -136,10 +136,10 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
 	cv::imshow("smooth d2", d2_v);
 
 	// Should be done in RGB-Depth source class
-	/*cv::Ptr<cv::xphoto::WhiteBalancer> wb;
+	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();
@@ -276,6 +276,30 @@ double Correspondances::icp() {
 	return icp.getFitnessScore();
 }
 
+double Correspondances::icp(const Eigen::Matrix4f &T_in, Eigen::Matrix4f &T_out, const vector<int> &idx) {
+	//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>);
+
+	for (int i=0; i<idx.size(); i++) {
+		src_cloud->push_back(src_cloud_->at(src_feat_[idx[i]]));
+		targ_cloud->push_back(targ_cloud_->at(targ_feat_[idx[i]]));
+	}
+
+	pcl::transformPointCloud(*src_cloud, *tsrc_cloud, T_in);
+
+	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();
+	T_out = T_in * icp.getFinalTransformation();
+	return icp.getFitnessScore();
+}
+
 static std::default_random_engine generator;
 
 void Correspondances::convertToPCL(const std::vector<std::tuple<int,int,int,int>> &p, std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &pp) {
@@ -326,7 +350,7 @@ void Correspondances::getTransformedFeatures2D(std::vector<Eigen::Vector2i> &f)
 
 double Correspondances::findBestSubset(Eigen::Matrix4f &tr, int K, int N) {
 	double score = 10000.0f;
-	vector<tuple<int,int,int,int>> best;
+	vector<int> best;
 	Eigen::Matrix4f bestT;
 
 	std::mt19937 rng(std::chrono::steady_clock::now().time_since_epoch().count());
@@ -348,8 +372,10 @@ double Correspondances::findBestSubset(Eigen::Matrix4f &tr, int K, int N) {
 		sidx.clear();
 		tidx.clear();
 
+		vector<int> idx;
 		for (int k=0; k<K; k++) {
 			int r = dice();
+			idx.push_back(r);
 			sidx.push_back(src_feat_[r]);
 			tidx.push_back(targ_feat_[r]);
 		}
@@ -360,14 +386,17 @@ double Correspondances::findBestSubset(Eigen::Matrix4f &tr, int K, int N) {
 		if (scoreT < score) {
 			score = scoreT;
 			bestT = T;
+			best = idx;
 		}
 	}
 
+	return icp(bestT, tr, best);
+
 	// TODO Add these best points to actual clouds.
 	//log_ = best;
-	tr = bestT;
+	//tr = bestT;
 	//uptodate_ = true;
-	return score;
+	//return score;
 }
 
 Eigen::Matrix4f Correspondances::transform() {

applications/registration/src/correspondances.hpp

@@ -69,6 +69,7 @@ class Correspondances {
 	double findBestSubset(Eigen::Matrix4f &tr, int K, int N);
 
 	double icp();
+	double icp(const Eigen::Matrix4f &T_in, Eigen::Matrix4f &T_out, const std::vector<int> &idx);
 
 	void convertToPCL(const std::vector<std::tuple<int,int,int,int>> &p, std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &p2);