一、pcl中点云配准算法

PCL 库中 ICP 的接口及其变种：

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

其中，IterativeClosestPoint 模板类是 ICP 算法的一个基本实现，其优化求解方法基于 Singular Value Decomposition (SVD)，算法迭代结束条件包括：

最大迭代次数：Number of iterations has reached the maximum user imposed number of iterations (via setMaximumIterations)
两次变换矩阵之间的差值：The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via setTransformationEpsilon)
均方误差：The sum of Euclidean squared errors is smaller than a user defined threshold (via setEuclideanFitnessEpsilon)

基本用法：

IterativeClosestPoint<PointXYZ, PointXYZ> icp;// Set the input source and target
icp.setInputCloud (cloud_source);
icp.setInputTarget (cloud_target);// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
icp.setMaxCorrespondenceDistance (0.05);
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (50);
// Set the transformation epsilon (criterion 2)
icp.setTransformationEpsilon (1e-8);
// Set the euclidean distance difference epsilon (criterion 3)
icp.setEuclideanFitnessEpsilon (1);// Perform the alignment
icp.align (cloud_source_registered);
// Obtain the transformation that aligned cloud_source to cloud_source_registered
Eigen::Matrix4f transformation = icp.getFinalTransformation ();

两帧点云配置算法可以看这里

How to incrementally register pairs of clouds — Point Cloud Library 0.0 documentation (pcl.readthedocs.io)

GitHub - geekerboy/pairwise_incremental_registration: 修复参考书代码中Segmentation fault (core dumped) 问题

二、关于svd原理求解部分

高翔视觉SLAM十四讲求解 ICP 的代码

void pose_estimation_3d3d(const vector<Point3f>& pts1,const vector<Point3f>& pts2,Mat& R, Mat& t)
{// 求质心Point3f p1, p2;int N = pts1.size();for (int i=0; i<N; i++){p1 += pts1[i];p2 += pts2[i];}p1 /= N;p2 /= N;// 去质心vector<Point3f> q1(N), q2(N);for (int i=0; i<N; i++){q1[i] = pts1[i] - p1;q2[i] = pts2[i] - p2;}// 计算 q1*q2^TEigen::Matrix3d W = Eigen::Matrix3d::Zero();for (int i=0; i<N; i++){W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x,q2[i].y, q2[i].z).transpose();}cout << "W=" << W << endl;// 对W进行SVD求解RtEigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);Eigen::Matrix3d U = svd.matrixU();Eigen::Matrix3d V = svd.matrixV();cout << "U=" << U << endl;cout << "V=" << V << endl;Eigen::Matrix3d R_ = U * (V.transpose());Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);// Eigen 转换成 cv::MatR = (Mat_<double>(3, 3) <<R_(0, 0), R_(0, 1), R_(0,2),R_(1, 0), R_(1, 1), R_(1,2),R_(2, 0), R_(2, 1), R_(2,2));t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}

另外的方法求RT，本质也是svd分解

/// <summary>
/// 通过svd分解求解旋转和平移
/// </summary>
/// <param name="A"></param>
/// <param name="B"></param>
/// <returns>返回值为4*4变换矩阵T</returns>
Eigen::Matrix4d best_fit_transform(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B) {/*Notice:1/ JacobiSVD return U,S,V, S as a vector, "use U*S*Vt" to get original Matrix;2/ matrix type 'MatrixXd' or 'MatrixXf' matters.*/Eigen::Matrix4d T = Eigen::MatrixXd::Identity(4, 4);Eigen::Vector3d centroid_A(0, 0, 0);Eigen::Vector3d centroid_B(0, 0, 0);Eigen::MatrixXd AA = A;Eigen::MatrixXd BB = B;int row = A.rows();for (int i = 0; i < row; i++) {centroid_A += A.block<1, 3>(i, 0).transpose();centroid_B += B.block<1, 3>(i, 0).transpose();}centroid_A /= row;centroid_B /= row;for (int i = 0; i < row; i++) {AA.block<1, 3>(i, 0) = A.block<1, 3>(i, 0) - centroid_A.transpose();BB.block<1, 3>(i, 0) = B.block<1, 3>(i, 0) - centroid_B.transpose();}Eigen::MatrixXd H = AA.transpose() * BB;Eigen::MatrixXd U;Eigen::VectorXd S;Eigen::MatrixXd V;Eigen::MatrixXd Vt;Eigen::Matrix3d R;Eigen::Vector3d t;JacobiSVD<Eigen::MatrixXd> svd(H, ComputeFullU | ComputeFullV);U = svd.matrixU();S = svd.singularValues();V = svd.matrixV();Vt = V.transpose();R = Vt.transpose() * U.transpose();if (R.determinant() < 0) {Vt.block<1, 3>(2, 0) *= -1;R = Vt.transpose() * U.transpose();}t = centroid_B - R * centroid_A;T.block<3, 3>(0, 0) = R;T.block<3, 1>(0, 3) = t;return T;}

icp求解是利用pcl工具来做，省时省力。

Introduction — Point Cloud Library 1.12.1-dev documentation (pointclouds.org)

Interactive Iterative Closest Point — Point Cloud Library 1.12.1-dev documentation (pointclouds.org)

三、pcl IterativeClosestPoint 完成demo

代码：

#include <iostream>
#include <numeric>
#include "icp.h"
#include "Eigen/Eigen"using namespace std;
using namespace Eigen;/// <summary>
/// 通过svd分解求解旋转和平移
/// </summary>
/// <param name="A"></param>
/// <param name="B"></param>
/// <returns>返回值为4*4变换矩阵T</returns>
Eigen::Matrix4d best_fit_transform(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B) {/*Notice:1/ JacobiSVD return U,S,V, S as a vector, "use U*S*Vt" to get original Matrix;2/ matrix type 'MatrixXd' or 'MatrixXf' matters.*/Eigen::Matrix4d T = Eigen::MatrixXd::Identity(4, 4);Eigen::Vector3d centroid_A(0, 0, 0);Eigen::Vector3d centroid_B(0, 0, 0);Eigen::MatrixXd AA = A;Eigen::MatrixXd BB = B;int row = A.rows();for (int i = 0; i < row; i++) {centroid_A += A.block<1, 3>(i, 0).transpose();centroid_B += B.block<1, 3>(i, 0).transpose();}centroid_A /= row;centroid_B /= row;for (int i = 0; i < row; i++) {AA.block<1, 3>(i, 0) = A.block<1, 3>(i, 0) - centroid_A.transpose();BB.block<1, 3>(i, 0) = B.block<1, 3>(i, 0) - centroid_B.transpose();}Eigen::MatrixXd H = AA.transpose() * BB;Eigen::MatrixXd U;Eigen::VectorXd S;Eigen::MatrixXd V;Eigen::MatrixXd Vt;Eigen::Matrix3d R;Eigen::Vector3d t;JacobiSVD<Eigen::MatrixXd> svd(H, ComputeFullU | ComputeFullV);U = svd.matrixU();S = svd.singularValues();V = svd.matrixV();Vt = V.transpose();R = Vt.transpose() * U.transpose();if (R.determinant() < 0) {Vt.block<1, 3>(2, 0) *= -1;R = Vt.transpose() * U.transpose();}t = centroid_B - R * centroid_A;T.block<3, 3>(0, 0) = R;T.block<3, 1>(0, 3) = t;return T;}/*
typedef struct{Eigen::Matrix4d trans;std::vector<float> distances;int iter;
}  ICP_OUT;
*/ICP_OUT icp(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, int max_iterations, int tolerance) {int row = A.rows();Eigen::MatrixXd src = Eigen::MatrixXd::Ones(3 + 1, row);Eigen::MatrixXd src3d = Eigen::MatrixXd::Ones(3, row);Eigen::MatrixXd dst = Eigen::MatrixXd::Ones(3 + 1, row);NEIGHBOR neighbor;Eigen::Matrix4d T;Eigen::MatrixXd dst_chorder = Eigen::MatrixXd::Ones(3, row);ICP_OUT result;int iter = 0;for (int i = 0; i < row; i++) {src.block<3, 1>(0, i) = A.block<1, 3>(i, 0).transpose();src3d.block<3, 1>(0, i) = A.block<1, 3>(i, 0).transpose();dst.block<3, 1>(0, i) = B.block<1, 3>(i, 0).transpose();}double prev_error = 0;double mean_error = 0;for (int i = 0; i < max_iterations; i++) {neighbor = nearest_neighbot(src3d.transpose(), B);for (int j = 0; j < row; j++) {dst_chorder.block<3, 1>(0, j) = dst.block<3, 1>(0, neighbor.indices[j]);}T = best_fit_transform(src3d.transpose(), dst_chorder.transpose());src = T * src;for (int j = 0; j < row; j++) {src3d.block<3, 1>(0, j) = src.block<3, 1>(0, j);}mean_error = std::accumulate(neighbor.distances.begin(), neighbor.distances.end(), 0.0) / neighbor.distances.size();if (abs(prev_error - mean_error) < tolerance) {break;}prev_error = mean_error;iter = i + 2;}T = best_fit_transform(A, src3d.transpose());result.trans = T;result.distances = neighbor.distances;result.iter = iter;return result;
}/*
typedef struct{std::vector<float> distances;std::vector<int> indices;
} NEIGHBOR;
*/NEIGHBOR nearest_neighbot(const Eigen::MatrixXd& src, const Eigen::MatrixXd& dst) {int row_src = src.rows();int row_dst = dst.rows();Eigen::Vector3d vec_src;Eigen::Vector3d vec_dst;NEIGHBOR neigh;float min = 100;int index = 0;float dist_temp = 0;for (int ii = 0; ii < row_src; ii++) {vec_src = src.block<1, 3>(ii, 0).transpose();min = 100;index = 0;dist_temp = 0;for (int jj = 0; jj < row_dst; jj++) {vec_dst = dst.block<1, 3>(jj, 0).transpose();dist_temp = dist(vec_src, vec_dst);if (dist_temp < min) {min = dist_temp;index = jj;}}// cout << min << " " << index << endl;// neigh.distances[ii] = min;// neigh.indices[ii] = index;neigh.distances.push_back(min);neigh.indices.push_back(index);}return neigh;
}float dist(const Eigen::Vector3d& pta, const Eigen::Vector3d& ptb) {return sqrt((pta[0] - ptb[0]) * (pta[0] - ptb[0]) + (pta[1] - ptb[1]) * (pta[1] - ptb[1]) + (pta[2] - ptb[2]) * (pta[2] - ptb[2]));
}

截图：

点云 ICP学习-IterativeClosestPoint相关推荐

点云深度学习系列博客(二): 点云配准网络PCRNet
目录一. 简介二. 基础结构三. 项目代码四. 实验结果总结 Reference 今天的点云深度学习系列博客为大家介绍一个用于点云配准的深度网络:PCRNet [1].凡是对点云相关应用有些 ...
浪潮云分布式云ICP加速千行百业羽化创新
刚刚告别的2022年,是党和国家历史上极为不寻常的一年,振奋于历史性盛会发出继往开来的豪迈宣言,在经济下行压力加大的情况下,数字经济作为国民经济的"加速器"作用凸显,成为经济恢复向 ...
《PCL点云库学习VS2010(X64)》Part 41 图形学领域的关键算法及源码链接
<PCL点云库学习&VS2010(X64)>Part 41 图形学领域的关键算法及源码链接原文链接: Conference papers Graphics Conference ...
Java云同桌学习系列（十三）——前端技术之HTML与CSS
本博客java云同桌学习系列,旨在记录本人学习java的过程,并与大家分享,对于想学习java的同学,可以跟随我的步伐一起学习,我希望这个系列能够鼓励大家一同与我学习java,成为"云同桌& ...
点云深度学习研究现状与趋势
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达本文转自:3D点云深度学习作者:霍尔顿在工业界,利用激光雷达获 ...
自动驾驶LiDAR点云深度学习综述
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 / 导读 / 本文是滑铁卢大学CogDrive实验室和Geospatial Sensing and D ...
2020最新点云深度学习综述
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达论文阅读模块将分享点云处理,SLAM,三维视觉,高精地图相关的文章. ●论文摘要点云是在三维空间中点 ...
国防科技大学发布最新「3D点云深度学习」综述论文
点击上方"深度学习技术前沿",选择"星标"公众号资源干货,第一时间送达 3D点云学习( Point Clouds)作为近年来的研究热点之一,受到了广泛关注,每 ...
分割点云数据_3D点云深度学习综述：三维形状分类、目标检测与跟踪、点云分割等...
3D点云学习( Point Clouds)作为近年来的研究热点之一,受到了广泛关注,每年在各大会议上都有大量的相关文章发表.当前,点云上的深度学习变得越来越流行,人们提出了许多方法来解决这一领域的不同 ...
重读经典（点云深度学习开山之作）：《Deep learning on point clouds for 3D scene understanding》(持续更新中)
本文介绍的是 PointNet 作者的博士论文:3D场景理解中的点云深度学习.从上图可以看到,整个博士论文主要贡献有两块:一是点云深度学习的网络架构(PointNet 和 PointNet++):二是 ...

点云 ICP学习-IterativeClosestPoint

一、pcl中点云配准算法

二、关于svd原理求解部分

三、pcl IterativeClosestPoint 完成demo

点云 ICP学习-IterativeClosestPoint相关推荐

最新文章

热门文章

点云 ICP学习-IterativeClosestPoint

一、pcl中 点云配准算法

二、关于svd原理求解部分

三、pcl IterativeClosestPoint 完成demo

点云 ICP学习-IterativeClosestPoint相关推荐

最新文章

热门文章

一、pcl中点云配准算法