算法输入:两个点集,source和target
算法输出:旋转矩阵R,平移矩阵T
算法步骤:
1、采集点集src属于source(均匀、随即、法失) (不一定必须要)
2、对于src中每一个点,在target中找到对应的最近点,组成点集tgt(点到点、点到面。。。)
3、求得使src刚体变换后与上述tgt对应点平均距离最小的旋转矩阵R和平移矩阵T(svd、四元数法)
4、对src做上一步得到的旋转、平移变换得到新的变换点集src'
5、如果新的变换点集src'与目标点集tgt满足目标函数(两点集的平均距离小于给定的阈值或大于某一迭代次数),则停止迭代,否则新的变换点集src'作为新的src继续迭代,直到达到目标函数要求
SVD方法求解:《视觉slam十四讲》173页
算法原理:百度搜
代码实现:参考PCL之ICP算法实现
代码目标:实现ICP并验证结果
代码实现思路:
(1)实现ICP:
1、建立源点云、目标点云和变换的点云
2、创建ICP的实例类并执行ICP
(2)验证结果:
1、读相机坐标系下变换数据(1秒到0秒的)
2、读雷达坐标系变换到相机坐标系的变换矩阵Tr
3、计算激光雷达坐标系下1-0变换矩阵
4、对比ICP得到的变换矩阵和上述计算的激光雷达下变换矩阵
代码:icpV2
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/parse.h> //pcl控制台解析
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算(逆,特征值等)
#include <Eigen/Dense>
int main ()
{
//****ICP求变换矩阵
//建立源点云、目标点云和变换后的点云
pcl::PointCloud<p