PCL 点云匹配 之NICP(Normal ICP)
2023-12-20 01:57:34
一、概述
?上面一篇中我们已经得出了一个结论,就是ICP虽然简单,但是也有明显的缺点
1、计算速度慢,收敛慢,迭代次数多
2、对内存的开销比较大
3、很容易陷入局部最优的困局
因此我们在经典ICP的基础上添加一两个约束:
第一个约束就是添加法向量,计算当前点R半斤内的法向量,算法向量的夹角
第二个约束就是添加曲率,添加曲率
这样我们就可以减少迭代的次数,加速收敛。
二、计算点云的法向量和曲率
原理:
?找到当前点pi的r 半斤类的所有的点V,然后计算出当前点集V的均值ui,然后开始计算V到的协方差
?由于协方差的对称性,我们可以对这个矩阵进行SVD矩阵分解
曲率:
?是按照从小到大的顺序排列的,那么曲率可以计算:
??,并且?越小表示当前点云半斤内越平坦
最小的特征值对应的就是法向量的方向。
法向量
求解最小特征值的特征向量即为法向量
法向量的定向
从上面我们可以计算出法向量,那么法向量如何定向呢,也就是说一条直线你规定那个方向为正方向呢???
可以用视点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 <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
//#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include "vtkAutoInit.h"
VTK_MODULE_INIT(vtkRenderingOpenGL);
VTK_MODULE_INIT(vtkInteractionStyle);
VTK_MODULE_INIT(vtkRenderingFreeType)
#if 1
int main()
{
//------------------加载点云数据-------------------
//C:\Users\Albert\Desktop\halcon_to_pcl\normal\normaltest2.pcd
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("C:\\Users\\Albert\\Desktop\\halcon_to_pcl\\classfiy\\0.pcd", *cloud) == -1)
{
// 找不到点云文件
return -1;
}
//------------------计算法线----------------------
pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;//OMP加速
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
//建立kdtree来进行近邻点集搜索
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
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<pcl::visualization::PCLVisualizer> 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<pcl::PointXYZ> single_color(cloud, 255, 225,255);
//添加坐标系
viewer->addCoordinateSystem(0.1);
viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample cloud");
//添加需要显示的点云法向。cloud为原始点云模型,normal为法向信息,10表示需要显示法向的点云间隔,即每10个点显示一次法向,0.1表示法向长度。
viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(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
四、算法原理
五、算法流程
文章来源:https://blog.csdn.net/weixin_39354845/article/details/135079296
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符,请联系我的编程经验分享网邮箱:veading@qq.com进行投诉反馈,一经查实,立即删除!
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。 如若内容造成侵权/违法违规/事实不符,请联系我的编程经验分享网邮箱:veading@qq.com进行投诉反馈,一经查实,立即删除!