基于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);
}
结果如下图:
拼接融合效果良好!!!