ransac算法_无人驾驶算法学习（一）：激光里程计之帧间匹配算法

对应关系基类　　   pcl::CorrespondenceGrouping< PointModelT, PointSceneT >
几何相似性对应　   pcl::GeometricConsistencyGrouping< PointModelT, PointSceneT >
相似性度量　　　   pcl::recognition::HoughSpace3D
多实例对应关系　   pcl::Hough3DGrouping< PointModelT, PointSceneT, PointModelRfT,，PointSceneRfT >
CRH直方图 pcl::CRHAlignment< PointT, nbins_ >
随机采样一致性估计 pcl::recognition::ObjRecRANSAC::Output
	pcl::recognition::ObjRecRANSAC::OrientedPointPair
	pcl::recognition::ObjRecRANSAC::HypothesisCreator
	pcl::recognition::ObjRecRANSAC
	
 	pcl::recognition::ORROctree::Node::Data
  	pcl::recognition::ORROctree::Node
   	pcl::recognition::ORROctree
   	pcl::recognition::RotationSpace
ICP主要函数：
1. setInputCloud (cloud_source) 设置原始点云 
2. setInputTarget (cloud_target) 设置目标点云 
3. setMaxCorrespondenceDistance（） 设置最大对应点的欧式距离，只有对应点之间的距离小于该设置值的对应点才作为ICP计算的点对。默认值为：1.7976931348623157e+308，基本上对所有点都计算了匹配点。 
4. 三个迭代终止条件设置： 
1) setMaximumIterations（） 设置最大的迭代次数。迭代停止条件之一 
2) setTransformationEpsilon（） 设置前后两次迭代的转换矩阵的最大容差（epsilion），一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，迭代停止。迭代停止条件之二，默认值为：0 
3) setEuclideanFitnessEpsilon（） 设置前后两次迭代的点对的欧式距离均值的最大容差，迭代终止条件之三，默认值为：-std::numeric_limits::max () 
迭代满足上述任一条件，终止迭代。
5.getFinalTransformation () 获取最终的配准的转化矩阵，即原始点云到目标点云的刚体变换，返回Matrix4数据类型，该数据类型采用了另一个专门用于矩阵计算的开源c++库eigen。 
6. align (PointCloudSource &output) 进行ICP配准，输出变换后点云
7. hasConverged（） 获取收敛状态，注意，只要迭代过程符合上述三个终止条件之一，该函数返回true。 
8. min_number_correspondences_  最小匹配点对数量，默认值为3，即由空间中的非共线的三点就能确定刚体变换，建议将该值设置大点，否则容易出现错误匹配。 
9. 最终迭代终止的原因可从convergence_criteria_变量中获取
注意：getFitnessScore（）用于获取迭代结束后目标点云和配准后的点云的最近点之间距离的均值。

#include <pcl/registration/icp.h>
  // Subscribers订阅话题
  ros::Subscriber param_sub = nh.subscribe("config/icp", 10, param_callback);//设置icp的参数
  ros::Subscriber gnss_sub = nh.subscribe("gnss_pose", 10, gnss_callback);//订阅gnss，回调函数：更新gnss的位姿
  ros::Subscriber map_sub = nh.subscribe("points_map", 10, map_callback);//订阅地图，回调：寻找icp的参考帧
  ros::Subscriber initialpose_sub = nh.subscribe("initialpose", 1000, initialpose_callback);//订阅带有协方差的初始值
  ros::Subscriber points_sub = nh.subscribe("filtered_points", _queue_size, points_callback);//订阅过滤后的当前点云，回调：设置当前参考帧进行icp匹配
  static void map_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
  {
	  ..........
	  icp.setInputTarget(map_ptr);//参考帧
	  ..........
	  }
  	static void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)//icp核心算法
 {
 .	........
  	icp.setInputSource(filtered_scan_ptr);//输入的当前点云帧
	.........
	//设置ICP参数
	icp.setMaximumIterations(maximum_iterations);//最大迭代次数
	icp.setTransformationEpsilon(transformation_epsilon);//最大转化矩阵差值
	icp.setMaxCorrespondenceDistance(max_correspondence_distance);//最大欧式距离差值
	icp.setEuclideanFitnessEpsilon(euclidean_fitness_epsilon);//收敛条件是均方误差和小于阈值， 停止迭代。
	icp.setRANSACOutlierRejectionThreshold(ransac_outlier_rejection_threshold);//剔除错误估计，可用 RANSAC 算法，或减少数量
	icp.align(*output_cloud, init_guess);//进行ICP配准，输出变换后点云
	
	t = icp.getFinalTransformation();  // localizer，得的最后的4x4变换矩阵
	.........


}

#include <pcl/registration/transformation_estimation_point_to_plane_lls.h>#头文件
ptp_icp=TransformationEstimationPointToPlaneLLS< PointSource, PointTarget, Scalar >::TransformationEstimationPointToPlaneLLS ();
//<PointSource, PointTarget, Scalar>模板的参数类型，提前定义
ptp_icp.estimateRigidTransformation<PointSource, PointTarget, Scalar>(const pcl::PointCloud< PointSource > & 
cloud_src,const pcl::PointCloud< PointTarget > &cloud_tgt ,M);//输入：当前帧和参考的地图帧   输出：M变换矩阵
//##函数重载，参数不一样实现不一样(alpha,beta,gamma,tx,ty,tz,M)

#ifdef USE_FAST_PCL
   #include <fast_pcl/registration/ndt.h>
 #else
   #include <pcl/registration/ndt.h>
 #endif
#ifdef CUDA_FOUND
  #include <fast_pcl/ndt_gpu/NormalDistributionsTransform.h>
 #endif

	...........核心算法代码，构建地图使用ndt算法
	static std::shared_ptr<gpu::GNormalDistributionsTransform> new_gpu_ndt_ptr = std::make_shared<gpu::GNormalDistributionsTransform>();//创建NDT转换矩阵类
	new_gpu_ndt_ptr->setResolution(ndt_res);//网格大小设置
	new_gpu_ndt_ptr->setInputTarget(map_ptr);//参考帧
	new_gpu_ndt_ptr->setMaximumIterations(max_iter);//迭代最大次数
	new_gpu_ndt_ptr->setStepSize(step_size);//牛顿法优化的最大步长
	new_gpu_ndt_ptr->setTransformationEpsilon(trans_eps);//连续变换之间允许的最大差值

	pcl::PointCloud<pcl::PointXYZ>::Ptr dummy_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PointXYZ dummy_point;
 	dummy_scan_ptr->push_back(dummy_point);//构建地图map
	new_gpu_ndt_ptr->setInputSource(dummy_scan_ptr);//当前帧
	
	new_ndt.omp_align(*output_cloud, Eigen::Matrix4f::Identity());
	fitness_score = ndt.omp_getFitnessScore();
	t = ndt.getFinalTransformation();
	// Update localizer_pose
	localizer_pose.x = t(0, 3);
	localizer_pose.y = t(1, 3);
	localizer_pose.z = t(2, 3);
	
.	.........

 
 -------------使用ndt算法与地图参考帧进行匹配---------------举一个列子gpu_ndt_ptr->setInputSource（），其他和地图一致

#ifdef CUDA_FOUND//在代码中标记可以用GPU并行加速
if (_use_gpu == true)
{
	gpu_ndt_ptr->setInputSource(filtered_scan_ptr);//参考点云
}
else
{
#endif
	if (_use_fast_pcl)
	{
		cpu_ndt.setInputSource(filtered_scan_ptr);
	}
	else
	{
		ndt.setInputSource(filtered_scan_ptr);
	}
#ifdef CUDA_FOUND	
}
#endif

//NDT默认值设置
static int max_iter = 30;        // 最大迭代次数
static float ndt_res = 1.0;      // 分辨率1m
static double step_size = 0.1;   // 步长0.1m
static double trans_eps = 0.01;  // 变换矩阵差值