Fixed bug with depth values

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.0000004f);
-	averageDepth(buffer[1], d2, 0.0000004f);
+	averageDepth(buffer[0], d1, 0.00000004f);
+	averageDepth(buffer[1], d2, 0.00000004f);
 	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);
@@ -173,7 +173,7 @@ bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
 				Eigen::Vector4f p2e = targ_->point(x,y,d2_value);
 				p2.x = p2e[0];
 				p2.y = p2e[1];
-				p2.z = p2e[3];
+				p2.z = p2e[2];
 				targ_cloud_->push_back(p2);
 				targ_index_.at<int>(y,x) = tix;
 				tix++;
@@ -223,19 +223,19 @@ 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>);
 
-	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, idx, *targ_cloud, idx, T);
-	return validate.validateTransformation(src_cloud, targ_cloud, T);
+	svd.estimateRigidTransformation(*src_cloud_, src_feat, *targ_cloud_, targ_feat, T);
+	return validate.validateTransformation(src_cloud_, targ_cloud_, T);
 }
 
 static std::default_random_engine generator;

applications/registration/src/manual.cpp

@@ -326,7 +326,7 @@ void ftl::registration::manual(ftl::Configurable *root) {
 					lastScore = 1000.0;
 					do {
 						Eigen::Matrix4f tr;
-						float s = current->findBestSubset(tr, 20, 20);
+						float s = current->findBestSubset(tr, 15, 10);
 						LOG(INFO) << "SCORE = " << s;
 						if (s < lastScore) {
 							lastScore = s;