y空间兑换代码_loam代码解析3

本章记录loam的第三部分地图构建，仍然从main函数看起。

首先订阅/laser_cloud_corner_last和/laser_cloud_surf_last特征点云，订阅特征点云对应的全局位姿/laser_odom_to_init。然后订阅第二部分laserOdometry处理后的实时点云/velodyne_cloud_3，订阅/imu/data用于读取imu姿态角。最后发布/laser_cloud_surround和/velodyne_cloud_registered和/aft_mapped_to_init。

点云回调函数将点云rosmsgs格式转换为pcl格式，全局位姿回调将位姿信息保存在transformSum中，imu回调函数保存时间戳和roll/pitch信息。

建图环节将整个空间三维区域划分为laserCloudNum个子cube，从while循环开始看。

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;
    frameCount++;    

while循环首先进行一次回调，并确保时间一致性，然后重置标志位，帧数量+1。

后续涉及到transformAssociateToMap()函数，先分析。该函数首先将每次地图匹配前的位姿transformBefMapped与订阅的初始位姿transformSum之间的平移偏差按照yxz顺序旋转到transformSum坐标系方向，得到transformIncre。然后和第二部分PluginIMURotation()函数一样，通过地图匹配前后的姿态transformBefMapped/transformAftMapped以及初始姿态transformSum计算得到地图匹配时transformTobeMapped的姿态。计算原理如下图。

最后将transformIncre位置偏差转换至transformTobeMapped坐标系方向，由transformAftMapped位置信息减去转换后的偏差得到transformTobeMapped位置信息。

下面回到main函数中：

/*坐标变换*/
if (frameCount >= stackFrameNum) {
  transformAssociateToMap();   //地图匹配坐标转换
  int laserCloudCornerLastNum = laserCloudCornerLast->points.size(); 
  for (int i = 0; i < laserCloudCornerLastNum; i++) {
    pointAssociateToMap(&laserCloudCornerLast->points[i], &pointSel);
    laserCloudCornerStack2->push_back(pointSel);
  }
  int laserCloudSurfLastNum = laserCloudSurfLast->points.size();  //面特征点
  for (int i = 0; i < laserCloudSurfLastNum; i++) {
    pointAssociateToMap(&laserCloudSurfLast->points[i], &pointSel);
    laserCloudSurfStack2->push_back(pointSel);
  }
}

上述代码首先调用transformAssociateToMap()函数进行地图匹配相关位姿信息变换，接着针对边特征点和面特征点调用pointAssociateToMap()函数，将pi坐标点通过transformTobeMapped转换到全局坐标系，将处理完的特征点保存到点云。

/*优化处理  找当前估计的lidar位姿属于哪个cube，I/J/K对应cube的索引*/
if (frameCount >= stackFrameNum) {
  frameCount = 0;
  //当前lidar坐标系{L}y轴一点(0,10,0)
  PointType pointOnYAxis;  
  pointOnYAxis.x = 0.0;
  pointOnYAxis.y = 10.0;
  pointOnYAxis.z = 0.0;
  pointAssociateToMap(&pointOnYAxis, &pointOnYAxis); //转换到全局坐标系
  //cube中心位置索引
  int centerCubeI = int((transformTobeMapped[3] + 25.0) / 50.0) + laserCloudCenWidth;     //[-25,25)->10  [25,75)->11 
  int centerCubeJ = int((transformTobeMapped[4] + 25.0) / 50.0) + laserCloudCenHeight;    //[-25,25)->5  [25,75)->6 
  int centerCubeK = int((transformTobeMapped[5] + 25.0) / 50.0) + laserCloudCenDepth;     //[-25,25)->10  [25,75)->11 
  if (transformTobeMapped[3] + 25.0 < 0) centerCubeI--;
  if (transformTobeMapped[4] + 25.0 < 0) centerCubeJ--;
  if (transformTobeMapped[5] + 25.0 < 0) centerCubeK--; 

上述代码首先在当前帧坐标系中定义一个坐标点pointOnYxis，并转换到全局坐标系。接着计算当前帧位姿所在的cube索引，如果是在负区间则索引需要减1。

//如果当前帧lidar位姿对应的cube在整个大cube边缘则将索引向中心方向挪动一个单位
while (centerCubeI < 3) {   //width方向的小端   将帧cube指针向中心方向平移
  for (int j = 0; j < laserCloudHeight; j++) {
    for (int k = 0; k < laserCloudDepth; k++) {
      int i = laserCloudWidth - 1