pcl学习

 1、点云读写

pcl官方教程真的比较详细,可惜是英文的,不过耐心看下去,也能看懂。

Reading Point Cloud data from PCD files — Point Cloud Library 0.0 documentation

 读取pcd文件

#include 
#include 
#include 

int main (int argc, char** argv)
{
    
  pcl::PointCloud::Ptr cloud (new pcl::PointCloud);

  if (pcl::io::loadPCDFile ("test_pcd.pcd", *cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
    return (-1);
  }
  
  //Loaded 个数 data points from 文件 with the following fields: x y z
  std::cout << "Loaded "
            << cloud->width * cloud->height   //点的个数
            << " data points from test_pcd.pcd with the following fields: "
            << std::endl;
            
  for (std::size_t i = 0; i < cloud->points.size (); ++i)
    std::cout << "' " << cloud->points[i].x //点云里点的值
                   << " " << cloud->points[i].y
                   << " " << cloud->points[i].z << std::endl;

  return (0);
}

解释

 //创建一个PointCloud Boost共享指针并对其进行初始化。
  //从堆中分配了一个PointCloud对象,把这个对象的指针赋值给了cloud

pcl::PointCloud::Ptr cloud (new pcl::PointCloud);

加载点云;换乘自己的pcd文件路径

pcl::io::loadPCDFile ("test_pcd.pcd", *cloud)

宽*高

cloud->width * cloud->height 

读取点的x坐标

cloud->points[i].x

 Cmake

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(pcd_read)
 
 find_package(PCL 1.8 REQUIRED)
 
 include_directories(${PCL_INCLUDE_DIRS})
 link_directories(${PCL_LIBRARY_DIRS})
 add_definitions(${PCL_DEFINITIONS})

add_executable (pcd_read pcd_read.cpp)
target_link_libraries (pcd_read ${PCL_LIBRARIES})

除了最后两行,上面的那几行都是调用pcl的库。使环境运行时能找到正确的地方。

使我们创建的pcd_read.cpp代码生成pcd_read可执行程序

add_executable (pcd_read pcd_read.cpp)

 生成的可执行程序pcd_read连接的pcl库

target_link_libraries (pcd_read ${PCL_LIBRARIES})

运行

//在cpp当前文件夹下运行;
mkdir build
cd build
cmake ..
make


//运行可执行文件
./pcd_read

结果

pcl学习_第1张图片

 写PCD文件

#include 
#include 
#include 

int main (int argc, char** argv)
{
  pcl::PointCloud cloud;

  // 写入点云数据
  cloud.width    = 5;
  cloud.height   = 1;
  cloud.is_dense = false;
  cloud.points.resize (cloud.width * cloud.height);

  for (std::size_t i = 0; i < cloud.points.size (); ++i)
  {
    cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }

  pcl::io::savePCDFileASCII ("test_pcd.pcd", cloud);//储存在pcd文件中
  std::cerr << "Saved " << cloud.points.size () << " data points to test_pcd.pcd." << std::endl;

  for (std::size_t i = 0; i < cloud.points.size (); ++i)
    std::cerr << "    " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << std::endl;

  return (0);
}

解释

包含PCD/IO操作库和点云结构库

#include 
#include 

点云类型为 pcl::PointXYZ

pcl::PointCloud cloud;

前两个参数好理解,第三个参数表示是否所有数据在点云中都是好的(true)或者是否xyz的值可能存在INF/NAN值(false)

// 写入点云数据
  cloud.width    = 5;
  cloud.height   = 1;
  cloud.is_dense = false;
  cloud.points.resize (cloud.width * cloud.height);

for (std::size_t i = 0; i < cloud.points.size (); ++i)
  {
    cloud.points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud.points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud.points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }

将这些点保存在test_pcd.pcd文件中

pcl::io::savePCDFileASCII ("test_pcd.pcd", cloud);

cmake

只需要在上面的cmake文件中加入这两句即可

add_executable (pcd_write pcd_write.cpp)
target_link_libraries (pcd_write ${PCL_LIBRARIES})

运行方法同上

./pcd_write

结果

pcl学习_第2张图片

2、创建自己的的点云类型

在LIO-SAM中用到的点云类型为PointXYZIRPYT;包括最基本的x,y,z和强度I;以及rol,pitch,yaw三个旋转角和时间T(time);

在PCL的库中没有这样的点云类型,但是PCL给我们提供了创建自己类型点云的方法。

完整代码

#include 
#include 
#include 

struct MyPointType    //定义点类型结构
{
    PCL_ADD_POINT4D;  //该点类型有4个元素      

    /*尝试新增一个自定义*/  
    float test;    //定义自己新增的类型名称

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW   //确保new操作符对齐操作 

}EIGEN_ALIGN16;   //强制SSE 对齐

POINT_CLOUD_REGISTER_POINT_STRUCT(MyPointType,    //注册点类型宏
    (float ,x,x)
    (float ,y,y)
    (float ,z,z)
    (float ,test,test)
    )

int main(int argc, char const *argv[])
{
    pcl::PointCloud  cloud;

    cloud.points.resize(2);

    cloud.width=2;

    cloud.height=1;

    cloud.points[0].x=cloud.points[0].y=cloud.points[0].z=1;

    cloud.points[1].x=cloud.points[1].y=cloud.points[1].z=3;

    cloud.points[0].test = 1.2;

    cloud.points[1].test = 3.4;


    pcl::io::savePCDFile("test.pcd",cloud);

    for (std::size_t i = 0; i < cloud.points.size (); ++i)
    for (std::size_t i = 0; i < cloud.points.size (); ++i)
        std::cerr << "    " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << " " <

解释

定义自己的点云类型其实也比较简单,有固定的套路;

第一步先创建自己类型的结构体

struct MyPointType    //定义点类型结构
{
    PCL_ADD_POINT4D;  //该点类型有4个元素      

    /*尝试新增一个自定义*/  
    float test;    //定义自己新增的类型名称

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW   //确保new操作符对齐操作 

}EIGEN_ALIGN16;   //强制SSE 对齐

 定义的点云类型有4个元素x,y,z+padding(坠语)似乎padding只是一个代称代表所有其他元素,你可以在x,y,z的基础上加一个,两个,三个都行。此处是只加了一个float类型的test;

PCL_ADD_POINT4D;  //该点类型有4个元素

其他强制对齐似乎都要加

第二步注册

POINT_CLOUD_REGISTER_POINT_STRUCT(MyPointType,    //注册点类型宏
    (float ,x,x)
    (float ,y,y)
    (float ,z,z)
    (float ,test,test)
    )

就是固定的格式;x,y,z是基本的类型,test是我们自己加的;MyPointType是新点云类型的名字

使用

int main(int argc, char const *argv[])
{
    //点云定义
    pcl::PointCloud  cloud;
    //确定点云大小
    cloud.points.resize(2);

    cloud.width=2;

    cloud.height=1;
    //为点云中的点的各个元素赋值(x,y,z,test)
    cloud.points[0].x=cloud.points[0].y=cloud.points[0].z=1;

    cloud.points[1].x=cloud.points[1].y=cloud.points[1].z=3;

    cloud.points[0].test = 1.2;

    cloud.points[1].test = 3.4;

    //写入test.pcd
    pcl::io::savePCDFile("test.pcd",cloud);
    //输出测试
    for (std::size_t i = 0; i < cloud.points.size (); ++i)
        std::cerr << "    " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << " " <

cmake

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
 
 project(pcd_read)
 
 find_package(PCL 1.8 REQUIRED)
 
 include_directories(${PCL_INCLUDE_DIRS})
 link_directories(${PCL_LIBRARY_DIRS})
 add_definitions(${PCL_DEFINITIONS})

# add_executable (pcd_read pcd_read.cpp)
# target_link_libraries (pcd_read ${PCL_LIBRARIES})

# add_executable (pcd_write pcd_write.cpp)
# target_link_libraries (pcd_write ${PCL_LIBRARIES})

add_executable (my_point my_point.cpp)
target_link_libraries (my_point ${PCL_LIBRARIES})

运行

//在cpp当前文件夹下运行;
mkdir build
cd build
cmake ..
make
 
 
//运行可执行文件
./my_point

结果

 LIO-SAM中定义的点云类型

定义成立x,y,z三个位置信息;roll,pitch,yaw三个旋转信息;以及time和intensiy强度信息;

并且利用typedef可以使用PointTypePose对该类型点云进行定义

struct PointXYZIRPYT
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;                  // preferred way of adding a XYZ+padding
    float roll;
    float pitch;
    float yaw;
    double time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW   // make sure our new allocators are aligned
} EIGEN_ALIGN16;                    // enforce SSE padding for correct memory alignment

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT,
                                   (float, x, x) (float, y, y)
                                   (float, z, z) (float, intensity, intensity)
                                   (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw)
                                   (double, time, time))

typedef PointXYZIRPYT  PointTypePose;

使用

PointTypePose thisPose6D;
thisPose6D.x = transformIn[3];
thisPose6D.y = transformIn[4];
thisPose6D.z = transformIn[5];
thisPose6D.roll  = transformIn[0];
thisPose6D.pitch = transformIn[1];
thisPose6D.yaw   = transformIn[2];

3、KD-Tree最近邻搜索

 首先要理解KD-tree最近邻搜索的算法原理可以参考这篇博客

K近邻法(KNN)原理小结 - 刘建平Pinard - 博客园

对于使用PCL库进行比较简单直接调用一个函数即可下面先看完整代码

下面利用PCL提供的函数完成一个任务:

给定一个点云test.pcd;找出该点云中某个点(比如说第三个点)的10个最近邻点;并输出他们的坐标以及距离

#include 
#include    //也有这个文件 但是会报错 
#include 
#include 
#include 
#include 
#include 
#include 

int main(int argc, char**argv)
{

    srand (time (NULL));//用系统时间初始化 随机种子
    /*创建KdTreeFLANN 对象*/
    pcl::KdTreeFLANN kdtree;

    //创建点云对象
    pcl::PointCloud::Ptr cloud (new pcl::PointCloud);
    //
   if (pcl::io::loadPCDFile ("/home/du/work/study/pcl/test_pcd.pcd", *cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
    return (-1);
  }

    /*把上面建立的点云 设置为 KDTREE的输入*/
    kdtree.setInputCloud(cloud);

    //创建一个查询点  同样分配随机坐标
    pcl::PointXYZ searchPoint=cloud->points[2];
    
    //创建两个向量 存储搜索到的k紧邻  一个存点的索引 一个存点的距离平方
    int K = 10;
    std::vector pointIdxNKNSearch(K);
    std::vector pointNKNSquareDistance(K);

     //终端输出 相关 信息
    std::cout<<"寻找该点"<points[ pointIdxNKNSearch[i] ].x 
            <<" "<< cloud->points[pointIdxNKNSearch[i] ].y 
            <<" "<< cloud->points[pointIdxNKNSearch[i] ].z 
            <<" (平方距离为: "<

解释

计算某点最近邻主要分为三个步骤类似于把大象装冰箱

首先构造kdtree对象

/*创建KdTreeFLANN 对象*/
    pcl::KdTreeFLANN kdtree;

第二确定搜索点和待搜索的点云集合装到kdtree对象

//创建点云对象
    pcl::PointCloud::Ptr cloud (new pcl::PointCloud);
    //
   if (pcl::io::loadPCDFile ("/home/du/work/study/pcl/test_pcd.pcd", *cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
    return (-1);
  }
//创建一个查询点
pcl::PointXYZ searchPoint=cloud->points[2];
/*把上面建立的点云 设置为 KDTREE的输入*/
    kdtree.setInputCloud(cloud);

第三nearestKSearch函数开始搜索

//searchPoint要搜索的点;K要找几个最近邻;pointIdxNKNSearch近邻点的索引;pointNKNSquareDistance近邻点对应的距离
kdtree.nearestKSearch(searchPoint,K,pointIdxNKNSearch,pointNKNSquareDistance)>0

结果

pcl学习_第3张图片

4、创建RGB点云+用八叉树压缩点云

在学习创建RGB点云时突然想到,似乎可以将点云投影到深度图后,利用语义分割的结果确定了点云属于那个类别,然后提取出范围坐标,将此范围内的点云RGB赋值为特定的颜色从而生成语义地图;--毕设相关与本文内容无关,突然有了想法,防止自己忘了记录一下;

一、创建RGB点云

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
int main (int argc, char** argv)
{
     pcl::PointCloud::Ptr point_cloud_ptr(new pcl::PointCloud());;

     uint8_t r(255), g(15), b(15);
  for (float z(-1.0); z <= 1.0; z += 0.1)
  {
    for (float angle(0.0); angle <= 360.0; angle += 60.0)
    {
      pcl::PointXYZ basic_point;
      basic_point.x = 0.5 * cosf (float(angle/180*M_PI));
      basic_point.y = sinf (float(angle/180*M_PI));
      basic_point.z = z;

      pcl::PointXYZRGB point;
      point.x = basic_point.x;
      point.y = basic_point.y;
      point.z = basic_point.z;
      uint32_t rgb = (static_cast(r) << 16 |
              static_cast(g) << 8 | static_cast(b));
      point.rgb = *reinterpret_cast(&rgb);
      point_cloud_ptr->points.push_back (point);
    }
    if (z < 0.0)
    {
      r -= 12;
      g += 12;
    }
    else
    {
      g -= 12;
      b += 12;
    }
  }
  point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
  point_cloud_ptr->height = 1;   

   pcl::io::savePCDFile("creat_xyzrgb.pcd",*point_cloud_ptr);

   pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
   viewer.showCloud (point_cloud_ptr);
   while (!viewer.wasStopped ())
   {
       
   }
    return 0;
}

解释

 创建基本的xyz点云;xy平面旋转成圆形创建,z方向不断升高

pcl::PointXYZ basic_point;
basic_point.x = 0.5 * cosf (float(angle/180*M_PI));
basic_point.y = sinf (float(angle/180*M_PI));
basic_point.z = z;

 在z<0时颜色都是相同的z颜色变化规律如下

if (z < 0.0)
    {
      r -= 12;
      g += 12;
    }

z>0时变化规律为

else
    {
      g -= 12;
      b += 12;
    }

保存显示点云

pcl::io::savePCDFile("creat_xyzrgb.pcd",*point_cloud_ptr);
pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
viewer.showCloud (point_cloud_ptr);
while (!viewer.wasStopped ())
{
       
}

cmake

add_executable (creat_rgb creat_rgb.cpp)
target_link_libraries (creat_rgb ${PCL_LIBRARIES})

结果

pcl学习_第4张图片

二、点云压缩

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
class SimpleOpenNIViewer
{
    public:
    SimpleOpenNIViewer()
    {
        
    }

    void run(const pcl::PointCloud::ConstPtr &cloud)
    {
            /* 设置压缩 和解压 时 的结果信息是否打印显示出来  */
        bool showStatistics = true ;
        //bool showStatistics = false;

        /* 压缩选项  可以参考 usr/include/pcl/compression/compression_profiles.h   */    
        pcl::io::compression_Profiles_e compressionProfile = pcl::io::MANUAL_CONFIGURATION;//设定为允许为高级参数化进行手工配置

        /*声明八叉树的压缩 变量  指针*/
        pcl::io::OctreePointCloudCompression * PointCloudEncoder;
            /*  进行 八叉树 的 压缩 配置  */
        PointCloudEncoder = new pcl::io::OctreePointCloudCompression(compressionProfile,showStatistics,0.001,0.01,true,100,true,8);//输入参数

            //压缩的字节流
        std::stringstream compressedData;

            /* 实现压缩 的  函数   第一个参数要求时只能指针,  .makeShared()即将cloud转成了 ptr */
        PointCloudEncoder->encodePointCloud(cloud,compressedData);

        //声明 解压 缩 后的 点云 指针
        pcl::PointCloud::Ptr cloudOut(new pcl::PointCloud());

        /* 实现 解压缩 的 地方 第一个参数字节流 第二个参数 点云指针*/
        PointCloudEncoder->decodePointCloud(compressedData,cloudOut);
        pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
        viewer.showCloud (cloudOut);
        while (!viewer.wasStopped ())
        {
            
        }
    }
};
int main (int argc, char** argv)
{
    pcl::PointCloud::Ptr point_cloud_ptr(new pcl::PointCloud());;

    uint8_t r(255), g(15), b(15);
  for (float z(-1.0); z <= 1.0; z += 0.02)
  {
    for (float angle(0.0); angle <= 360.0; angle += 60.0)
    {
      pcl::PointXYZ basic_point;
      basic_point.x = 0.5 * cosf (float(angle/180*M_PI));
      basic_point.y = sinf (float(angle/180*M_PI));
      basic_point.z = z;

      pcl::PointXYZRGB point;
      point.x = basic_point.x;
      point.y = basic_point.y;
      point.z = basic_point.z;
      uint32_t rgb = (static_cast(r) << 16 |
              static_cast(g) << 8 | static_cast(b));
      point.rgb = *reinterpret_cast(&rgb);
      point_cloud_ptr->points.push_back (point);
    }
    if (z < 0.0)
    {
      r -= 12;
      g += 12;
    }
    else
    {
      g -= 12;
      b += 12;
    }
  }
  point_cloud_ptr->width = (int) point_cloud_ptr->points.size ();
  point_cloud_ptr->height = 1;   

   pcl::io::savePCDFile("creat_xyzrgb.pcd",*point_cloud_ptr);

   //pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
//    viewer.showCloud (point_cloud_ptr);
//    while (!viewer.wasStopped ())
//    {
       
//    }


    SimpleOpenNIViewer v;
    v.run(point_cloud_ptr);


    return 0;
}

 创建一个点云压缩的class

通过实例化出对象,调用函数直接运行即可

5、体素滤波网格降采样

 概述

         通过体素网格实现降采样,可以减少点数量的同时,保证点云的形状特征,可以提高配准、曲面重建、形状识别等算法的速度,并保证准确性

原理

        PCL实现的VoxelGrid类通过输入的点云数据创建一系列三维的体素栅格(可以想象为一个个微小的空间三维立方体的集合),然后在每一个体素中(就是每一个三维体素栅格)内,用这个体素栅格内所有点的重心来近似的显示体素中的其他点(用一个点代表栅格内的所有点)。

          经过体素网格处理过的点云的密度将会显著减小,并且能够很好的保留原始的曲面特征,这也就是降采样(或称下采样的原因)。
 

代码

#include 
#include 
#include 
#include 
 
int main ()
 {
   //定义输入点云和滤波后的点云
   pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
   pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());

    // 创建点云读取对象
    pcl::PCDReader reader;
    // 读取点云到cloud
    reader.read ("/home/du/work/study/pcl/table_scene_lms400.pcd", *cloud); 

    std::cerr << "点云滤波前大小: " << cloud->width * cloud->height 
        << " data points (" << pcl::getFieldsList (*cloud) << ")." << std::endl;

    // 创建滤波对象
    pcl::VoxelGrid sor;
    sor.setInputCloud (cloud);
    sor.setLeafSize (0.01f, 0.01f, 0.01f);
    sor.filter (*cloud_filtered);

    std::cerr << "滤波后点云大小: " << cloud_filtered->width * cloud_filtered->height 
        << " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << std::endl;

    pcl::PCDWriter writer;
    //将滤波后的点云写入pcd文件
    writer.write ("/home/du/work/study/pcl/table_scene_lms400_downsampled.pcd", *cloud_filtered, 
            Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);

    //delete[] cloud;
        
    return (0);
}

cmake

add_executable (voxel_grid voxel_grid.cpp)
target_link_libraries (voxel_grid ${PCL_LIBRARIES})

 运行

pcl学习_第5张图片

查看结果

pcl_viewer -multiview 1 table_scene_lms400.pcd table_scene_lms400_downsampled.pcd

pcl学习_第6张图片

 LIO-SAM中的点云降采样滤波

//创建滤波对象 其中PointType为PointXYZI  
pcl::VoxelGrid downSizeFilterCorner;

//将降采样点云输入到滤波对象
downSizeFilterCorner.setInputCloud(laserCloudCornerStack2);
//讲采样输出到laserCloudCornerStack
downSizeFilterCorner.filter(*laserCloudCornerStack);
//查看输出后的大小
int laserCloudCornerStackNum = laserCloudCornerStack->points.size();

6、点云配准

 ICP理论部分可以参考我的另一篇博客

手写ICP-SVD_111111111112454545的博客-CSDN博客

用pcl库调用icp比较简单;

使用上一节降采样的点云

#include 
#include 
#include 
#include 
#include 

 int main ()
{
    pcl::PointCloud::Ptr cloud_in (new pcl::PointCloud);
    pcl::PointCloud::Ptr cloud_out (new pcl::PointCloud);

    pcl::PCDReader reader;
    reader.read ("/home/du/work/study/pcl/table_scene_lms400_downsampled.pcd", *cloud_in); 
     
    *cloud_out = *cloud_in;

    std::cout << "size:" << cloud_out->size() << std::endl;
    for (auto& point : *cloud_out)
        point.x += 0.07f;
    // for(auto& point : *cloud_out)
    // {
    //     point.x=point.x*(-0.2)+0*point.y+0*point.z;
    //     point.y=point.x*0+0*point.y+0*point.z;
    //     point.z=point.x*0+0*point.y+1*point.z;
    // }
        
    std::cout << "Transformed " << cloud_in->size () << " data points:" << std::endl;
        
    // for (auto& point : *cloud_out)
    //     std::cout << point << std::endl;

    pcl::IterativeClosestPoint icp;
    /*有4个参数可以设置 */
    // 设置最大的对应关系距离为 多少 单位m, (对应距离大于此距离将被忽略) 
    icp.setMaxCorrespondenceDistance (0.8);
    // 设置最大迭代次数(默认 1)
    icp.setMaximumIterations (50);
    // 设置 两次变换矩阵之间的差值 (默认 2)
    icp.setTransformationEpsilon (1e-8);
    // 设置 均方误差(默认 3)
    icp.setEuclideanFitnessEpsilon (1);

    
    icp.setInputSource(cloud_in);
    icp.setInputTarget(cloud_out);
    
    pcl::PointCloud Final;
    icp.align(Final);

    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
    icp.getFitnessScore() << std::endl;
    std::cout << icp.getFinalTransformation() << std::endl;

    return (0);
}

利用pcl使用icp有以下几步

第一:确定原始点云与目标点云

pcl::PointCloud::Ptr cloud_in (new pcl::PointCloud);
pcl::PointCloud::Ptr cloud_out (new pcl::PointCloud);

第二:构造icp对象

pcl::IterativeClosestPoint icp;

第三:配置icp参数(默认也可以)

/*有4个参数可以设置 */
    // 设置最大的对应关系距离为 多少 单位m, (对应距离大于此距离将被忽略) 
    icp.setMaxCorrespondenceDistance (0.8);
    // 设置最大迭代次数(默认 1)
    icp.setMaximumIterations (50);
    // 设置 两次变换矩阵之间的差值 (默认 2)
    icp.setTransformationEpsilon (1e-8);
    // 设置 均方误差(默认 3)
    icp.setEuclideanFitnessEpsilon (1);

    
    icp.setInputSource(cloud_in);
    icp.setInputTarget(cloud_out);

第四查看结果

std::cout << "has converged:" << icp.hasConverged() << " score: " <<
    icp.getFitnessScore() << std::endl;
    std::cout << icp.getFinalTransformation() << std::endl;

结果

pcl学习_第7张图片

你可能感兴趣的:(pcl,学习,slam,c++)