点云配准matlab代码_点到面的ICP配准

一、算法原理

  点到平面度量通常使用标准非线性最小二乘法来求解，例如Levenberg-Marquardt。点到平面ICP算法的每次迭代通常比点到点算法慢，但收敛速度明显更快。两个点云之间的相对旋转小于30°，在旋转矩阵中用θ替换sin θ，用1替换cos θ实现用线性逼近非线性最小二乘优化问题，加快计算速度。
  为了提高计算的数值稳定性，需要使用与旋转角度大小相当的距离。最简单的方法是重新缩放和移动两个输入点云，使它们在以原点为中心的单位球体或立方体内有界。

参考文献：
[1] Low K L. Linear least-squares optimization for point-to-plane icp surface registration[J]. Chapel Hill, University of North Carolina, 2004, 4(10): 1-3
[2] 李宇翔,郭际明,潘尚毅,吕丽丽,卢主兴,章迪.一种基于ISS-SHOT特征的点云配准算法[J].测绘通报,2020(04):21-26.

二、代码实现

1、C++版本

#include 
#include 
#include 
#include 
#include  // icp算法
#include"ViewerCloud.h"

using namespace std;

void cloud_with_normal(pcl::PointCloud<:pointxyz>::Ptr& cloud, pcl::PointCloud<:pointnormal>::Ptr& cloud_normals) {   //---------------------拼接点云数据与法线信息-------------------
    pcl::NormalEstimationOMP<:pointxyz> n;//OMP加速
    pcl::PointCloud<:normal&g