基于ICP的点云融合拼接

基于ICP的点云融合拼接

实际应用中基于单个雷达扫描的局限性（遮挡，特征信息不全问题），我们需要将不同坐标系下的点云进行拼接融合，以最大程度上还原真实场景

一、ICP

​ ICP即迭代最近点算法，通过不断调整，把不同角度的3D点数据整合到一个完整的模型中。它的目的在于在一个全局坐标系下找到不同视角的定位与定向，两个视角交叉部分重叠完好为最优，给定输入数据集，首先做一个估计，然后通过旋转和平移变换一个数据集，找到一个正确的点集对应方式完美匹配

二、PCD文件读取与可视化

数据集源自于通过RSview保存的两个不同坐标系下的Pcap文件，通过RSlidar雷达驱动编译运行转化为了Pcd文件，其可视化如下图

​ 测顶雷达

​ 测边雷达

三、滤除异常点

数据显示如下：

由上图可知，PCD点云数据中存在nan异常点，对于融合拼接效果有影响，因此我们需要去除异常点，执行以下操作

#include <pcl/filters/filter.h>//过滤头文件
std::vector<int> index;
pcl::removeNaNFromPointCloud(*cloud_in, *cloud_out, index);//过滤后的新点云储存在cloud_out

由上图可知，异常点过滤成功！！！

四、点云拼接融合

#include <string>
#include <vector>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/filter.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
using namespace std;
using namespace pcl;
typedef pcl::PointXYZ PointType;
  //创建了一个名为cloud的指针，储存XYZ类型的点云数据
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
   PointType pointSel;
class PcdRead {
public:
    void readPcdFile(const std::string filePath);
};
void PcdRead::readPcdFile(const string filePath) {
   
    /*打开点云文件*/
    if (io::loadPCDFile<PointXYZ>(filePath, *cloud) == -1) {
        PCL_ERROR("Couldn't read file xxx.pcd\n");
      
    }
}
void point1To2(PointType const * const p1, PointType * const p2)
{
  p2->x = p1->x * 0.394694 + p1->y * 0.909508 + p1->z * 0.130444;
  p2->y = p1->x * -0.916368 + p1->y * 0.379305 + p1->z * 0.128056;
  p2->z = p1->x * 0.06699 + p1->y * -0.170077 + p1->z * 0.983152;
}
int main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
  std::vector<int> index;
  // Fill in the CloudIn data
  PcdRead read;
  read.readPcdFile("/home/ubuntu/pcl_test/B1.pcd");
  pcl::removeNaNFromPointCloud(*cloud, *cloud, index);
  *cloud_in = *cloud;
  std::cout << "Saved " << cloud_in->size () << " data points from B1.pcd:" << std::endl;
  read.readPcdFile("/home/ubuntu/pcl_test/H1.pcd");
  pcl::removeNaNFromPointCloud(*cloud, *cloud, index);
  *cloud_out = *cloud;
  std::cout << "Saved " << cloud_in->size () << " data points from H1.pcd:" << std::endl;

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputSource(cloud_in);
  icp.setInputTarget(cloud_out);
  
  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;
  for (int i = 0; i <  cloud_in->points.size(); i++) {
      point1To2(&cloud_in->points[i], &pointSel);  //雷达1数据转换到雷达2坐标系中
	    cloud_out -> push_back(pointSel);
    }
  std::cout << "雷达1数据转换到雷达2坐标系中" << std::endl;
   /*点云pcd文件可视化*/
    visualization::PCLVisualizer::Ptr viewer (new visualization::PCLVisualizer ("3D Viewer"));
    viewer->addPointCloud (cloud_out, "rabbit-cloud");
    viewer->spin();
 return (0);
}

结果如下图：

拼接融合效果良好！！！