源码解析6 ： laserMapping（一）"/>
loam源码解析6 ： laserMapping（一）
transformMaintenance.cpp解析
- 一、概述
- 二、变量说明
- 三、预处理
- 1. 接受来自laserOdometry的ROS消息
- (1) laserCloudCornerLastHandler
- (2) laserCloudSurfLastHandler
- (3) laserCloudFullResHandler
- (4) laserOdometryHandler
- 2. 定义消息发布
- 3. 数据初始化
- 四、transformAssociateToMap初始化位姿 T ˉ k W ( t k + 1 ) \bar T_{k}^W(t_{k+1}) TˉkW(tk+1)
- 1. 计算位移增量
- 2. 修正旋转增量
- 3. 修正平移增量
- 五、粗略拼接地图 Q ˉ k \bar Q_k Qˉk
- 1. 计算粗略的 k + 1 k+1 k+1时刻世界坐标系下的位姿为 T ˉ k W ( t k + 1 ) \bar T_{k}^W(t_{k+1}) TˉkW(tk+1)：
- 2. transformAssociateToMap位姿变换
- 3. 利用粗略的 k + 1 k+1 k+1时刻地图坐标系下的位姿将点云转换到地图坐标系下
loam源码地址： .
论文学习： LOAM: Lidar Odometry and Mapping in Real-time 论文阅读.
loam源码解析汇总：
loam源码解析1 ： scanRegistration（一）.
loam源码解析2 ： scanRegistration（二）.
loam源码解析3 ： laserOdometry（一）.
loam源码解析4 ： laserOdometry（二）.
loam源码解析5 ： laserOdometry（三）.
loam源码解析6 ： laserMapping（一）.
loam源码解析7 ： laserMapping（二）.
loam源码解析8 ： transformMaintenance.
一、概述
到这里，我们已经分析laserOdometry中lidar里程计的工作，也就完成了loam两大算法的其中一个。然而，上一部分Lidar里程计的结果不准确，由它拼起来的点云完全不成样子，而且Lidar里程计会不断发散，因此误差也会越来越大。所以这一节主要是分析loam的另一大算法：建图算法（laserOdometry），通过将多帧的激光特征点云基于laserOdometry提供的pose拼接,形成特征点云地图。（包含corner和surface两种特征点云地图），将新入的帧与地图作配准,得到更精准的pose.相当于scan-to-map，我们又将这个pose将其加入到全局地图中。
二、变量说明
扫描周期：
const float scanPeriod = 0.1;
处理周期：
//控制接收到的点云数据，每隔几帧处理一次
const int stackFrameNum = 1;
//控制处理得到的点云map，每隔几次publich给rviz显示
const int mapFrameNum = 5;
接受全部点云、比较大曲率点、比较小曲率点、里程计的时间戳：
double timeLaserCloudCornerLast = 0;
double timeLaserCloudSurfLast = 0;
double timeLaserCloudFullRes = 0;
double timeLaserOdometry = 0;
接受全部点云、比较大曲率点、比较小曲率点、里程计的标志：
//接收标志
bool newLaserCloudCornerLast = false;
bool newLaserCloudSurfLast = false;
bool newLaserCloudFullRes = false;
bool newLaserOdometry = false;
cube的宽、高、深度信息，以及点云的分割cube个数
int laserCloudCenWidth = 10;
int laserCloudCenHeight = 5;
int laserCloudCenDepth = 10;
const int laserCloudWidth = 21;
const int laserCloudHeight = 11;
const int laserCloudDepth = 21;
//点云方块集合最大数量
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;//4851
点云集索引：
//lidar视域范围内(FOV)的点云集索引
int laserCloudValidInd[125];
//lidar周围的点云集索引
int laserCloudSurroundInd[125];
存放各种点云的定义：
//最新接收到的边沿点
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
//最新接收到的平面点
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());
//存放当前收到的下采样之后的边沿点(in the local frame)
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
//存放当前收到的下采样之后的平面点(in the local frame)
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
//存放当前收到的边沿点，作为下采样的数据源
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack2(new pcl::PointCloud<PointType>());
//存放当前收到的平面点，作为下采样的数据源
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack2(new pcl::PointCloud<PointType>());
//原始点云坐标
pcl::PointCloud<PointType>::Ptr laserCloudOri(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr coeffSel(new pcl::PointCloud<PointType>());
//匹配使用的特征点（下采样之后的）
pcl::PointCloud<PointType>::Ptr laserCloudSurround(new pcl::PointCloud<PointType>());
//匹配使用的特征点（下采样之前的）
pcl::PointCloud<PointType>::Ptr laserCloudSurround2(new pcl::PointCloud<PointType>());
//map中提取的匹配使用的边沿点
pcl::PointCloud<PointType>::Ptr laserCloudCornerFromMap(new pcl::PointCloud<PointType>());
//map中提取的匹配使用的平面点
pcl::PointCloud<PointType>::Ptr laserCloudSurfFromMap(new pcl::PointCloud<PointType>());
//点云全部点
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());
//array都是以50米为单位的立方体地图，运行过程中会一直保存(有需要的话可考虑优化，只保存近邻的，或者直接数组开小一点)
//存放边沿点的cube
pcl::PointCloud<PointType>::Ptr laserCloudCornerArray[laserCloudNum];
//存放平面点的cube
pcl::PointCloud<PointType>::Ptr laserCloudSurfArray[laserCloudNum];
//中间变量，存放下采样过的边沿点
pcl::PointCloud<PointType>::Ptr laserCloudCornerArray2[laserCloudNum];
//中间变量，存放下采样过的平面点
pcl::PointCloud<PointType>::Ptr laserCloudSurfArray2[laserCloudNum];//kd-tree
pcl::KdTreeFLANN<PointType>::Ptr kdtreeCornerFromMap(new pcl::KdTreeFLANN<PointType>());
pcl::KdTreeFLANN<PointType>::Ptr kdtreeSurfFromMap(new pcl::KdTreeFLANN<PointType>());
变换矩阵：
/*************高频转换量**************/
//odometry计算得到的到世界坐标系下的转移矩阵
float transformSum[6] = {0};
//转移增量，只使用了后三个平移增量
float transformIncre[6] = {0};/*************低频转换量*************/
//以起始位置为原点的世界坐标系下的转换矩阵（猜测与调整的对象）
float transformTobeMapped[6] = {0};
//存放mapping之前的Odometry计算的世界坐标系的转换矩阵（注：低频量，不一定与transformSum一样）
float transformBefMapped[6] = {0};
//存放mapping之后的经过mapping微调之后的转换矩阵
float transformAftMapped[6] = {0};
imu变量：
int imuPointerFront = 0;
int imuPointerLast = -1;
const int imuQueLength = 200;double imuTime[imuQueLength] = {0};
float imuRoll[imuQueLength] = {0};
float imuPitch[imuQueLength] = {0};
三、预处理
1. 接受来自laserOdometry的ROS消息
在mian函数的一开始，就接受来自laserOdometry的消息并作相应的解析：
int main(int argc, char** argv)
{ros::init(argc, argv, "laserMapping");ros::NodeHandle nh;ros::Subscriber subLaserCloudCornerLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2, laserCloudCornerLastHandler);ros::Subscriber subLaserCloudSurfLast = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2, laserCloudSurfLastHandler);ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> ("/laser_odom_to_init", 5, laserOdometryHandler);ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2> ("/velodyne_cloud_3", 2, laserCloudFullResHandler);ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);
接下来我们就分别查看每个订阅的消息的回调函数。可以看出来，其实每个函数都相对类似，主要是将ROS的消息格式转换。
(1) laserCloudCornerLastHandler
接受边缘点（曲率较大的点）的回调函数
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudCornerLast2)
{timeLaserCloudCornerLast = laserCloudCornerLast2->header.stamp.toSec();laserCloudCornerLast->clear();pcl::fromROSMsg(*laserCloudCornerLast2, *laserCloudCornerLast);newLaserCloudCornerLast = true;
}(2) laserCloudSurfLastHandler
接受平面点（曲率较小的点）的回调函数
void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudSurfLast2)
{timeLaserCloudSurfLast = laserCloudSurfLast2->header.stamp.toSec();laserCloudSurfLast->clear();pcl::fromROSMsg(*laserCloudSurfLast2, *laserCloudSurfLast);newLaserCloudSurfLast = true;
}
(3) laserCloudFullResHandler
接受全部点云的回调函数
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudFullRes2)
{timeLaserCloudFullRes = laserCloudFullRes2->header.stamp.toSec();laserCloudFullRes->clear();pcl::fromROSMsg(*laserCloudFullRes2, *laserCloudFullRes);newLaserCloudFullRes = true;
}
(4) laserOdometryHandler
接受里程计信息（包括旋转和平移）的回调函数
//接收旋转平移信息
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{timeLaserOdometry = laserOdometry->header.stamp.toSec();double roll, pitch, yaw;//四元数转换为欧拉角geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);transformSum[0] = -pitch;transformSum[1] = -yaw;transformSum[2] = roll;transformSum[3] = laserOdometry->pose.pose.position.x;transformSum[4] = laserOdometry->pose.pose.position.y;transformSum[5] = laserOdometry->pose.pose.position.z;newLaserOdometry = true;
}
2. 定义消息发布
ROS消息发布：
ros::Publisher pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2> ("/laser_cloud_surround", 1);ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_cloud_registered", 2);ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init", 5);nav_msgs::Odometry odomAftMapped;odomAftMapped.header.frame_id = "/camera_init";odomAftMapped.child_frame_id = "/aft_mapped";
tf形式发布：
tf::TransformBroadcaster tfBroadcaster;tf::StampedTransform aftMappedTrans;aftMappedTrans.frame_id_ = "/camera_init";aftMappedTrans.child_frame_id_ = "/aft_mapped";
3. 数据初始化
定义特征点及特征点索引vector：
std::vector<int> pointSearchInd;std::vector<float> pointSearchSqDis;PointType pointOri, pointSel, pointProj, coeff;
定义优化矩阵：
cv::Mat matA0(5, 3, CV_32F, cv::Scalar::all(0));cv::Mat matB0(5, 1, CV_32F, cv::Scalar::all(-1));cv::Mat matX0(3, 1, CV_32F, cv::Scalar::all(0));cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));
定义退化预测矩阵：
bool isDegenerate = false;cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));
建VoxelGrid滤波器，确定体素大小：
//创建VoxelGrid滤波器（体素栅格滤波器）pcl::VoxelGrid<PointType> downSizeFilterCorner;//设置体素大小downSizeFilterCorner.setLeafSize(0.2, 0.2, 0.2);pcl::VoxelGrid<PointType> downSizeFilterSurf;downSizeFilterSurf.setLeafSize(0.4, 0.4, 0.4);pcl::VoxelGrid<PointType> downSizeFilterMap;downSizeFilterMap.setLeafSize(0.6, 0.6, 0.6);
初始化cube：
//指针初始化for (int i = 0; i < laserCloudNum; i++) {laserCloudCornerArray[i].reset(new pcl::PointCloud<PointType>());laserCloudSurfArray[i].reset(new pcl::PointCloud<PointType>());laserCloudCornerArray2[i].reset(new pcl::PointCloud<PointType>());laserCloudSurfArray2[i].reset(new pcl::PointCloud<PointType>());}
调帧频率控制：
int frameCount = stackFrameNum - 1; //0int mapFrameCount = mapFrameNum - 1; //4
执行周期：
ros::Rate rate(100);bool status = ros::ok();
确保同步额的收到每帧信息：
while (status) {ros::spinOnce();if (newLaserCloudCornerLast && newLaserCloudSurfLast && newLaserCloudFullRes && newLaserOdometry &&fabs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&fabs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&fabs(timeLaserCloudFullRes - timeLaserOdometry) < 0.005) {newLaserCloudCornerLast = false;newLaserCloudSurfLast = false;newLaserCloudFullRes = false;newLaserOdometry = false;
四、transformAssociateToMap初始化位姿 T ˉ k W ( t k + 1 ) \bar T_{k}^W(t_{k+1}) TˉkW(tk+1)
基于匀速模型，根据上次微调的结果 T m a p ( t k ) T_{map}(t_{k}) Tmap(tk)和odometry这次与上次计算的结果 T c u r W ( t k + 1 ) T_{{cur}}^W(t_{k+1}) TcurW(tk+1)、 T c u r W ( t k ) T_{{cur}}^W(t_{k}) TcurW(tk)，猜测一个新的世界坐标系的转换矩阵 T ˉ k W ( t k + 1 ) \bar T_{k}^W(t_{k+1}) TˉkW(tk+1)
1. 计算位移增量
transformBefMapped中储存的是上一时刻odometry解算的值，计算上一时刻和这时刻odometry的增量，由于两个时刻的位姿都是基于起始时刻的，所以需要投影到当前时刻。即 δ T c u r ( t k + 1 ) = ( T c u r W ( t k + 1 ) ) − 1 ∗ ( T c u r W ( t k + 1 ) − T c u r W ( t k ) ) \delta T_{cur}(t_{k+1})=(T^W_{cur}(t_{k+1}))^{-1}*(T_{{cur}}^W(t_{k+1})-T_{{cur}}^W(t_{k})) δTcur(tk+1)=(TcurW(tk+1))−1∗(TcurW(tk+1)−TcurW(tk))。
void transformAssociateToMap()
{float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) - sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);float y1 = transformBefMapped[4] - transformSum[4];float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3]) + cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);float x2 = x1;float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;//平移增量transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;transformIncre[5] = z2;
2. 修正旋转增量
其原理与loam源码解析3 ： laserOdometry（一）中修正当前点欧拉角PluginIMURotation的方法类似，其公式为： δ R m a p ( t k + 1 ) = R m a p ( t k ) ∗ ( R c u r m a p ( t k ) ) − 1 ∗ δ R c u r ( t k + 1 ) \delta R_{map}(t_{k+1})=R_{map}(t_{k})*(R_{cur}^{map}(t_{k}))^{-1}*\delta R_{cur}(t_{k+1}) δRmap(tk+1)=Rmap(tk)∗(Rcurmap(tk))−1∗δRcur(tk+1)。
float sbcx = sin(transformSum[0]);float cbcx = cos(transformSum[0]);float sbcy = sin(transformSum[1]);float cbcy = cos(transformSum[1]);float sbcz = sin(transformSum[2]);float cbcz = cos(transformSum[2]);float sblx = sin(transformBefMapped[0]);float cblx = cos(transformBefMapped[0]);float sbly = sin(transformBefMapped[1]);float cbly = cos(transformBefMapped[1]);float sblz = sin(transformBefMapped[2]);float cblz = cos(transformBefMapped[2]);float salx = sin(transformAftMapped[0]);float calx = cos(transformAftMapped[0]);float saly = sin(transformAftMapped[1]);float caly = cos(transformAftMapped[1]);float salz = sin(transformAftMapped[2]);float calz = cos(transformAftMapped[2]);float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz)- cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)- cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz) - calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx);transformTobeMapped[0] = -asin(srx);float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly)- cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx)- cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz)+ (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly)+ cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz)+ (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly);float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz)- cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx)+ cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)+ (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly)- cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly)+ (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly);transformTobeMapped[1] = atan2(srycrx / cos(transformTobeMapped[0]), crycrx / cos(transformTobeMapped[0]));float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)- (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)+ cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)- (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)+ cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);transformTobeMapped[2] = atan2(srzcrx / cos(transformTobeMapped[0]), crzcrx / cos(transformTobeMapped[0]));
3. 修正平移增量
利用刚刚得到的 δ R m a p ( t k + 1 ) \delta R_{map}(t_{k+1}) δRmap(tk+1)修正平移增量:
x1 = cos(transformTobeMapped[2]) * transformIncre[3] - sin(transformTobeMapped[2]) * transformIncre[4];y1 = sin(transformTobeMapped[2]) * transformIncre[3] + cos(transformTobeMapped[2]) * transformIncre[4];z1 = transformIncre[5];x2 = x1;y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;transformTobeMapped[3] = transformAftMapped[3] - (cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2);transformTobeMapped[4] = transformAftMapped[4] - y2;transformTobeMapped[5] = transformAftMapped[5] - (-sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2);
}
五、粗略拼接地图 Q ˉ k \bar Q_k Qˉk
根据论文分析，我们假设 k k k时刻世界坐标系下的位姿为 T k − 1 W ( t k ) T_{k-1}^W(t_{k}) Tk−1W(tk)，通过里程计的信息我们可以得到在 [ t k , t k + 1 ] [t_k,t_{k+1}] [tk,tk+1]内，相对运动为 T k L ( t k + 1 ) T^L_k(t_{k+1}) TkL(tk+1)，点云为 P ˉ k \bar P_k Pˉk,所以我们可以得到一个粗略的 k + 1 k+1 k+1时刻世界坐标系下的位姿为 T ˉ k W ( t k + 1 ) \bar T_{k}^W(t_{k+1}) TˉkW(tk+1)，并将 P ˉ k \bar P_k Pˉk转换到世界坐标系下，得到 Q ˉ k \bar Q_k Qˉk。具体实现如下：
1. 计算粗略的 k + 1 k+1 k+1时刻世界坐标系下的位姿为 T ˉ k W ( t k + 1 ) \bar T_{k}^W(t_{k+1}) TˉkW(tk+1)：
frameCount++;//控制跳帧数，>=这里实际并没有跳帧，只取>或者增大stackFrameNum才能实现相应的跳帧处理if (frameCount >= stackFrameNum) {//获取世界坐标系转换矩阵transformAssociateToMap();
2. transformAssociateToMap位姿变换
这个函数的实现是为下一小节的转换作准备，利用得到的 T ˉ k W ( t k + 1 ) \bar T_{k}^W(t_{k+1}) TˉkW(tk+1)将点云转换到地图坐标下。
oid pointAssociateToMap(PointType const * const pi, PointType * const po)
{//绕z轴旋转（transformTobeMapped[2]）float x1 = cos(transformTobeMapped[2]) * pi->x- sin(transformTobeMapped[2]) * pi->y;float y1 = sin(transformTobeMapped[2]) * pi->x+ cos(transformTobeMapped[2]) * pi->y;float z1 = pi->z;//绕x轴旋转（transformTobeMapped[0]）float x2 = x1;float y2 = cos(transformTobeMapped[0]) * y1 - sin(transformTobeMapped[0]) * z1;float z2 = sin(transformTobeMapped[0]) * y1 + cos(transformTobeMapped[0]) * z1;//绕y轴旋转（transformTobeMapped[1]），再平移po->x = cos(transformTobeMapped[1]) * x2 + sin(transformTobeMapped[1]) * z2+ transformTobeMapped[3];po->y = y2 + transformTobeMapped[4];po->z = -sin(transformTobeMapped[1]) * x2 + cos(transformTobeMapped[1]) * z2+ transformTobeMapped[5];po->intensity = pi->intensity;
}
3. 利用粗略的 k + 1 k+1 k+1时刻地图坐标系下的位姿将点云转换到地图坐标系下
利用粗略的 k + 1 k+1 k+1时刻世界坐标系下的位姿将边缘点转换到世界坐标系下：
//将最新接收到的平面点和边沿点进行旋转平移转换到世界坐标系下(这里和后面的逆转换应无必要)int laserCloudCornerLastNum = laserCloudCornerLast->points.size();for (int i = 0; i < laserCloudCornerLastNum; i++) {pointAssociateToMap(&laserCloudCornerLast->points[i], &pointSel);laserCloudCornerStack2->push_back(pointSel);}
利用粗略的 k + 1 k+1 k+1时刻世界坐标系下的位姿将平面点转换到世界坐标系下：
int laserCloudSurfLastNum = laserCloudSurfLast->points.size();for (int i = 0; i < laserCloudSurfLastNum; i++) {pointAssociateToMap(&laserCloudSurfLast->points[i], &pointSel);laserCloudSurfStack2->push_back(pointSel);}}
更多推荐
loam源码解析6 ： laserMapping（一）
发布评论