ICP最近点迭代点云匹配算法

最新推荐文章于 2023-08-26 13:16:13 发布

迷失的季节NUAA

最新推荐文章于 2023-08-26 13:16:13 发布

阅读量172

点赞数 1

文章标签：算法 c++ 开发语言

本文链接：https://blog.csdn.net/tktkktk/article/details/129867180

版权

使用PCL库

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <ros/ros.h>
int main(int argc, char **argv)
{
  ros::init(argc, argv, "iterative_closest_point");

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
  // 填入点云数据
  cloud_in->width = 500;
  cloud_in->height = 1;
  cloud_in->is_dense = false;
  cloud_in->points.resize(cloud_in->width * cloud_in->height);
  for (size_t i = 0; i < cloud_in->points.size(); ++i)
  {
    cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
    cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
    cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
  }
  std::cout << "Saved " << cloud_in->points.size() << " data points to input:"
            << std::endl;
  // for (size_t i = 0; i < cloud_in->points.size(); ++i)
  //   std::cout << "    " << cloud_in->points[i].x << " " << cloud_in->points[i].y << " " << cloud_in->points[i].z << std::endl;
  *cloud_out = *cloud_in;
  // std::cout << "size:" << cloud_out->points.size() << std::endl;
  for (size_t i = 0; i < cloud_in->points.size(); ++i)
  {
    cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
    cloud_out->points[i].y = cloud_in->points[i].y + 0.7f;
    cloud_out->points[i].z = cloud_in->points[i].z + 0.7f;
  }
  // std::cout << "Transformed " << cloud_in->points.size() << " data points:"
  //           << std::endl;
  // for (size_t i = 0; i < cloud_out->points.size(); ++i)
  //   std::cout << "    " << cloud_out->points[i].x << " " << cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;
  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputCloud(cloud_in);
  icp.setInputTarget(cloud_out);
  icp.setMaxCorrespondenceDistance(100); // 目标中两个对应点之间的最大距离阈值。如果距离大于此阈值，则在对齐过程中将忽略这些点
  icp.setMaximumIterations(1000);       //迭代次数已达到用户施加的最大迭代次数
  // icp.setTransformationEpsilon(1e-6);   //先前转换和当前估计转换（即两次位姿转换）之间的 epsilon（差异）小于用户施加的值
  // icp.setEuclideanFitnessEpsilon(1e-6); //欧几里得平方误差的总和小于用户定义的阈值
  // icp.setRANSACIterations(0);           // 设置RANSAC运行次数
  // icp.setInputCloud(cloud_in);
  pcl::PointCloud<pcl::PointXYZ> Final;
  icp.align(Final);
  std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
  std::cout << icp.getFinalTransformation() << std::endl;
  return (0);
}

不使用PCL库

#include <iostream>
#include <ros/ros.h>
#include <eigen3/Eigen/Dense>
#include <vector>
using namespace std;
using namespace Eigen;

#define PI (double)3.141592653589793

// distance of two points (x1, y1,z1) and (x2, y2,z2)
float getDistanceOfTwoPoints(float x1, float y1, float z1,float x2, float y2,float z2)
{
    return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2)+pow(z1 - z2, 2));
}

// get closet point of [x,y,z] in {[sourcePoints_x, sourcePoints_y,sourcePoints_z]}
void getClosestPoint(float x, float y,float z, const vector<Vector3f> &sourcePoints,float &find_x, float &find_y, float &find_z)
{
    double dist_min = 99999;
    for (int i = 0; i < sourcePoints.size(); i++)
    {
        double dist = getDistanceOfTwoPoints(x, y, z,sourcePoints[i](0), sourcePoints[i](1),sourcePoints[i](2));
        if (dist < dist_min)
        {
            dist_min = dist;
            find_x = sourcePoints[i](0);
            find_y = sourcePoints[i](1);
            find_z = sourcePoints[i](2);
        }
    }
}

int getClosestPointID(float x, float y,float z, const vector<Vector3f> &sourcePoints)
{
    int id = 0;
    double dist_min = 99999;
    for (int i = 0; i < sourcePoints.size(); i++)
    {
        double dist = getDistanceOfTwoPoints(x, y, z, sourcePoints[i](0),sourcePoints[i](1),sourcePoints[i](2));
        if (dist < dist_min)
        {
            dist_min = dist;
            id = i;
        }   
    }
    return id;
}


// get all points distance of {sourcePoints_x, sourcePoints_y, sourcePoints_z} and {[targetPoints_x,targetPoints_y,sourcePoints_z]} as loss
float getLossShapeMatch(const vector<Vector3f> &sourcePoints,const vector<Vector3f> &targetPoints)
{
    float loss_sum = 0;
    for (int i = 0; i < targetPoints.size(); i++)
    {
        float closest_x, closest_y,closest_z;
        getClosestPoint(targetPoints[i](0), targetPoints[i](1),targetPoints[i](2), sourcePoints,closest_x, closest_y, closest_z);
        float loss = getDistanceOfTwoPoints(targetPoints[i](0), targetPoints[i](1), targetPoints[i](2),closest_x, closest_y,closest_z);
        loss_sum = loss_sum + loss;
    }
    return loss_sum;
}

//    
/* 点云配准算法–kabsch
 * 两个点云P和P’内各个顶点一一对应
 * 求出旋转平移变换矩阵
 * pts1 是原始点云   pts2是待匹配的点云
 */
void KABSCH(const vector<Vector3f> &pts1, const vector<Vector3f> &pts2)
{

    // 计算质心坐标
    Vector3f p1(0,0,0);
    Vector3f p2(0,0,0); // center of mass

    int N = pts1.size();

    for (int i = 0; i < N; i++)
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }

    p1 = Vector3f((p1) / N);
    p2 = Vector3f((p2) / N);
    
    // 计算每个点的去质心坐标
    std::vector<Vector3f> q1(N), q2(N); // remove the center

    for (int i = 0; i < N; i++)
    {
        q1[i] = pts1[i] - p1;

        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T 求和

    Eigen::Matrix3f W = Eigen::Matrix3f::Zero();

    for (int i = 0; i < N; i++)
    {
        W += Eigen::Vector3f(q1[i](0), q1[i](1), q1[i](2)) * Eigen::Vector3f(q2[i](0), q2[i](1), q2[i](2)).transpose();
    }
 
    // SVD on W  
 
    Eigen::JacobiSVD<Eigen::Matrix3f> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);

    Eigen::Matrix3f U = svd.matrixU();

    Eigen::Matrix3f V = svd.matrixV();

    Eigen::Matrix3f R_12 = U * (V.transpose());

    Eigen::Vector3f t_12 = Eigen::Vector3f(p1(0), p1(1), p1(2)) - R_12 * Eigen::Vector3f(p2(0), p2(1), p2(2));

    // 验证

    Eigen::AngleAxisf R_21;
    

    R_21.fromRotationMatrix(R_12.transpose());

    std::cout<<"R_12.transpose(): "<<std::endl;
    std::cout<<R_12.transpose()<<std::endl;

    cout << "aix: " << R_21.axis().transpose() << endl;

    cout << "angle: " << R_21.angle() * 180 / PI << endl;

    cout << "t: " << (-R_12.transpose() * t_12).transpose() << endl;

}
/* 
 * ICP 算法 
 * source_pcls 原始点云  target_pcls 变换后的点云
 */

void ICP(const vector<Vector3f> &source_pcls, const vector<Vector3f> &target_pcls)
{
    int iter_times = 5000;
    double loss_now;
    double loss_improve = 1000;
    double loss_before = getLossShapeMatch(target_pcls,source_pcls) / source_pcls.size();

    
    int source_pcls_size = source_pcls.size();
    int target_pcls_size = target_pcls.size();

    Vector3f source_pcls_c(0,0,0);
    Vector3f target_pcls_c(0,0,0);

    // 计算质心坐标
    for (size_t i = 0; i < source_pcls_size; i++)
    {
        source_pcls_c += source_pcls[i];  
    }
    source_pcls_c = Vector3f((source_pcls_c) / source_pcls_size);

    for (size_t i = 0; i < target_pcls_size; i++)
    {
        target_pcls_c += target_pcls[i];  
    }
    target_pcls_c = Vector3f((target_pcls_c) / target_pcls_size);

    // 计算去质心坐标
  
    vector<Vector3f> q1(source_pcls_size), q2(target_pcls_size); // remove the center

    q1 = source_pcls;

    // for (size_t i = 0; i < source_pcls_size; i++)
    // {
    //     q1[i] = source_pcls[i] - source_pcls_c;
    // }

    for (size_t i = 0; i < target_pcls_size; i++)
    {
        q2[i] = target_pcls[i] - target_pcls_c;
    }

    Eigen::Matrix3f R0, R1;
    R0.setIdentity();
    R1.setIdentity();
    Eigen::Vector3f t0;
    std::vector<Vector3f> q0 ; //定义 为计算误差
    
    int num = 0;
    float loss_st = 9999; //原始点云到目标点云的距离损失
    while (num < iter_times && /* loss_st>0.5 */loss_improve > 0.04)
    {
        Vector3f q1_c(0,0,0);
        // 计算质心坐标
        for (size_t i = 0; i < q1.size(); i++)
        {
            q1_c += q1[i];
        }
        q1_c = Vector3f((q1_c) / q1.size());

        // 计算去质心坐标
        for (size_t i = 0; i < q1.size(); i++)
        {
            q1[i] = q1[i] - q1_c;
        }

        // compute q1*q2^T 求和
        Eigen::Matrix3f W = Eigen::Matrix3f::Zero();

        // 在q2 中寻找 q1 对应的最近点
        for (size_t i = 0; i < source_pcls_size; i++)
        {
            int id = getClosestPointID(q1[i](0), q1[i](1), q1[i](2),q2);
            W += Eigen::Vector3f(q1[i](0), q1[i](1), q1[i](2)) * Eigen::Vector3f(q2[id](0), q2[id](1), q2[id](2)).transpose();
        }

        // SVD on W

        Eigen::JacobiSVD<Eigen::Matrix3f> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);

        Eigen::Matrix3f U = svd.matrixU();

        Eigen::Matrix3f V = svd.matrixV();

        Eigen::Matrix3f R_12 = U * (V.transpose());

        Eigen::Vector3f t_12 = Eigen::Vector3f(q1_c(0), q1_c(1), q1_c(2)) - R_12 * Eigen::Vector3f(target_pcls_c(0), target_pcls_c(1), target_pcls_c(2));

        Eigen::Vector3f t_21 = (-R_12.transpose() * t_12).transpose();
        Eigen::Matrix3f R_21 = R_12.transpose();
        

        R0 = R0*R_21;  // 最终旋转变换矩阵   从原始点云到目标点云
        R1 = R1*R_12;

        Eigen::Vector3f t1 = Eigen::Vector3f(source_pcls_c(0), source_pcls_c(1), source_pcls_c(2)) - R1 * Eigen::Vector3f(target_pcls_c(0), target_pcls_c(1), target_pcls_c(2));

        t0 = (-R1.transpose() * t1).transpose();


        // 更新变换后的点云向量 下一次循环使用
        for (size_t i = 0; i < source_pcls_size; i++)
        {
            Eigen::Vector3f p = R_21 * q1[i] + t_21;
            q1[i] = p;
        }

        
        q0.clear(); 
        for (size_t i = 0; i < source_pcls_size; i++)
        {
            Eigen::Vector3f p = R0 * source_pcls[i] + t0;
            q0.push_back(p);
        }

        loss_st = getLossShapeMatch(target_pcls, q0) / q0.size();
        
        num++;
        float loss_now = getLossShapeMatch(q2, q1) / q1.size();
        loss_improve = abs(loss_before - loss_now);  // 前一个变换矩阵和当前变换矩阵的差异小于阈值时，就认为已经收敛了，
        // std::cout<<"itertime: "<<iter_times<<", loss now: "<<loss_now<<", improve: "<<loss_improve<<std::endl;
        loss_before = loss_now;
    }


    std::cout<<"------ICP RESULT-------"<<std::endl;
    std::cout << "num: " << num << std::endl;
    std::cout << "R0: " << std::endl<< R0 << std::endl;
    std::cout << "t0: "<< t0.transpose() <<std::endl;
    // std::cout << "R_12.transpose(): " << std::endl;
    // std::cout << R_12.transpose() << std::endl;
    // std::cout << "t: " << (-R_12.transpose() * t_12).transpose() << std::endl;
    std::cout << "loss_improve: " << loss_improve << std::endl;
    std::cout << "loss_st: " << loss_st <<std::endl;

    // for (size_t i = 0; i < q0.size(); i++)
    // {
    //     std::cout<<"q0 "<<i<<" "<<q0[i].transpose()<<std::endl;
    // }
    
    // for (size_t i = 0; i < target_pcls.size(); i++)
    // {
    //     std::cout<<"target_pcls "<<i<<" "<<target_pcls[i].transpose()<<std::endl;
    // }
    

}