error fix, empty correspondences

README.md

@@ -71,7 +71,7 @@ catkin build -DQUATRO_TBB=ON
         using QuatroPointType = pcl::PointXYZI; //can be changed
 
         shared_ptr<quatro<QuatroPointType>> m_quatro_handler = nullptr;
-        m_quatro_handler = std::make_shared<quatro<QuatroPointType>>(fpfh_normal_radius_, fpfh_radius_, noise_bound_, rot_gnc_factor_, rot_cost_diff_thr_, quatro_max_iter_, quatro_max_iter_); //refer https://github.com/engcang/FAST-LIO-SAM-QN/blob/master/fast_lio_sam_qn/config/config.yaml#L28
+        m_quatro_handler = std::make_shared<quatro<QuatroPointType>>(fpfh_normal_radius_, fpfh_radius_, noise_bound_, rot_gnc_factor_, rot_cost_diff_thr_, quatro_max_iter_, estimat_scale_); //refer https://github.com/engcang/FAST-LIO-SAM-QN/blob/master/fast_lio_sam_qn/config/config.yaml#L28
 
         ////// use
         pcl::PointCloud<QuatroPointType> src_; //this should be not empty but the real data

src/quatro_module.cc

@@ -52,18 +52,24 @@ Eigen::Matrix4d quatro<PointType>::align(const pcl::PointCloud<PointType> &src,
 	teaser::FPFHCloudPtr scene_descriptors_ = fpfh_.computeFPFHFeatures(tgt_cloud_, m_normal_radius, m_fpfh_radius);
 
 	teaser::Matcher matcher_;
-	std::vector<std::pair<int, int>> correspondences_ = matcher_.calculateCorrespondences(src_cloud_, tgt_cloud_, *obj_descriptors_, *scene_descriptors_, false, true, true, 0.95);
+	std::vector<std::pair<int, int>> correspondences_ = matcher_.calculateCorrespondences(src_cloud_, tgt_cloud_, *obj_descriptors_, *scene_descriptors_, false, true, true, 0.95); //false true true important~
 
-	// set_params(); //TODO: check if this should be called each time
-	teaser::RobustRegistrationSolver Quatro_(m_quatro_params);
-
-	Quatro_.solve(src_cloud_, tgt_cloud_, correspondences_);
-	teaser::RegistrationSolution solution_by_quatro_ = Quatro_.getSolution();
-	if_valid = solution_by_quatro_.valid; //if the result is valid or not
-
-	out_tf_.block<3, 3>(0, 0) = solution_by_quatro_.rotation;
-	out_tf_.block<3, 1>(0, 3) = solution_by_quatro_.translation;
+	if (correspondences_.empty()) // no correspondences!!!
+	{
+		if_valid = false;
+	}
+	else
+	{
+		// set_params(); //TODO: check if this should be called each time
+		teaser::RobustRegistrationSolver Quatro_(m_quatro_params);
 
+		Quatro_.solve(src_cloud_, tgt_cloud_, correspondences_);
+		teaser::RegistrationSolution solution_by_quatro_ = Quatro_.getSolution();
+		if_valid = solution_by_quatro_.valid; //if the result is valid or not
+
+		out_tf_.block<3, 3>(0, 0) = solution_by_quatro_.rotation;
+		out_tf_.block<3, 1>(0, 3) = solution_by_quatro_.translation;
+	}
 	return out_tf_;
 }