• 点云 ICP学习-IterativeClosestPoint

    目录

    一、pcl中 点云配准算法 

     二、关于svd原理求解部分

    三、pcl IterativeClosestPoint 完成demo

    一、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)

    基本用法：

    1. IterativeClosestPoint<PointXYZ, PointXYZ> icp;
    2.  
    3. // Set the input source and target
    4. icp.setInputCloud (cloud_source);
    5. icp.setInputTarget (cloud_target);
    6.  
    7. // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
    8. icp.setMaxCorrespondenceDistance (0.05);
    9. // Set the maximum number of iterations (criterion 1)
    10. icp.setMaximumIterations (50);
    11. // Set the transformation epsilon (criterion 2)
    12. icp.setTransformationEpsilon (1e-8);
    13. // Set the euclidean distance difference epsilon (criterion 3)
    14. icp.setEuclideanFitnessEpsilon (1);
    15.  
    16. // Perform the alignment
    17. icp.align (cloud_source_registered);
    18. // Obtain the transformation that aligned cloud_source to cloud_source_registered
    19. 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 的代码

    1. void pose_estimation_3d3d(const vector<Point3f>& pts1,
    2.                           const vector<Point3f>& pts2,
    3.                           Mat& R, Mat& t)
    4. {
    5.     // 求质心
    6.     Point3f p1, p2;
    7.     int N = pts1.size();
    8.     for (int i=0; i<N; i++)
    9.     {
    10.         p1 += pts1[i];
    11.         p2 += pts2[i];
    12.     }
    13.     p1 /= N;
    14.     p2 /= N;
    15.  
    16.     // 去质心
    17.     vector<Point3f> q1(N), q2(N);
    18.     for (int i=0; i<N; i++)
    19.     {
    20.         q1[i] = pts1[i] - p1;
    21.         q2[i] = pts2[i] - p2;
    22.     }
    23.  
    24.     // 计算 q1*q2^T
    25.     Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    26.     for (int i=0; i<N; i++)
    27.     {
    28.         W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x,
    29.                 q2[i].y, q2[i].z).transpose();
    30.     }
    31.     cout << "W=" << W << endl;
    32.  
    33.     // 对W进行SVD求解Rt
    34.     Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    35.     Eigen::Matrix3d U = svd.matrixU();
    36.     Eigen::Matrix3d V = svd.matrixV();
    37.     cout << "U=" << U << endl;
    38.     cout << "V=" << V << endl;
    39.  
    40.     Eigen::Matrix3d R_ = U * (V.transpose());
    41.     Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);
    42.  
    43.     // Eigen 转换成 cv::Mat
    44.     R = (Mat_<double>(3, 3) <<
    45.             R_(0, 0), R_(0, 1), R_(0,2),
    46.             R_(1, 0), R_(1, 1), R_(1,2),
    47.             R_(2, 0), R_(2, 1), R_(2,2));
    48.     t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
    49. }

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

    1. /// <summary>
    2. /// 通过svd分解求解旋转和平移
    3. /// </summary>
    4. /// <param name="A"></param>
    5. /// <param name="B"></param>
    6. /// <returns>返回值为4*4变换矩阵T</returns>
    7. Eigen::Matrix4d best_fit_transform(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B) {
    8.     /*
    9.     Notice:
    10.     1/ JacobiSVD return U,S,V, S as a vector, "use U*S*Vt" to get original Matrix;
    11.     2/ matrix type 'MatrixXd' or 'MatrixXf' matters.
    12.     */
    13.     Eigen::Matrix4d T = Eigen::MatrixXd::Identity(4, 4);
    14.     Eigen::Vector3d centroid_A(0, 0, 0);
    15.     Eigen::Vector3d centroid_B(0, 0, 0);
    16.     Eigen::MatrixXd AA = A;
    17.     Eigen::MatrixXd BB = B;
    18.     int row = A.rows();
    19.  
    20.     for (int i = 0; i < row; i++) {
    21.         centroid_A += A.block<1, 3>(i, 0).transpose();
    22.         centroid_B += B.block<1, 3>(i, 0).transpose();
    23.     }
    24.     centroid_A /= row;
    25.     centroid_B /= row;
    26.     for (int i = 0; i < row; i++) {
    27.         AA.block<1, 3>(i, 0) = A.block<1, 3>(i, 0) - centroid_A.transpose();
    28.         BB.block<1, 3>(i, 0) = B.block<1, 3>(i, 0) - centroid_B.transpose();
    29.     }
    30.  
    31.     Eigen::MatrixXd H = AA.transpose() * BB;
    32.     Eigen::MatrixXd U;
    33.     Eigen::VectorXd S;
    34.     Eigen::MatrixXd V;
    35.     Eigen::MatrixXd Vt;
    36.     Eigen::Matrix3d R;
    37.     Eigen::Vector3d t;
    38.  
    39.     JacobiSVD<Eigen::MatrixXd> svd(H, ComputeFullU | ComputeFullV);
    40.     U = svd.matrixU();
    41.     S = svd.singularValues();
    42.     V = svd.matrixV();
    43.     Vt = V.transpose();
    44.     R = Vt.transpose() * U.transpose();
    45.  
    46.     if (R.determinant() < 0) {
    47.         Vt.block<1, 3>(2, 0) *= -1;
    48.         R = Vt.transpose() * U.transpose();
    49.     }
    50.  
    51.     t = centroid_B - R * centroid_A;
    52.  
    53.     T.block<3, 3>(0, 0) = R;
    54.     T.block<3, 1>(0, 3) = t;
    55.     return T;
    56.  
    57. }

    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

    代码：

    1. #include <iostream>
    2. #include <numeric>
    3. #include "icp.h"
    4. #include "Eigen/Eigen"
    5.  
    6. using namespace std;
    7. using namespace Eigen;
    8.  
    9.  
    10. /// <summary>
    11. /// 通过svd分解求解旋转和平移
    12. /// </summary>
    13. /// <param name="A"></param>
    14. /// <param name="B"></param>
    15. /// <returns>返回值为4*4变换矩阵T</returns>
    16. Eigen::Matrix4d best_fit_transform(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B) {
    17.     /*
    18.     Notice:
    19.     1/ JacobiSVD return U,S,V, S as a vector, "use U*S*Vt" to get original Matrix;
    20.     2/ matrix type 'MatrixXd' or 'MatrixXf' matters.
    21.     */
    22.     Eigen::Matrix4d T = Eigen::MatrixXd::Identity(4, 4);
    23.     Eigen::Vector3d centroid_A(0, 0, 0);
    24.     Eigen::Vector3d centroid_B(0, 0, 0);
    25.     Eigen::MatrixXd AA = A;
    26.     Eigen::MatrixXd BB = B;
    27.     int row = A.rows();
    28.  
    29.     for (int i = 0; i < row; i++) {
    30.         centroid_A += A.block<1, 3>(i, 0).transpose();
    31.         centroid_B += B.block<1, 3>(i, 0).transpose();
    32.     }
    33.     centroid_A /= row;
    34.     centroid_B /= row;
    35.     for (int i = 0; i < row; i++) {
    36.         AA.block<1, 3>(i, 0) = A.block<1, 3>(i, 0) - centroid_A.transpose();
    37.         BB.block<1, 3>(i, 0) = B.block<1, 3>(i, 0) - centroid_B.transpose();
    38.     }
    39.  
    40.     Eigen::MatrixXd H = AA.transpose() * BB;
    41.     Eigen::MatrixXd U;
    42.     Eigen::VectorXd S;
    43.     Eigen::MatrixXd V;
    44.     Eigen::MatrixXd Vt;
    45.     Eigen::Matrix3d R;
    46.     Eigen::Vector3d t;
    47.  
    48.     JacobiSVD<Eigen::MatrixXd> svd(H, ComputeFullU | ComputeFullV);
    49.     U = svd.matrixU();
    50.     S = svd.singularValues();
    51.     V = svd.matrixV();
    52.     Vt = V.transpose();
    53.     R = Vt.transpose() * U.transpose();
    54.  
    55.     if (R.determinant() < 0) {
    56.         Vt.block<1, 3>(2, 0) *= -1;
    57.         R = Vt.transpose() * U.transpose();
    58.     }
    59.  
    60.     t = centroid_B - R * centroid_A;
    61.  
    62.     T.block<3, 3>(0, 0) = R;
    63.     T.block<3, 1>(0, 3) = t;
    64.     return T;
    65.  
    66. }
    67.  
    68. /*
    69. typedef struct{
    70.     Eigen::Matrix4d trans;
    71.     std::vector<float> distances;
    72.     int iter;
    73. }  ICP_OUT;
    74. */
    75.  
    76. ICP_OUT icp(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, int max_iterations, int tolerance) {
    77.     int row = A.rows();
    78.     Eigen::MatrixXd src = Eigen::MatrixXd::Ones(3 + 1, row);
    79.     Eigen::MatrixXd src3d = Eigen::MatrixXd::Ones(3, row);
    80.     Eigen::MatrixXd dst = Eigen::MatrixXd::Ones(3 + 1, row);
    81.     NEIGHBOR neighbor;
    82.     Eigen::Matrix4d T;
    83.     Eigen::MatrixXd dst_chorder = Eigen::MatrixXd::Ones(3, row);
    84.     ICP_OUT result;
    85.     int iter = 0;
    86.  
    87.     for (int i = 0; i < row; i++) {
    88.         src.block<3, 1>(0, i) = A.block<1, 3>(i, 0).transpose();
    89.         src3d.block<3, 1>(0, i) = A.block<1, 3>(i, 0).transpose();
    90.         dst.block<3, 1>(0, i) = B.block<1, 3>(i, 0).transpose();
    91.     }
    92.  
    93.     double prev_error = 0;
    94.     double mean_error = 0;
    95.     for (int i = 0; i < max_iterations; i++) 
    96.     {
    97.         neighbor = nearest_neighbot(src3d.transpose(), B);
    98.         for (int j = 0; j < row; j++) 
    99.         {
    100.             dst_chorder.block<3, 1>(0, j) = dst.block<3, 1>(0, neighbor.indices[j]);
    101.         }
    102.         T = best_fit_transform(src3d.transpose(), dst_chorder.transpose());
    103.         src = T * src;
    104.         for (int j = 0; j < row; j++) 
    105.         {
    106.             src3d.block<3, 1>(0, j) = src.block<3, 1>(0, j);
    107.         }
    108.         mean_error = std::accumulate(neighbor.distances.begin(), neighbor.distances.end(), 0.0) / neighbor.distances.size();
    109.         if (abs(prev_error - mean_error) < tolerance) 
    110.         {
    111.             break;
    112.         }
    113.         prev_error = mean_error;
    114.         iter = i + 2;
    115.     }
    116.  
    117.     T = best_fit_transform(A, src3d.transpose());
    118.     result.trans = T;
    119.     result.distances = neighbor.distances;
    120.     result.iter = iter;
    121.  
    122.     return result;
    123. }
    124.  
    125. /*
    126. typedef struct{
    127.     std::vector<float> distances;
    128.     std::vector<int> indices;
    129. } NEIGHBOR;
    130. */
    131.  
    132. NEIGHBOR nearest_neighbot(const Eigen::MatrixXd& src, const Eigen::MatrixXd& dst) {
    133.     int row_src = src.rows();
    134.     int row_dst = dst.rows();
    135.     Eigen::Vector3d vec_src;
    136.     Eigen::Vector3d vec_dst;
    137.     NEIGHBOR neigh;
    138.     float min = 100;
    139.     int index = 0;
    140.     float dist_temp = 0;
    141.  
    142.     for (int ii = 0; ii < row_src; ii++) {
    143.         vec_src = src.block<1, 3>(ii, 0).transpose();
    144.         min = 100;
    145.         index = 0;
    146.         dist_temp = 0;
    147.         for (int jj = 0; jj < row_dst; jj++) {
    148.             vec_dst = dst.block<1, 3>(jj, 0).transpose();
    149.             dist_temp = dist(vec_src, vec_dst);
    150.             if (dist_temp < min) {
    151.                 min = dist_temp;
    152.                 index = jj;
    153.             }
    154.         }
    155.         // cout << min << " " << index << endl;
    156.         // neigh.distances[ii] = min;
    157.         // neigh.indices[ii] = index;
    158.         neigh.distances.push_back(min);
    159.         neigh.indices.push_back(index);
    160.     }
    161.  
    162.     return neigh;
    163. }
    164.  
    165.  
    166. float dist(const Eigen::Vector3d& pta, const Eigen::Vector3d& ptb) {
    167.     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]));
    168. }

    截图： 

     

  • 相关阅读:
    第十四届蓝桥杯省赛 C/C++ A 组 H 题——异或和之和（AC）
    pgsql中实现按周统计，计算日期是周几
    Kubectl 使用详解——k8s陈述式资源管理
    instanceOf原理及手动实现
    .NET 跨平台应用开发动手教程 ｜用 Uno Platform 构建一个 Kanban-style Todo App
    《Effective C++》学习笔记——区分接口继承和实现继承
    LVS+Keepalived群集
    【Jetson Nano】jetson nano一些基本功能命令
    遮挡Windows电脑上烦人的微信/企业微信/钉钉消息闪烁提醒
    vue项目嵌套（vue2嵌套vue3）
  • 原文地址：https://blog.csdn.net/cangqiongxiaoye/article/details/127935346