LOAM_velodyne学习（三）

2023-10-30

终于到第三个模块了，我们先来回顾下之前的工作：点云数据进来后，经过前两个节点的处理可以完成一个完整但粗糙的里程计，可以概略地估计出Lidar的相对运动。如果不受任何测量噪声的影响，这个运动估计的结果足够精确，没有任何漂移，那我们可以直接利用估计的Lidar位姿和对应时刻的量测值完成建图。但这就如同现实中不存在一个不受外力就能匀速直线运动的小球一样，量测噪声是不可避免的，因此Lidar位姿估计偏差一定存在。

Lidar里程计的结果不准确，拼起来的点也完全不成样子，且它会不断发散，因此误差也会越来越大。我们对特征的提取仅仅只是关注了他们的曲率，且点云中的点是离散的，无法保证上一帧的点在下一帧中仍会被扫到。因此，我们需要依靠别的方式去优化Lidar里程计的位姿估计精度。在SLAM领域，一般会采用与地图匹配的方式来优化这一结果。我们始终认为后一时刻的观测较前一时刻带有更多的误差，换而言之，我们更加信任前一时刻结果。因此我们对已经构建地图的信任程度远高于临帧点云配准后的Lidar运动估计。所以我们可以利用已构建地图对位姿估计结果进行修正。

LaserMapping

这一模块的功能：优化Lidar的位姿，在此基础上完成低频的环境建图

依旧从主函数开始

int main(int argc, char** argv)
{
  ros::init(argc, argv, "laserMapping");
  ros::NodeHandle nh;

************订阅5个节点************
  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);

*************发布3个节点*************
  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::TransformBroadcaster tfBroadcaster;
  tf::StampedTransform aftMappedTrans;
  aftMappedTrans.frame_id_ = "/camera_init";
  aftMappedTrans.child_frame_id_ = "/aft_mapped";

在订阅器订阅到了laserOdometry发布的消息后即可开始进行处理。

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让优化处理的部分与laserodometry的发布速度一致？
      frameCount++;
      if (frameCount >= stackFrameNum) {
        // 将坐标转移到世界坐标系下->得到可用于建图的Lidar坐标，即修改了transformTobeMapped的值
        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);
        }
      }

接下来就是较为复杂的优化处理部分,我们先看看论文怎么说的

To find correspondences for the feature points, we store the point cloud on the map, $Q_{k-1}$ , in 10m cubic areas. The points in the cubes that intersect with $\bar{Q}_{k}$ are extracted and stored in a 3D KD-tree in {W}. We find the points in $Q_{k-1}$ within a certain region (10cm × 10cm × 10cm) around the feature points.

就是说：将地图 $Q_{k-1}$ 保存在一个10m的立方体中，若cube中的点与当前帧中的点云 $\bar{Q}_{k}$ 有重叠部分就把他们提取出来保存在KD树中。我们找地图 $Q_{k-1}$ 中的点时，要在特征点附近宽为10cm的立方体邻域内搜索


      if (frameCount >= stackFrameNum) {
        frameCount = 0;
        
        //pointOnYAxis应该是lidar中心在当前坐标系下的位置
        PointType pointOnYAxis;
        pointOnYAxis.x = 0.0;
        pointOnYAxis.y = 10.0;
        pointOnYAxis.z = 0.0;
        //变换到世界坐标系下
        pointAssociateToMap(&pointOnYAxis, &pointOnYAxis);

        //transformTobeMapped记录的是lidar的位姿，在transformAssociateToMap（）函数中进行了更新
        //下面计算的i,j，k是一种索引，指明当前收到的点云所在的cube的（中心？）位置
        int centerCubeI = int((transformTobeMapped[3] + 25.0) / 50.0) + laserCloudCenWidth;
        int centerCubeJ = int((transformTobeMapped[4] + 25.0) / 50.0) + laserCloudCenHeight;
        int centerCubeK = int((transformTobeMapped[5] + 25.0) / 50.0) + laserCloudCenDepth;

        if (transformTobeMapped[3] + 25.0 < 0) centerCubeI--;
        if (transformTobeMapped[4] + 25.0 < 0) centerCubeJ--;
        if (transformTobeMapped[5] + 25.0 < 0) centerCubeK--;

接下来对做一些调整，确保位姿在cube中的相对位置（centerCubeI，centerCubeJ，centerCubeK）能够有一个5*5*5的邻域。

        while (centerCubeI < 3) {
          for (int j = 0; j < laserCloudHeight; j++) {
            for (int k = 0; k < laserCloudDepth; k++) {
              int i = laserCloudWidth - 1;
              pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              for (; i >= 1; i--) {
                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudCornerArray[i - 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k];
                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              }
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeCornerPointer;
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeSurfPointer;
              laserCloudCubeCornerPointer->clear();
              laserCloudCubeSurfPointer->clear();
            }
          }

          centerCubeI++;
          laserCloudCenWidth++;
        }

        while (centerCubeI >= laserCloudWidth - 3) {
          for (int j = 0; j < laserCloudHeight; j++) {
            for (int k = 0; k < laserCloudDepth; k++) {
              int i = 0;
              pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              for (; i < laserCloudWidth - 1; i++) {
                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudCornerArray[i + 1 + laserCloudWidth*j + laserCloudWidth * laserCloudHeight * k];
                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudSurfArray[i + 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              }
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeCornerPointer;
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeSurfPointer;
              laserCloudCubeCornerPointer->clear();
              laserCloudCubeSurfPointer->clear();
            }
          }

          centerCubeI--;
          laserCloudCenWidth--;
        }

        while (centerCubeJ < 3) {
          for (int i = 0; i < laserCloudWidth; i++) {
            for (int k = 0; k < laserCloudDepth; k++) {
              int j = laserCloudHeight - 1;
              pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              for (; j >= 1; j--) {
                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudCornerArray[i + laserCloudWidth*(j - 1) + laserCloudWidth * laserCloudHeight*k];
                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudSurfArray[i + laserCloudWidth * (j - 1) + laserCloudWidth * laserCloudHeight*k];
              }
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeCornerPointer;
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeSurfPointer;
              laserCloudCubeCornerPointer->clear();
              laserCloudCubeSurfPointer->clear();
            }
          }
 
          centerCubeJ++;
          laserCloudCenHeight++;
        } 

        while (centerCubeJ >= laserCloudHeight - 3) {
          for (int i = 0; i < laserCloudWidth; i++) {
            for (int k = 0; k < laserCloudDepth; k++) {
              int j = 0;
              pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              for (; j < laserCloudHeight - 1; j++) {
                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudCornerArray[i + laserCloudWidth*(j + 1) + laserCloudWidth * laserCloudHeight*k];
                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudSurfArray[i + laserCloudWidth * (j + 1) + laserCloudWidth * laserCloudHeight*k];
              }
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeCornerPointer;
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeSurfPointer;
              laserCloudCubeCornerPointer->clear();
              laserCloudCubeSurfPointer->clear();
            }
          }

          centerCubeJ--;
          laserCloudCenHeight--;
        }

        while (centerCubeK < 3) {
          for (int i = 0; i < laserCloudWidth; i++) {
            for (int j = 0; j < laserCloudHeight; j++) {
              int k = laserCloudDepth - 1;
              pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              for (; k >= 1; k--) {
                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k - 1)];
                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k - 1)];
              }
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeCornerPointer;
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeSurfPointer;
              laserCloudCubeCornerPointer->clear();
              laserCloudCubeSurfPointer->clear();
            }
          }

          centerCubeK++;
          laserCloudCenDepth++;
        }
      
        while (centerCubeK >= laserCloudDepth - 3) {
          for (int i = 0; i < laserCloudWidth; i++) {
            for (int j = 0; j < laserCloudHeight; j++) {
              int k = 0;
              pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
              for (; k < laserCloudDepth - 1; k++) {
                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudCornerArray[i + laserCloudWidth*j + laserCloudWidth * laserCloudHeight*(k + 1)];
                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight*(k + 1)];
              }
              laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeCornerPointer;
              laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] = 
              laserCloudCubeSurfPointer;
              laserCloudCubeCornerPointer->clear();
              laserCloudCubeSurfPointer->clear();
            }
          }

          centerCubeK--;
          laserCloudCenDepth--;
        }

处理完毕边沿点，接下来就是在取到的子cube的5*5*5的邻域内找对应的配准点了。

        int laserCloudValidNum = 0;
        int laserCloudSurroundNum = 0;
        //5*5*5的邻域里进行循环寻找
        for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++) {
          for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) {
            for (int k = centerCubeK - 2; k <= centerCubeK + 2; k++) {
              if (i >= 0 && i < laserCloudWidth && 
                  j >= 0 && j < laserCloudHeight && 
                  k >= 0 && k < laserCloudDepth) {

                //int centerCubeI = int((transformTobeMapped[3] + 25.0) / 50.0) + laserCloudCenWidth;
                //int centerCubeJ = int((transformTobeMapped[4] + 25.0) / 50.0) + laserCloudCenHeight;
                //int centerCubeK = int((transformTobeMapped[5] + 25.0) / 50.0) + laserCloudCenDepth;
                //centerX,Y,Z = transformTobeMapped[3,4,5]+25
                float centerX = 50.0 * (i - laserCloudCenWidth);
                float centerY = 50.0 * (j - laserCloudCenHeight);
                float centerZ = 50.0 * (k - laserCloudCenDepth);

                bool isInLaserFOV = false;
                //确定邻域的点是否可用（是否在lidar的视角内）
                for (int ii = -1; ii <= 1; ii += 2) {
                  for (int jj = -1; jj <= 1; jj += 2) {
                    for (int kk = -1; kk <= 1; kk += 2) {
                      float cornerX = centerX + 25.0 * ii;
                      float cornerY = centerY + 25.0 * jj;
                      float cornerZ = centerZ + 25.0 * kk;

                      float squaredSide1 = (transformTobeMapped[3] - cornerX) 
                                         * (transformTobeMapped[3] - cornerX) 
                                         + (transformTobeMapped[4] - cornerY) 
                                         * (transformTobeMapped[4] - cornerY)
                                         + (transformTobeMapped[5] - cornerZ) 
                                         * (transformTobeMapped[5] - cornerZ);

                      float squaredSide2 = (pointOnYAxis.x - cornerX) * (pointOnYAxis.x - cornerX) 
                                         + (pointOnYAxis.y - cornerY) * (pointOnYAxis.y - cornerY)
                                         + (pointOnYAxis.z - cornerZ) * (pointOnYAxis.z - cornerZ);
                      // 根据余弦定理进行判断
                      float check1 = 100.0 + squaredSide1 - squaredSide2
                                   - 10.0 * sqrt(3.0) * sqrt(squaredSide1);

                      float check2 = 100.0 + squaredSide1 - squaredSide2
                                   + 10.0 * sqrt(3.0) * sqrt(squaredSide1);
                      //视角在60°范围内
                      if (check1 < 0 && check2 > 0) {
                        isInLaserFOV = true;
                      }
                    }
                  }
                }
                
                //将选择好的点存入数组中
                if (isInLaserFOV) {
                  laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j 
                                                       + laserCloudWidth * laserCloudHeight * k;
                  laserCloudValidNum++;
                }
                laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j 
                                                             + laserCloudWidth * laserCloudHeight * k;
                laserCloudSurroundNum++;
              }
            }
          }
        }

接下来就准备精度更加高的配准了，首先是准备工作，我们需要两堆进行配准的点云

        laserCloudCornerFromMap->clear();
        laserCloudSurfFromMap->clear();
        //已选择好的上一时刻的用来进行配准的点
        for (int i = 0; i < laserCloudValidNum; i++) {
          *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
          *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
        }
        int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
        int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();
         
        //当前时刻的点，转到世界坐标系下
        int laserCloudCornerStackNum2 = laserCloudCornerStack2->points.size();
        for (int i = 0; i < laserCloudCornerStackNum2; i++) {
          pointAssociateTobeMapped(&laserCloudCornerStack2->points[i], &laserCloudCornerStack2->points[i]);
        }

        int laserCloudSurfStackNum2 = laserCloudSurfStack2->points.size();
        for (int i = 0; i < laserCloudSurfStackNum2; i++) {
          pointAssociateTobeMapped(&laserCloudSurfStack2->points[i], &laserCloudSurfStack2->points[i]);
        }
        
        //降采样
        laserCloudCornerStack->clear();
        downSizeFilterCorner.setInputCloud(laserCloudCornerStack2);
        downSizeFilterCorner.filter(*laserCloudCornerStack);
        int laserCloudCornerStackNum = laserCloudCornerStack->points.size();

        laserCloudSurfStack->clear();
        downSizeFilterSurf.setInputCloud(laserCloudSurfStack2);
        downSizeFilterSurf.filter(*laserCloudSurfStack);
        int laserCloudSurfStackNum = laserCloudSurfStack->points.size();

        laserCloudCornerStack2->clear();
        laserCloudSurfStack2->clear();

这样，我们就得到了用来配准的点云，接下来步入正题。我们再次拿出KD树，来寻找最邻近的5个点。对点云协方差矩阵进行主成分分析：若这五个点分布在直线上，协方差矩阵的特征值包含一个元素显著大于其余两个，与该特征值相关的特征向量表示所处直线的方向；若这五个点分布在平面上，协方差矩阵的特征值存在一个显著小的元素，与该特征值相关的特征向量表示所处平面的法线方向。

if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 100) {//数量足够多才进行处理
          //kd树寻找最近点
          kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
          kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);

          for (int iterCount = 0; iterCount < 10; iterCount++) {
            laserCloudOri->clear();
            coeffSel->clear();

            for (int i = 0; i < laserCloudCornerStackNum; i++) {
              pointOri = laserCloudCornerStack->points[i];
              pointAssociateToMap(&pointOri, &pointSel);
              kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
              
              if (pointSearchSqDis[4] < 1.0) {
                float cx = 0;
                float cy = 0; 
                float cz = 0;
                for (int j = 0; j < 5; j++) {
                  cx += laserCloudCornerFromMap->points[pointSearchInd[j]].x;
                  cy += laserCloudCornerFromMap->points[pointSearchInd[j]].y;
                  cz += laserCloudCornerFromMap->points[pointSearchInd[j]].z;
                }
                //五个点坐标的算术平均值
                cx /= 5;
                cy /= 5; 
                cz /= 5;

                float a11 = 0;
                float a12 = 0; 
                float a13 = 0;
                float a22 = 0;
                float a23 = 0; 
                float a33 = 0;
                for (int j = 0; j < 5; j++) {
                  float ax = laserCloudCornerFromMap->points[pointSearchInd[j]].x - cx;
                  float ay = laserCloudCornerFromMap->points[pointSearchInd[j]].y - cy;
                  float az = laserCloudCornerFromMap->points[pointSearchInd[j]].z - cz;
                  
                  a11 += ax * ax;
                  a12 += ax * ay;
                  a13 += ax * az;
                  a22 += ay * ay;
                  a23 += ay * az;
                  a33 += az * az;
                }
                a11 /= 5;
                a12 /= 5; 
                a13 /= 5;
                a22 /= 5;
                a23 /= 5; 
                a33 /= 5;
                //协方差矩阵
                matA1.at<float>(0, 0) = a11;
                matA1.at<float>(0, 1) = a12;
                matA1.at<float>(0, 2) = a13;
                matA1.at<float>(1, 0) = a12;
                matA1.at<float>(1, 1) = a22;
                matA1.at<float>(1, 2) = a23;
                matA1.at<float>(2, 0) = a13;
                matA1.at<float>(2, 1) = a23;
                matA1.at<float>(2, 2) = a33;

                //求特征值及特征向量
                cv::eigen(matA1, matD1, matV1);

之后则是和LaserOdometry中一样的优化步骤，这里就不再贴出代码了。

在更新了位姿之后，将当前时刻的点云存入cube中，为下一次的配准做准备

for (int i = 0; i < laserCloudCornerStackNum; i++) {
          pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);

          int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
          int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
          int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

          if (pointSel.x + 25.0 < 0) cubeI--;
          if (pointSel.y + 25.0 < 0) cubeJ--;
          if (pointSel.z + 25.0 < 0) cubeK--;

          if (cubeI >= 0 && cubeI < laserCloudWidth && 
              cubeJ >= 0 && cubeJ < laserCloudHeight && 
              cubeK >= 0 && cubeK < laserCloudDepth) {
            int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
            laserCloudCornerArray[cubeInd]->push_back(pointSel);
          }
        }

        for (int i = 0; i < laserCloudSurfStackNum; i++) {
          pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);

          int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
          int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
          int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

          if (pointSel.x + 25.0 < 0) cubeI--;
          if (pointSel.y + 25.0 < 0) cubeJ--;
          if (pointSel.z + 25.0 < 0) cubeK--;

          if (cubeI >= 0 && cubeI < laserCloudWidth && 
              cubeJ >= 0 && cubeJ < laserCloudHeight && 
              cubeK >= 0 && cubeK < laserCloudDepth) {
            int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
            laserCloudSurfArray[cubeInd]->push_back(pointSel);
          }
        }

最后，将点云数据发布出去

        mapFrameCount++;
        if (mapFrameCount >= mapFrameNum) {
          mapFrameCount = 0;

          laserCloudSurround2->clear();
          for (int i = 0; i < laserCloudSurroundNum; i++) {
            int ind = laserCloudSurroundInd[i];
            *laserCloudSurround2 += *laserCloudCornerArray[ind];
            *laserCloudSurround2 += *laserCloudSurfArray[ind];
          }

          laserCloudSurround->clear();
          downSizeFilterCorner.setInputCloud(laserCloudSurround2);
          downSizeFilterCorner.filter(*laserCloudSurround);

          sensor_msgs::PointCloud2 laserCloudSurround3;
          pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
          laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
          laserCloudSurround3.header.frame_id = "/camera_init";
          pubLaserCloudSurround.publish(laserCloudSurround3);
        }

        int laserCloudFullResNum = laserCloudFullRes->points.size();
        for (int i = 0; i < laserCloudFullResNum; i++) {
          pointAssociateToMap(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
        }

        sensor_msgs::PointCloud2 laserCloudFullRes3;
        pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
        laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
        laserCloudFullRes3.header.frame_id = "/camera_init";
        pubLaserCloudFullRes.publish(laserCloudFullRes3);

        geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw
                                  (transformAftMapped[2], -transformAftMapped[0], -transformAftMapped[1]);

        odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
        odomAftMapped.pose.pose.orientation.x = -geoQuat.y;
        odomAftMapped.pose.pose.orientation.y = -geoQuat.z;
        odomAftMapped.pose.pose.orientation.z = geoQuat.x;
        odomAftMapped.pose.pose.orientation.w = geoQuat.w;
        odomAftMapped.pose.pose.position.x = transformAftMapped[3];
        odomAftMapped.pose.pose.position.y = transformAftMapped[4];
        odomAftMapped.pose.pose.position.z = transformAftMapped[5];
        odomAftMapped.twist.twist.angular.x = transformBefMapped[0];
        odomAftMapped.twist.twist.angular.y = transformBefMapped[1];
        odomAftMapped.twist.twist.angular.z = transformBefMapped[2];
        odomAftMapped.twist.twist.linear.x = transformBefMapped[3];
        odomAftMapped.twist.twist.linear.y = transformBefMapped[4];
        odomAftMapped.twist.twist.linear.z = transformBefMapped[5];
        pubOdomAftMapped.publish(odomAftMapped);

        aftMappedTrans.stamp_ = ros::Time().fromSec(timeLaserOdometry);
        aftMappedTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
        aftMappedTrans.setOrigin(tf::Vector3(transformAftMapped[3], 
                                             transformAftMapped[4], transformAftMapped[5]));
        tfBroadcaster.sendTransform(aftMappedTrans);

      }
    }

    status = ros::ok();
    rate.sleep();
  }

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

LOAM

Slam

LOAM_velodyne学习（三）的相关文章

坐标变换与基变换到底哪个左乘，哪个右乘??

Version Date By Change Cost A 2020 3 18 AYZP First Version 三小时前言学习目的 1 坐标变换与基变换到底哪个左乘哪个右乘答案根本就是由基和坐标的维数决定其到底左乘还是右乘
大师兄！SLAM 为什么需要李群与李代数？

from https mp weixin qq com s sVjy9kr 8qc9W9VN78JoDQ 作者 electech6 来源计算机视觉life 编辑 Tony 很多刚刚接触SLAM的小伙伴在看到李群和李代数这部分的时候都有点
SLAM笔记（四）运动恢复结构的几何数学(本征矩阵、单应矩阵、基础矩阵)

1 间接法进行运动恢复的前提假设对于结构与运动或视觉三维重建中通常假设已经通过特征匹配等方法获取了匹配好的点对先求出匹配点对再获取结构和运动信息的方法称作间接法间接法最重要的三个假设是 1 拥有一系列两帧之间的匹配点对但同时假设匹
slam数学基础——最小二乘

一前言 1 最小二乘是一类特殊形式的最优化求解问题相比于其他优化问题求解方式比较简洁 2 最小二乘被广泛应用于各种实际问题的建模在求解实际工程问题中有着广泛的应用例如 slam 中随处可见最小二乘的声影二线性最小二乘法 1 预
vscode_c++_slambook 编译配置

工作目录配置文件 launch json version 0 2 0 configurations name slamBook程序调试 type cppdbg request launch program fileDirname buil
微信小程序SLAM AR零基础入门教程

鬼灭之刃花街篇开播在即今天带大家零基础使用Kivicube制作一个炭治郎的SLAM AR云手办可以通过微信小程序将AR版的炭治郎放置在家中提前感受鬼灭的氛围先上个GIF大家看看动态的展示效果在这里先科普一下本次教程使用到的AR技
SLAM入门

SLAM定义 SLAM Simultaneous localization and mapping 同时定位我在哪里与建图我周围有什么当某种移动设备汽车扫地机手机无人机机器人从一个未知环境的未知地点出发在运动过程中通
使用EKF融合odometry及imu数据

整理资料发现早前学习robot pose ekf的笔记大抵是一些原理基础的东西加一些自己的理解可能有不太正确的地方当时做工程遇到的情况为机器人在一些如光滑的地面上打滑的情形期望使用EKF利用imu对odom数据进行校正就结果来看
Difference Between LiDAR and RADAR——LiDAR和RADAR的不同

Difference Between LiDAR and RADAR 原文连接 https www differencebetween com difference between lidar and vs radar 翻译 RADAR和L
关于GPS、惯导、视觉里程计的几个定义

1 首先写几个定义惯性导航系统 Inertial Navigation System INS 全球定位卫星系统 Global Navigation Satellite System GNSS GNSS 包括全球定位系统 Global Po
Sophus安装踩坑

装SLAM十四讲第二版提供的Sophus Eigen版本3 4 0 报错 home ch 下载 Sophus 13fb3288311485dc94e3226b69c9b59cd06ff94e test core test so2 cpp 9
IMU预积分的一些理解

IMU预积分算是比较简单的一个算法无奈网上找到的资料都讲的晦涩难懂看明白了也觉得不过如此讲一下我的理解整体流程 1 推导IMU离散运动方程 2 根据离散运动方程进行预积分并将预积分的误差项拆分出来因为我们在定义误差的时候有
ORB-SLAM2：基于可识别特征的自主导航与地图构建

目录 ORB SLAM2 基于可识别特征的自主导航与地图构建简介地图 A 地图特征点或3D ORB B 关键帧 C 可视化图像位置识别 A 图像识别数据库 B 高效优化的ORB匹配 C 视觉一致性自主导航追踪 A ORB特征获取 B
docker dbus-x11

本来想用terminator启动nvidia docker 显示出图形界面的结果发现启动的时候出问题了 terminator 1 dbind WARNING 07 31 53 725 Couldn t connect to accessi
LOAM算法详解

激光SLAM 帧间匹配方法 Point to Plane ICP NDT Feature based Method 回环检测方法 Scan to Scan Scan to Map LOAM创新点定位和建图的分离里程计模块高频低质量的帧
Eigen几何模块的使用方法

include
ORB_SLAM2运行官方数据集/自己数据集

官方数据集运行结果 WeChat 20230210194425 可以正常运行自己数据集运行结果自己的数据集主要是用手机摄像头采集的实验室进行了一下简单的运行可以成功运行但是由于查看的相关程序的是死循环不能像运行官方数据集那样完整保
Ubuntu18.04安装Autoware1.15（解决Openplanner无法绕障的问题：Openplanner2.5）

文章目录一下载Autoware1 15源码二安装依赖三修改CUDA版本四编译以及报错解决编译 1 报 undefined reference to cv Mat Mat 的错就按照下面方式改相应包 2 遇到OpenCV的C
什么是深度学习的无监督学习与有监督学习

无监督学习深度学习中的无监督学习方法是一种训练算法它在没有标注输出的情况下从输入数据中学习模式和特征这种方法的核心是探索和理解数据的内在结构和分布而不是通过已知的输出来指导学习过程无监督学习在深度学习领域有许多不同的形式和应用以
高翔博士Faster-LIO论文和算法解析

说明题目 Faster LIO 快速激光IMU里程计参考链接 Faster LIO 快速激光IMU里程计 iVox Faster Lio 智行者高博团队开源的增量式稀疏体素结构 Faster Lio是高翔博士在Fast系列的新作对标基

随机推荐

pandas的时间对象

pandas时间处理对象 pandas中有个时间库 datautil 可以使用其中的方法把多种字符串时间格式转化为时间对象 import dateutil import pandas as pd a dateutil parser pars
git还原到某个版本

1 tortoisegit还原 v2还原到v1 1 1 强制还原 git reset 如果使用这种方式还原到v1 将丢失还原到v1到v2之间的所有提交及日志 1 1 1 显示日志有save1 save2两条提交记录 1 1 2 重置版本
Git的理解与使用

文章目录一初识Git 1 1 分布式管理系统 1 2 Git的安装与配置二 Git理论 2 1 四个工作区域 2 2 提交代码的简易流程 2 3 Git所管理文件的四种状态三 Git命令 3 1 基础命令 git init git
hash函数应用（整理）

评估hash函数优劣的基准主要有以下两个指标 1 散列分布性即桶的使用率backet usage 已使用桶数总的桶数这个比例越高说明分布性良好是好的hash设计 2 平均桶长即avg backet len 所有已使用桶的平均长度
iTween基础之Rotate（旋转角度）

一基础介绍二基础属性原文地址 http blog csdn net dingkun520wy article details 50696489 一基础介绍 RotateTo 旋转游戏
Java判断字符串是否为数字(正负、小数)

需求传来一个String类型的参数需要判断该参数是否为数字正负正数小数都要能判断吗如果是小数则保留2位小数开始采用Character isDigit 方法来判断一个字符串是否为数字只能判断全是数字的字符串不能判断小数负
Anaconda创建虚拟环境+Pycharm使用Anaconda创建的虚拟环境

首先需要下载anaconda然后在搜索栏中搜索Anaconda Prompt anaconda 点击进入进入到envs目录然后输入以下命令 conda create n to pack python 3 7 创建一个名为 to pack且
vue动态绑定class属性

vue动态绑定class的方式第一种对象的方式 class iscolor boo 第一个参数iscolor为类名第二个参数boo是一个变量类型为一个boolean值 div scope row state div data ret
三角形边长求高的c语言函数公式,三角形边长计算公式

大写数字网今天精心准备的是三角形边长计算公式下面是详解求三角形的边长公式三角形的边长公式 1 在任何一个三角形中任意一边的平方等于另外两边的平方和减去这两边的2倍乘以它们夹角的余弦几何语言在 ABC中 a b c 2bc co
js数组分类,一维数组转二维数组

原始数组 var arrayFirst code 1 datas a网吧 code 1 datas b网吧 code 2 datas a酒店 code 2 datas b酒店 code 3 datas a学校 code 3 datas b学
Flume系统搭建和使用的一些经验总结-搭建篇

对于很多公司来说日志的收集和集中管理是一个必然要经历的阶段我们公司在经历了一拖再拖之后终于不得不开始搭建日志收集系统了对于日志收集系统我们的首选就是Flume 为何这么坚决呢难道没有其他工具能做个这个事情么当然有不过考虑到
神经网络 01(介绍)

一神经网络人工神经网络 Artificial Neural Network 简写为ANN 也简称为神经网络 NN 是一种模仿生物神经网络结构和功能的计算模型人脑可以看做是一个生物神经网络由众多的神经元连接而成各个神经元传递复杂的
【夜莺监控方案】01-n9e-v5-server部署

文章目录前言 1 在线一键安装不推荐 2 自主安装推荐官方安装脚本 2 1 mysql 2 2 prometheus 2 3 n9e server 2 4 启动和开机自启 2 5 web查看 3 配置LDAP 前言相关文档如下 0
Python自制音乐下载器，实现听歌自由

前言今天发的就是最实用的文章让你用Python实现听歌自由不用再担心自己的钱包了文章末尾名片可直接领取代码代码实现导入模块 import os import re from urllib import parse import
[人工智能-深度学习-75]：环境 - Windows配置Github、Gitee共存的Git环境

作者主页文火冰糖的硅基工坊文火冰糖王文兵的博客文火冰糖的硅基工坊 CSDN博客本文网址 https blog csdn net HiWangWenBing article details 122261638 目录前言前置条件
获取response里的数据

String out null try ServletOutputStream os servletResponse getOutputStream Field ob ReflectionUtils findField os getClas
VS2015/QT creator + Qt5.8.0

PS 两个版本IDE都试过 VS的报错更详细方便找bug QT creator的界面更可爱输入时有绿色的Q弹的图标嘻嘻 QT版本 qt opensource windows x86 msvc2015 64 5 8 0 win10 vs2
微信小程序实例系列

实战微信小程序 redux 在原生微信小程序的使用实例微信小程序 weapp redux的使用文档微信小程序 Promise then success fail 执行顺序的问题微信小程序监听页面停止滚动微信小程序 CustomB
PIM协议原理与配置

PIM协议原理 PIM Protocol Independent Multicast 协议无关组播目前常用版本是PIMv2 PIM报文直接封装在IP报文中协议号为103 PIMv2组播地址为224 0 0 13 在PIM组播域中以组播
LOAM_velodyne学习（三）

终于到第三个模块了我们先来回顾下之前的工作点云数据进来后经过前两个节点的处理可以完成一个完整但粗糙的里程计可以概略地估计出Lidar的相对运动如果不受任何测量噪声的影响这个运动估计的结果足够精确没有任何漂移那我们可以直接利用

LOAM_velodyne学习（三）

LaserMapping

LOAM_velodyne学习（三） 的相关文章

随机推荐

热门标签

LOAM_velodyne学习（三）的相关文章