PCL 点云匹配 2 之NICP(Normal ICP)

一、概述

PCL 库中提供了以下 ICP 的接口及其变种：

• 点到点：pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar >
• 点到面：pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >
• 面到面：pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >

 上面一篇中我们已经得出了一个结论，就是ICP虽然简单，但是也有明显的缺点

1、计算速度慢，收敛慢，迭代次数多

2、对内存的开销比较大

3、很容易陷入局部最优的困局

因此我们在经典ICP的基础上添加一两个约束：

第一个约束就是添加法向量，计算当前点R半斤内的法向量，算法向量的夹角

第二个约束就是添加曲率，添加曲率

这样我们就可以减少迭代的次数，加速收敛。

二、计算点云的法向量和曲率

原理：

 找到当前点pi的r 半斤类的所有的点V，然后计算出当前点集V的均值ui，然后开始计算V到的协方差

 由于协方差的对称性，我们可以对这个矩阵进行SVD矩阵分解

曲率：

 是按照从小到大的顺序排列的，那么曲率可以计算：

  ,并且 越小表示当前点云半斤内越平坦

最小的特征值对应的就是法向量的方向。

数学技巧篇69:特征值、特征向量的求法与证明 - 知乎

法向量

求解最小特征值的特征向量即为法向量

法向量的定向

从上面我们可以计算出法向量，那么法向量如何定向呢，也就是说一条直线你规定那个方向为正方向呢？？？

可以用视点Vp *Ni(当前点的法向量) >0 为正 等

PCL中的法向量定向

已知视点Vp ，对于任意的半径内的点坐标Pi 以及其对应的法向量ni,其定向如下：

                                                             

flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Vector4f &normal);

法向量方向测试

 比较如下

code

#include 
#include 
//#include 
#include 
#include 
#include 
#include "vtkAutoInit.h"
VTK_MODULE_INIT(vtkRenderingOpenGL);
VTK_MODULE_INIT(vtkInteractionStyle);
VTK_MODULE_INIT(vtkRenderingFreeType)

using namespace std;


#if 1

int main()
{
		//------------------加载点云数据-------------------

	  //C:\Users\Albert\Desktop\halcon_to_pcl\normal\normaltest2.pcd
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
	if (pcl::io::loadPCDFile("C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\classfiy\\0.pcd", *cloud) == -1)
	{

		// 找不到点云文件
		return  -1;
	}

	//------------------计算法线----------------------
	pcl::NormalEstimationOMP n;//OMP加速
	pcl::PointCloud::Ptr normals(new pcl::PointCloud);
	//建立kdtree来进行近邻点集搜索
	pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());
	n.setNumberOfThreads(10);//设置openMP的线程数 我一般是10条就够了


  // 主要是改变这个值来看看点云的法向量的方向变换
	n.setViewPoint(1,1,1);//设置视点，默认为（0，0，0）





	n.setInputCloud(cloud);
	n.setSearchMethod(tree);
	n.setKSearch(10);//点云法向计算时，需要所搜的近邻点大小
	//n.setRadiusSearch(0.03);//半径搜素
	n.compute(*normals);//开始进行法向计

	//----------------可视化--------------
	boost::shared_ptr viewer(new pcl::visualization::PCLVisualizer("Normal viewer"));
	//viewer->initCameraParameters();//设置照相机参数，使用户从默认的角度和方向观察点云
	//设置背景颜色
	viewer->setBackgroundColor(0.3, 0.3, 0.3);
	viewer->addText("Normal", 10, 10, "text");
	//设置点云颜色
	pcl::visualization::PointCloudColorHandlerCustom single_color(cloud, 255, 225,255);
	//添加坐标系
	viewer->addCoordinateSystem(0.1);
	viewer->addPointCloud(cloud, single_color, "sample cloud");


	//添加需要显示的点云法向。cloud为原始点云模型，normal为法向信息，10表示需要显示法向的点云间隔，即每10个点显示一次法向，0.1表示法向长度。
	viewer->addPointCloudNormals(cloud, normals, 100, 0.03, "normals");



	//设置点云大小
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return 0;

}

#endif 

四、算法原理

点云配准方法---ICP升级版本NICP(Normal ICP) - 古月居

 匹配原则

  1、点间距

  2、曲率

  3、法向量

原理推导：

我们将法向量加进去                         

然后我们再把Rt 加进去                    

这里要注意的是法向量也要旋转   ，对点的坐标p 就是平移+旋转。

接着开始计算目标的误差函数

                                                  

 这个是一个6维度的数据  ， x  y  z   normal_x   normal_y   noraml_z

求解的就算了，我还是要去复习一下我的《数学分析》和《高等代数》再说

五、算法流程

  同ICP，只是在判别时候收敛的哪里添加了一个曲率和法向量角度的收敛条件

六、PCL测试

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloud;

PointCloud::Ptr cloud_target(new PointCloud);
PointCloud::Ptr cloud_source(new PointCloud);
PointCloud::Ptr cloud_icp(new PointCloud);
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("global_visualizer"));

const double translation_step = 0.005;  // 设定一个平移的步长
const double rotation_step = M_PI / 72;  // 设定一个旋转的步长
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* viewer_void);
Eigen::Affine3f G = Eigen::Affine3f::Identity();

double f1, f2, f3;


#if 1
int main() {
	//加载点云
	string target_path = "C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\1.pcd";
	string source_path = "C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\2.pcd";
	pcl::io::loadPCDFile(target_path, *cloud_target);
	pcl::io::loadPCDFile(source_path, *cloud_source);

	//点云降采样
	pcl::VoxelGrid VG;
	VG.setInputCloud(cloud_target);
	VG.setLeafSize(0.001f, 0.001f, 0.001f);
	VG.filter(*cloud_target);

	VG.setInputCloud(cloud_source);
	VG.setLeafSize(0.001f, 0.001f, 0.001f);
	VG.filter(*cloud_source);

	//可视化相关
	int v1 = 1;
	viewer->createViewPort(0.0, 0.0, 1.0, 1.0, v1);
	viewer->setBackgroundColor(255, 255, 255, v1);
	viewer->addPointCloud(cloud_target, pcl::visualization::PointCloudColorHandlerCustom(cloud_target, 0.0, 0.0, 0.0), "cloud_target", v1);
	viewer->addPointCloud(cloud_source, pcl::visualization::PointCloudColorHandlerCustom(cloud_source, 255.0, 0.0, 0.0), "cloud_source", v1);
	viewer->registerKeyboardCallback(keyboardEventOccurred, (void*)viewer.get());


	while (!viewer->wasStopped()) {
		// 应用变换
		pcl::transformPointCloud(*cloud_source, *cloud_source, G);
		G = Eigen::Affine3f::Identity();


		// 更新视图
		viewer->removePointCloud("cloud_source");
		viewer->addPointCloud(cloud_source, pcl::visualization::PointCloudColorHandlerCustom(cloud_source, 255.0, 0.0, 0.0), "cloud_source", v1);
		viewer->removePointCloud("cloud_target");
		viewer->addPointCloud(cloud_target, pcl::visualization::PointCloudColorHandlerCustom(cloud_target, 0.0, 0.0, 0.0), "cloud_target", v1);

		viewer->spinOnce(10);  // 每次更新等待10ms
	}

}

#endif // 0

//键盘回调函数
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event, void* viewer_void)
{
	if (event.keyDown())
	{
		if (event.getKeySym() == "space") {
			pcl::IterativeClosestPoint icp;
			icp.setInputSource(cloud_source);
			icp.setInputTarget(cloud_target);
			icp.setTransformationEpsilon(1e-10);
			icp.setMaxCorrespondenceDistance(100);
			icp.setEuclideanFitnessEpsilon(0.001); 
			icp.setMaximumIterations(10000);
			icp.setUseReciprocalCorrespondences(false);

			icp.align(*cloud_icp);
			G = icp.getFinalTransformation();

		}else if(event.getKeySym() == "m")  // 
		{ 
		       pcl::PointCloud::Ptr source(new pcl::PointCloud);
		       pcl::PointCloud::Ptr target(new pcl::PointCloud);
		       pcl::PointCloud::Ptr icp_normal(new pcl::PointCloud);
		        // 计算点云的法向量
		       pcl::NormalEstimationOMP n;//OMP加速
		       pcl::PointCloud::Ptr normals_source(new pcl::PointCloud);
		       //建立kdtree来进行近邻点集搜索
		       pcl::search::KdTree::Ptr tree(new pcl::search::KdTree());
		       n.setNumberOfThreads(10);//设置openMP的线程数 我一般是10条就够了
		        //主要是改变这个值来看看点云的法向量的方向变换
		       n.setViewPoint(0, 0, 0);//设置视点，默认为（0，0，0）
		       n.setInputCloud(cloud_source);
		       n.setSearchMethod(tree);
		       n.setKSearch(10);//点云法向计算时，需要所搜的近邻点大小
		       //n.setRadiusSearch(0.03);//半径搜素
		       n.compute(*normals_source);//开始进行法向计
		       pcl::concatenateFields(*cloud_source, *normals_source, *source);
		       
		       n.setInputCloud(cloud_target);
		       pcl::PointCloud::Ptr normals_target(new pcl::PointCloud);
		       n.compute(*normals_target);//开始进行法向计
		       pcl::concatenateFields(*cloud_target, *normals_target, *target);


			   // 开始匹配
				pcl::IterativeClosestPointWithNormals icp;
				icp.setInputSource(source);
				icp.setInputTarget(target);
				//icp.setCorrespondenceEstimation(0.1f);  // 计算点云的共同部分  对应关系估计
				icp.setTransformationEpsilon(1e-10);  //两个连续转换之间的最大允许平移平方差
				icp.setMaxCorrespondenceDistance(100); //目标中两个对应点之间的最大距离阈值。如果距离大于此阈值，则在对齐过程中将忽略这些点
				icp.setEuclideanFitnessEpsilon(0.001);   // 算法收敛之前最大误差
				icp.setMaximumIterations(10000);
				icp.setUseReciprocalCorrespondences(false);

				icp.align(*icp_normal);
				G = icp.getFinalTransformation();
				f1=icp.getFitnessScore(); //getFitnessScore()函数
		}
		else
		{
			switch (event.getKeySym()[0])
			{
			case 'w':
				G.translation() += Eigen::Vector3f(translation_step, 0, 0);
				break;
			case 's':
				G.translation() -= Eigen::Vector3f(translation_step, 0, 0);
				break;
			case 'a':
				G.translation() -= Eigen::Vector3f(0, translation_step, 0);
				break;
			case 'd':
				G.translation() += Eigen::Vector3f(0, translation_step, 0);
				break;
			case 'z':
				G.translation() += Eigen::Vector3f(0, 0, translation_step);
				break;
			case 'c':
				G.translation() -= Eigen::Vector3f(0, 0, translation_step);
				break;
			case 'r':
				G.rotate(Eigen::AngleAxisf(rotation_step, Eigen::Vector3f::UnitX()));
				break;  // Roll
			case 'p':
				G.rotate(Eigen::AngleAxisf(rotation_step, Eigen::Vector3f::UnitY()));
				break;  // Pitch
			case 'y':
				G.rotate(Eigen::AngleAxisf(rotation_step, Eigen::Vector3f::UnitZ()));
				break;  // Yaw
			default: break;
			}
		}
	}
	cout << "变换矩阵   " << endl;
	cout << G.matrix() << endl;
	cout << endl;
	cout << endl;
	cout << endl;
}