激光slam学习笔记3--轨迹建图经验接口介绍

Posted 2023-03-02 鸿_H

背景：如果给了一条轨迹和轨迹时间戳上的激光点云，那么拼接地图是一个有趣的事情。

概要：先介绍来自liosam里面手动计算的接口，后面介绍一种pcl自带的接口。

1、手动计算的接口

该接口采用手撕方式写的，定制性，运行速度更快些，但通用性不行。

pcl::PointCloud<PointType>::Ptr transformPointCloud(pcl::PointCloud<PointType>::Ptr cloudIn, PointTypePose* transformIn)

pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());

int cloudSize = cloudIn->size();
cloudOut->resize(cloudSize);

Eigen::Affine3f transCur = pcl::getTransformation(transformIn->x, transformIn->y, transformIn->z, transformIn->roll, transformIn->pitch, transformIn->yaw);
// 多线程处理方式
#pragma omp parallel for num_threads(numberOfCores)
for (int i = 0; i < cloudSize; ++i)

const auto &pointFrom = cloudIn->points[i];
cloudOut->points[i].x = transCur(0,0) * pointFrom.x + transCur(0,1) * pointFrom.y + transCur(0,2) * pointFrom.z + transCur(0,3);
cloudOut->points[i].y = transCur(1,0) * pointFrom.x + transCur(1,1) * pointFrom.y + transCur(1,2) * pointFrom.z + transCur(1,3);
cloudOut->points[i].z = transCur(2,0) * pointFrom.x + transCur(2,1) * pointFrom.y + transCur(2,2) * pointFrom.z + transCur(2,3);
cloudOut->points[i].intensity = pointFrom.intensity;

return cloudOut;

2、pcl自带的接口

pcl::PointCloud<pcl::PointXYZI>::Ptr temp;// 单帧点云
pcl::PointCloud<pcl::PointXYZI>::Ptr globalTemp(new pcl::PointCloud<pcl::PointXYZI>());// 拼接后的点云
Eigen::Isometry3d temp_iso = Eigen::Isometry3d::Identity(); //存储轨迹对应的变换矩阵
pcl::transformPointCloud(*temp, *globalTemp, temp_iso.matrix());	// 拼接的接口

注意，对于pcl自带接口，需要转换为xyzi类型的点云进行计算
3、补充点
初学者或者会疑惑，上面说的是轨迹，怎么没看见，反而是一个变换矩阵。在slam里面，轨迹也就是一些带有时间戳的变换矩阵，整体包含时间戳、位置、姿态信息，位置和姿态信息就是变换矩阵

#####################
不积硅步，无以至千里
好记性不如烂笔头
觉得nice，记得点赞收藏