liosam详解

摘要

关于liosam的总结

接下来对liosam进行梳理总结

1. 整体架构

• 系统架构
  

• 输入：IMU、点云、GPS

• 输出：odometry

• 四个模块：

模块	功能	输入	输出
imuPreintegration	imu预积分，估计bias、优化	imu	odometry
imageProjection	点云去畸变	point cloud、imu raw、imu odometry	PCD(deskewed)
featureExtraction	点云特征提取（角点、平面）	PCD(deskewed)	PCD+feature(corner, surface)
mapOptimization	激光里程计、回环检测、位姿优化	PCD+feature	lidar odometry

2. imuPreintegration模块

• 订阅：imu raw data、lidar odometry
• 发布：imu odometry
• 功能：imu预积分，图优化估计bias
• 代码详解：

2.1. 构造函数

• 消息订阅

  // 订阅imu raw data，在回调函数imuHandler中进行预积分处理
  // 参数1：topic名称
  // 参数2：消息队列的大小，当处理速度小于消息的帧率时，消息会缓存起来，如果缓存长度超出这个数值时，最早来的消息会被丢掉
  // 参数3：回调函数
  subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());// 订阅lidar odometry，在回调函数odometryHandler中进行图优化估计bias
  subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());
  

• 消息发布

  pubImuOdometry = nh.advertise<nav_msgs::Odometry>(odomTopic +"_incremental", 2000);
  

• 初始化

  // 使用gtsam提供的预积分类，注意两个积分器的区别TODO
  imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias);
  imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias);
  

2.2. 预积分回调函数-imuHandler

• 将imu坐标轴的方向对齐到ROS REP-105约定下（x前，y左，z上）

  sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
  imuQueOpt.push_back(thisImu);
  imuQueImu.push_back(thisImu);
  

• 计算imu两帧之间的dt

  // 注意第一帧的处理，上一帧时间还没有，这是dt设为imu数据帧率。如果你自己的imu的帧率没有500Hz的话，这里需要改一下。
  double imuTime = ROS_TIME(&thisImu);
  double dt = (lastImuT_imu < 0) ? (1.0/500.0) : (imuTime - lastImuT_imu);
  

• 预积分，发布odometry

  // 积分
  imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),gtsam::Vector3(thisImu.angular_velocity.x, thisImu.angular_velocity.y, thisImu.angular_velocity.z), dt);
  // 预测当前状态
  gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);// 发布imu odometry
  nav_msgs::Odometry odometry;
  odometry.header.stamp = thisImu.header.stamp;
  ...// 这里在发布之前，还要进行一次坐标转换
  gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
  gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);
  // 放到odometry变量中
  odometry.pose.pose.position.x = lidarPose.translation().x();
  ...
  pubImuOdometry.publish(odometry);
  

• 关于坐标转化
  我们发现代码中有两次和坐标系相关的操作，分别是

  sensor_msgs::Imu thisImu = imuConverter(*imu_raw);
  

  gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);
  

  可能初读的时候对坐标系的变换有些混乱（反正我是有点混乱），有必要对坐标系进行一下梳理：

  1. imu raw: 这是原始的imu坐标系；
  2. thisImu: 经过第一次转换得到的IMU坐标系，
  3. lidar: lidar坐标系
  4. world: 全局/世界/odometry坐标系

  解释一下：

  1. imuConverter的作用是将imu的坐标系对齐到ROS REP-105约定下（x前，y左，z上）。
     需要注意的是，这并不是转到lidar坐标系（有些文章中称为lidar坐标系，这是不对的，会引起混乱）
  2. 经过积分得到imuPose为全局坐标系下imu的位姿
  3. 到现在为止，我们只知道IMU在哪里。我们需要知道lidar在哪儿，因为点云都是以lidar为基准的。imu2Lidar是imu和lidar之间的外参，它描述了imu相对于lidar是怎么安装的。这里其实只需要平移，因为在第1步的时候，已经交给坐标轴的方向都对齐了。通过compose(imu2Lidar)，得到了激光雷达的位姿。

2.3. odometryHandler

这个函数的功能是：以激光里程计(lidar odometry)为“真值”或“锚点”，融合两次激光里程计之间的所有imu数据，进行因子图优化，最后得到一个平滑且高精度的状态估计（包含位姿、速度、imubias）。
并完成了两个imu积分器的管理

• 数据提取

  float p_x = odomMsg->pose.pose.position.x;
  ...
  float r_w = odomMsg->pose.pose.orientation.w;
  bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
  gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
  

  degenerate:检查odomMsg协方差的第一个元素。这是LIOSAM的一个特性，如果前端的激光匹配发生了退化（比如在长廊里，无法确定前后方向的位移），这个标志会置为true。

• 初始化
  resetOptimization();清空因子图和优化器
  while(!imuQueOpt.empty()) ...丢掉时间戳早于odomMsg的imu数据，只关心两帧激光之间的imu数据
  prevPose_ = lidarPose.Compose(lidar2Imu);由于积分是以imu位姿为基准进行的，所以需要转成imu位姿。
  gtsam::PriorFactor<gtsam::Pose3> prioPose...添加先验位姿因子
  gtsam::PriorFactor<gtsam::Vector3> priorVel...添加先验速度因子
  gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias...添加先验偏置因子
  graphValues.insert(X(0), prevPose_);添加位姿变量
  graphValues.insert(V(0), prevVel_);添加速度变量
  graphValues.insert(B(0), prevBias_);添加偏置变量
  optimizer.update(graphValues, graphFactors);执行一次优化，因为只有先验因子，所以作用只是固定住初始状态
  graphFactors.resize(0); 清空因子图
  graphValues.clear(); 清空
  imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);...重置imu积分器，设置初始偏置

• 重置因子图
  这部分代码的作用是重置因子图，防止因子图过大，影响优化速度
  具体的，在关键帧数量达到100时，清空图，并将最新一帧的结果作为新的先验，来构建一个新的图。

  // reset graph for speed
  if (key == 100)
  {// get updated noise before resetgtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise  = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));// reset graphresetOptimization();// add posegtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);graphFactors.add(priorPose);// add velocitygtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);graphFactors.add(priorVel);// add biasgtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);graphFactors.add(priorBias);// add valuesgraphValues.insert(X(0), prevPose_);graphValues.insert(V(0), prevVel_);graphValues.insert(B(0), prevBias_);// optimize onceoptimizer.update(graphFactors, graphValues);graphFactors.resize(0);graphValues.clear();key = 1;
  }
  

• imu积分
  对上一帧激光雷达和当前激光之间的imu数据进行积分。

  while (!imuQueOpt.empty())
  {// pop and integrate imu data that is between two optimizationssensor_msgs::Imu *thisImu = &imuQueOpt.front();double imuTime = ROS_TIME(thisImu);if (imuTime < currentCorrectionTime - delta_t){double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);imuIntegratorOpt_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);lastImuT_opt = imuTime;imuQueOpt.pop_front();}elsebreak;
  }
  

• 构建因子图优化
  添加imu因子，这是运动模型，它约束了上一帧状态和当前状态之间的运动关系。

  const gtsam::PreintegratedImuMeasurements& preint_imu = ...;
  gtsam::ImuFactor imuFactor...;
  graphFactors.add(imuFactor);
  

  添加bias因子

  graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias:ConstBias>(...));
  

  添加激光里程计因子，这里根据是否退化选择了不同的方差，如果退化的话，方差会更大，意味着不太相信当前的观测。

  gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
  gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate? correctionNoise2 : correctionNoise);
  graphFactors.add(pose_factor);
  

  将imu积分的预测值作为初始估计值插入因子图中，这样可以加快优化收敛。

  gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
  graphValues.insert(X(key), propState_.pose());
  graphValues.insert(V(key), propState_.v());
  graphValues.insert(B(key), prevBias_);
  

  执行优化

  optimizer.update(graphFactors, graphValues);
  optimizer.update(); // ?
  

  提取优化结果

  gtsam::Values result = optimizer.calculateEstimate();
  prevPose_  = result.at<gtsam::Pose3>(X(key));
  prevVel_   = result.at<gtsam::Vector3>(V(key));
  prevState_ = gtsam::NavState(prevPose_, prevVel_);
  prevBias_  = result.at<gtsam::imuBias::ConstantBias>(B(key));
  

  重置积分器偏置

  imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
  

• 重传播（repropogate）
  这部分代码的背后的思想：
  在以上部分，我们经过因子图优化得到一个优化后的偏置prevBias_。
  这个时间点是当前激光的时间，由于imu的帧率较高，在得到上述更新后的偏置之后的一段时间里，imuHandler中仍然在使用旧的偏置。
  所以应该用这个新的偏置代替就的偏置重新对currentCorrectionTime之后的imu数据进行重新积分。
  imuHandler下次再调用predict时，是基于优化后的偏置进行的。

  // 这里的关键是这个时间点，丢掉这个时刻之前的imu，对这个时刻之后的imu进行重新积分
  while(!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delat_t)
  {lastImuQT = ROS_TIME(&imuQueImu.front());imuQueImu.pop_front();
  }
  // repropagate imu measurements
  while (!imuQueImu.empty())
  {// 重置积分的偏置imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);// 重积分for(...) {...}
  }
  

3. ImageProjection模块

• 订阅：imu raw, imu odometry, point cloud raw
• 发布：去畸变的点云，点云信息
• 功能：点云去畸变
• 代码详解：

3.1. 构造函数

• 略

3.2. imuHandler

• 回调函数，接收到imu数据时触发

  // 和前面讲到的一样，这里也是先对齐坐标轴
  sensor_msgs::Imu thisImu  = imuConverter(*imuMsg);
  std::lock_guard<std::mutex> lock1(imuLock);
  imuQueue.push_back(thisImu); // 输入放入队列
  

3.3 odometryHandler

• 回调函数，接收到imu odometry数据时触发

  std::lock_guard<std::mutex> lock2(odoLock);
  odomQueue.push_back(*odometryMsg); // 加入队列缓存
  

3.4 cloudHandler

• 回调函数，接收到点云数据时触发

  void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
  {// 点云预处理、格式转换、必要字段检查if(!cachePointCloud(laserCloudMsg))return;// 利用imu和odometry队列，准备好用于去畸变所需的运动信息if(!deskewInfo())return;// 遍历点云去除运动畸变projectPointCloud();// 将点云重新组织打包cloudExtraction();// 将去畸变后的点云和提取的一些简单信息发布出去，供后续使用publishClouds();// 一些参数重置resetParameters();
  }
  

• cachePointCloud点云预处理、格式转换与有效性检查
  将点云缓存到队列中，队列size需要大于等于2

  cloudQueue.push_back(*laserCloudMsg);
  if(cloudQueue.size() <= 2) return false;
  

  取出队列头上（old）的点云进行格式转换，将ROS格式转为PCL格式

  currentCloudMsg = std::move(cloudQueue.front());
  cloudQueue.pop_front();
  // 根据激光雷达的类型进行格式转换
  if(sensor==SensorType:: || sensor == SensorType::LIVOX)
  {...
  }
  else if(sensor == SensorType::OUSTER)
  {...
  }
  

  获取时间戳

  cloudHeader = currentCloudMsg.header;
  timeScanCur = cloudHeader.stamp.toSec(); // 开始扫描时间
  timsScanEnd = timeScanCur + laserCloudIn->points.back().time; // 结束扫描时间
  

  检查点云是否稠密：laserCloudIn->is_dense == false，需要去除无效NaN点。
  检查ring字段：currentCloudMsg.fields[i].name == "ring"，去畸变以及后续的特征提取都是在ring维度（哪条扫描线）上进行的
  检查time字段：currentCloudMsg.fields[i].name == "time"，需要有时间戳信息，否则无法通过运动信息去畸变。

• deskewInfo准备去畸变所需的运动信息
  imuDeskewInfo用imu raw data计算运动信息，对开始扫描和结束扫描之间的imu角速度进行积分，得到角度。

  imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
  imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
  imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
  imuTime[imuPointerCur] = currentImuTime;
  ++imuPointerCur;
  

  odomDeskewInfo用imu Odometry计算运动信息
  根据imu odometry信息给cloud赋值cloud的初始位置，startOdomMsg对应扫描开始时刻对应的位姿。

  cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x;
  cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y;
  cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z;
  cloudInfo.initialGuessRoll = roll;
  cloudInfo.initialGuessPitch = pitch;
  cloudInfo.initialGuessYaw = yaw;
  

  计算从开始扫描时刻到结束扫描时刻，位姿变化增量，用于畸变矫正

  // 将开始扫描时刻位姿转成仿射变换矩阵
  Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.y, roll, pitch, yaw); 
  // 将结束时刻位姿转成仿射变换矩阵
  Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.y, roll, pitch, yaw); 
  // 计算位姿变换增量
  Eigen::Affine3f transBt = transBegin.inverse() * transEnd;
  

• projectPointCloud去畸变
  遍历点云，对每个点进行去畸变

  for (size_t i = 0; i < cloudSize; i++)
  {PointType thisPoint = laserCloudIn->points[i];...thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);
  }
  

  deskewPoint根据点的时间戳，去除畸变

  // 根据时间戳找到对应的旋转角度
  findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);
  // 根据时间找到对应的位移，这里统一返回了0，并没有起作用
  findPosition(relTime, &posXCur, &posYCur, &posZCur);
  ...
  // 从扫描开始时刻到当前点时刻的变换矩阵
  Eigen::Affine3f transBt = transStartInverse * transFinal;
  // 对点进行去畸变
  PointType newPoint;
  newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3);
  newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3);
  newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3);
  newPoint.intensity = point->intensity;
  

• cloudExtraction对去畸变后的点云重新组织打包

  记录每条线束的起始和结束索引，注意每条线束的起始和结束索引都向里收缩了5个点。

  for(int i = 0; i < N_SCAN; ++i)
  {cloudInfo.startRingIndex[i] = count - 1 + 5;...cloudInfo.endRingIndex[i] = count - 1 + 5; 
  }
  

  记录每个点的一维索引和二维索引

  for(int i = 0; i < N_SCAN; ++i)
  {for(int j = 0; j < Horizon_SCAN; ++j){cloudInfo.pointColInd[count] = j;cloudInfo.pointRange[count] = rangeMat.at<float>(i, j);extractedCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]);++count;}
  }
  

• publishClouds点云即点云信息的发布
  略

• resetParameters参数重置
  略

• 两个疑问：

1. 已经有了imu odometry了为什么还需要imu原始数据？
2. 为什么用imu raw data只计算了旋转角度，没有位移？

4. featureExtraction模块

• 订阅：去畸变的点云
• 发布：点云+特征（角点、平面）
• 功能：提取点云的角点和平面特征
• 代码详解：

4.1. 构造函数

• 订阅去畸变点云，注册回调函数laserCloudInfoHandler
• 初始化点云信息发布器、角点特征发布器、平面特征发布器
• 初始化参数

4.2. laserCloudInfoHandler

• calculateSmoothness计算点云平滑度
  掐头去尾，从第6个点到倒数第6个点，计算平滑度

  float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]+ cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]+ cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10+ cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]+ cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]+ cloudInfo.pointRange[i+5];
  // 用中心点与前后5个点的距离差的平方表示曲率，作为平滑度，用于后续特征提取
  cloudCurvature[i] =  diffRange*diffRange; 
  cloudNeighborPicked[i] = 0;
  cloudLabel[i] = 0;
  cloudSmoothness[i].value = cloudCurvature[i];
  cloudSmoothness[i].ind = i;
  

• markOccludedPoints标记遮挡点和平行点
  这里可以参考LOAM原文的解释，从原文拷贝的图片，见图。
  图a是beam和平面平行的情况，这种情况下B点是不固定的，所以要去除掉，以免引入不确定性。
  图b是遮挡的情况，如图橙色虚线部分被遮挡，当激光雷达的位置变化时，角点A是变动的不固定，所以要把像A点这样的角点去掉。这个函数就是做以上两件事情。
  

• extracFeatures提取点云的角点和平面特征
  每个beam分六段提取特征，分段的代码如下：

  for(int i = 0; i < N_SCAN; i++)
  {for(int j = 0; j < 6; j++){int sp = (cloudInfo.startRingIndex[i] *(6-j) + cloudInfo.endRingIndex[i] * j) / 6;int ep = (cloudInfo.startRingIndex[i] *(5-j) + cloudInfo.endRingIndex[i] *(j + 1)) / 6 - 1;}
  }
  

  上述分段的代码，需要推导一下，看着比较绕，还原后的公式如下：
  $sp=start+(end-start)/6\ast j$
  $ep=start+(end-start)/6\ast (j+1)$

  根据平滑度排序

  std::sort(cloudSmoothness.begin() + sp, cloudSmoothness.begin() + ep, by_value());
  

  提取角点，这部分逻辑比较简单，即平滑度大于阈值的点认为是角点。
  还有一步额外的操作，就是把角点周围的点标记一下，不再作为角点提取。

  提取平面，逻辑和角点类似，把平滑度小于阈值的点认为是平面点。

  使用栅格化进行降采样。

• publishFeatureCloud发布
  略

5. mapOptimization模块

• 订阅：点云+特征，gps，loop
• 发布：odometry，odometry_incremental, 等等
• 功能：
  1. 当前帧与局部地图匹配
  2. 关键帧保存
  3. 回环检测
  4. 位姿优化
• 代码详解：

5.1. 构造函数

• 略

5.2. laserCloudInfoHandler

回调函数，接收到提取好特征的点云时触发，该模块核心函数

• updateInitialGuess
  通过增量递推的方式，向前传递更新transformTobeMapped，为当前帧提供一个尽可能准确的位姿初始估计。
  初始化

  if(cloudKeyPoses3D->points.empty())
  {transformTobeMapped[0] = cloudInfo.imuRollInit;transformTobeMapped[1] = cloudInfo.imuPitchInit;transformTobeMapped[2] = cloudInfo.imuYawInit;lastImuTransformation = pcl::getTransformation(0,0,0,cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
  }
  

  使用imu odometry递推更新

  Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX, cloudInfo.initialGuessY, cloudInfo.initialGuessZ, cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw); 
  // 计算递推增量
  Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
  // 格式转换，数组转成仿射变换矩阵
  Eigen::Affine3f transTobe = trans2Affines3f(transformTobeMapped); 
  // 增量递推后的仿射变换矩阵
  Eigen::Affine3f transFinal = transTobe * transIncre;
  // 格式转换，有仿射变换矩阵转成数组格式
  pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
  

  当odomAvailable == false时，采用次优选择，用imu的自带的RPY进行递推，递推的方式同上述一致，不再赘述。

• extractSurroundingKeyFrames

  提取最新的关键帧周围的关键帧点云

  使用kdTree加速搜索的速度

  // 设置kdtree搜索的目标
  kdtreeSurroundingKeyPoses->setInputCloud(cloudKeyPoses3D);
  // 搜索距离最新的keyPose小于surroundingKeyframeSearchRadius的pose，搜索结果，索引放入pointSearchInd，距离值存入pointSearchSqDis
  kdtreeSurroundingKeyPoses->radiusSearch(cloudKeyPoses3D->back(), (double)surroundingKeyframeSearchRadius, pointSearchInd, pointSearchSqDis);
  

  搜索结果保存起来，并进行下采样

  for(int i = 0; i < pointSearchInd.size(); i++)
  {int id = pointSearchInd[i];surroundingKeyPoses->push_back(cloudKeyPoses3D->points[id]);
  }
  downSizeFilterSurroundingKeyPoses.setInputCloud(surroundingKeyPoses);
  downSizeFilterSurroundingKeyPoses.filter(*surroundingKeyPosesDS);
  

  恢复下采样后的关键帧pose在原始关键帧序列中的索引，这么做的原因是下采样的时候可能会破坏索引值

  for(auto& pt:surroundingKeyPosesDS->points)
  {// 使用kdtree最近邻搜索最近的一个点kdtreeSurroundingKeyPoses->nearestKSearch(pt, 1, pointSearchInd, pointSearchSqDis);// 把索引赋值给下采样的关键帧pt.intensity = cloudKeyPoses3D->points[pointSearchInd[0]].intensity;
  }
  

  注意：这里对于关键帧pose的intensity字段存放的是其在pose序列中的索引，并不是反照度，这里初读会又一定的迷惑性

  保留一写最新的关键帧，以防在原地转圈的情况。

  int numPoses = cloudKeyPoses3D->size();
  for (int i = numPoses-1; i >= 0; --i)
  {if (timeLaserInfoCur - cloudKeyPoses6D->points[i].time < 10.0)surroundingKeyPosesDS->push_back(cloudKeyPoses3D->points[i]);elsebreak;
  }
  

  提取对应的点云extractCloud，这里比较简单，不再赘述

• downsampleCurrentScan
  对当前帧的特征进行下采样，简单不赘述

• scan2MapOptimization
  核心函数*，scan to map匹配，因子图优化，*前提条件，特征数量大于设定的最小数量
  cornerOptimization角点特征匹配
  遍历当前帧每个角点，从local map中，K近邻搜索，得到距离它最近的5个点

  for(int i = 0; i < laserCloudCornerLastDSNum; i++)
  {// 转到全局坐标系下pointAssociateToMap(&pointOri, &pointSel);// kdtree最近邻搜索5个点kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);// 这里有个保护，搜索到的最近的5个点，距离要小于1mif(pointSearchSqDis[4] < 1.0){...}
  }
  

  计算5个点的均值
  $$c=\frac{1}{5}\sum _{i=1}^5{\mathbf{p}}_{\mathbf{i}}$$

  计算协方差
  $$\begin{gathered} \mathbf{d}=\mathbf{p}-\mathbf{c} \\ =\left[\begin{array}{ccc}{x}_1-cx& y_1-cy& z_1-cz\\ x_2-cx& y_2-cy& z_2-cz\\ x_3-cx& y_3-cy& z_3-cz\\ x_4-cx& y_4-cy& z_4-cz\\ x_5-cx& y_5-cy& z_5-cz\end{array}\right] \end{gathered}$$
  $$cov={\mathbf{d}}^{\mathbf{T}}\mathbf{d}$$
  对应的代码，协方差$cov$对应代码中的matA1：

  float cx = 0, cy = 0, cz = 0;
  for (int j = 0; j < 5; j++) {cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
  }
  cx /= 5; cy /= 5;  cz /= 5; // 均值float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
  for (int j = 0; j < 5; j++) {float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;float az = laserCloudCornerFromMapDS->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;
  

  对matA1进行特征分解，得到特征向量和特征值

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

  特征分解之后，比较了最大的特征值和第二大的特征值的关系

  // 最大的特征值大于次大特征值的3倍，认为5个点构成了直线
  if(matD1.at<float>(0,0) > 3 * matD1.at<float>(0,1))
  

  这里可以稍微解释一下，特征值反应的是特征向量方向上点云的离散程度/方差。当在第一个特征向量方向上的方差很大，其他特征向量方向上的方差很小。刚好符合直线的特征。
  接下来在特征向量方向（即直线方向）上取了两个点，表示边缘线，即代码中的$p1(x_1,y_1,z_1)$和$p2(x_2,y_2,z_2)$

  float x1 = cx + 0.1 * matV1.at<float>(0, 0);
  float y1 = cy + 0.1 * matV1.at<float>(0, 1);
  float z1 = cz + 0.1 * matV1.at<float>(0, 2);
  float x2 = cx - 0.1 * matV1.at<float>(0, 0);
  float y2 = cy - 0.1 * matV1.at<float>(0, 1);
  float z2 = cz - 0.1 * matV1.at<float>(0, 2);   
  

  接下来计算残差，即点到直线的距离：
  $$d_{ϵ}=\frac{\left|(p_0-p1)\times (p0-p2)\right|}{\left|p1-p2\right|}$$
  其中，$p_0$是当前帧的特征角点，$p_1,p_2$表示local map中距离$p_0$最近的边缘线上的两个点
  计算边缘线的法向量（计算雅可比矩阵时会用到）：
  $$\mathbf{n}=({\mathbf{p}}_0-{\mathbf{p}}_1)\times ({\mathbf{p}}_0-{\mathbf{p}}_2)\times ({\mathbf{p}}_1-{\mathbf{p}}_2)$$

  对应的代码：

  // |(p0-p1) x (p0 - p2)| 
  float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
  // |p1 - p2|
  float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));// (p0-p1)x(p0-p2)x(p1-p2)表示直线的法向量，经过归一化 n = (la,lb,lc)
  float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
  

  $p_0$到直线$p_1{p}_2$的距离

  float ld2 = a012 / l12;
  

  最后一个点，鲁棒系数，目的是降低离群点对优化的负面影响，使优化过程更鲁棒

  float s = 1 - 0.9 * fabs(ld2);
  

  surfOptimization平面特征匹配
  对于平面特征点，匹配方式类似，首先也是将当前帧的特征点转到全局坐标系，然后从localmap的特征中检索最近的5个特征点。同样也有一个条件保护，搜索到的点最远不能超过1m。然后直接用最小二乘法求解平面方程。平面方程形式如下：
  $$ax+by+cz+d=0$$
  令$d=1$，
  $$A[a,b,c{]}^T=B$$
  其中$\mathbf{A}$的每一行表示一个临近点$(x_i,y_i,z_i)$，$B$是所有元素都为1的列向量。
  通过QR分解求解超定方程组，得到平面方程系数a,b,c，对应的代码如下：

  Eigen::Matrix<float, 5, 3> matA0;
  Eigen::Matrix<float, 5, 1> matB0;
  Eigen::Vector3f matX0;
  matA0.setZero();
  matB0.fill(-1);
  matX0.setZero();
  ...
  for(int j = 0; j < 5; j++)
  {matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
  }
  matX0 = matA0.colPivHouseholderQr().solve(matB0);
  float pa = matX0(0,0); // 平面方程系数a,b,c,d
  float pb = matX0(1,0);
  float pc = matX0(2,0);
  float pd = 1;
  

  算出平面方程后，需要校验一下，检查5个点到平面的距离是否小于0.2m，没有超出即认为是合理的平面。
  已知平面方程后，可的平面的法向量
  $$\mathbf{n}=(\mathbf{a},\mathbf{b},\mathbf{c})$$
  以及$p_0$到平面的距离，
  $$d_{ϵ}=\frac{\left|a{x}_0+b{y}_0+c{z}_0+d\right|}{\sqrt{a^2+b^2+c^2}}$$
  对应的代码：

  float sp = sqrt(pa*pa + pb*pb + pc*pc);
  pa /= ps; pb /= ps; pc /= ps; pd /= ps; // 归一化
  ...
  float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; // 距离
  

  最后，计算鲁棒系数同cornerOptimization。将法向量、距离残差分别乘以鲁棒系数保存下来，计算雅可比矩阵时会用到。

  combineOptimizationCoeffs匹配结果stack到一起

  LMOptimization使用LM算法求解最优位姿$T$
  scan-to-map匹配的目标是找到一个位姿变换$T$，使得当前帧的特征点$p_i$变换到地图坐标系后，与地图$\mathcal{M}$的对应几何特征（线或面）之间的距离$d_i$最小，即：
  $$\underset{\mathbf{T}}{\min }\sum _i{d}_i^2({\mathbf{p}}_i,\mathcal{M})$$
  使用LM或高斯牛顿算法，通过迭代求解一个位姿增量$\Delta \mathbf{x}$来逼近最优解。求解位姿增量$\Delta \mathbf{x}$需要将误差函数$f_i(\mathbf{T})=d_i$在当前位姿估计$\mathbf{T}$处进行线性化：
  $$f_i(\mathbf{T}+\Delta \mathbf{x})\approx f_i(\mathbf{T})+{\mathbf{J}}_i\Delta \mathbf{x}$$
  其中$\Delta \mathbf{x}=[\Delta \text{roll},\Delta \text{pitch},\Delta \text{yaw},\Delta t_x,\Delta t_y,\Delta t_z{]}^T$是6维的位姿增量，${\mathbf{J}}_{\mathbf{i}}$是误差对$\Delta \mathbf{x}$的雅可比矩阵${\mathbf{J}}_i=\frac{\partial f_i}{\partial \Delta \mathbf{x}}$。
  我们希望误差为0，即：
  $$f_i(\mathbf{T})+{\mathbf{J}}_i\Delta \mathbf{x}\approx 0$$
  这使我们得到一个线性方程组：
  $${\mathbf{J}}_i\Delta \mathbf{x}=-f_i(\mathbf{T})$$
  将其写成矩阵形式：
  $$\mathbf{J}\Delta \mathbf{x}\approx -\mathbf{f}$$
  其中$\mathbf{J}$是${\mathbf{J}}_{\mathbf{i}}$堆叠而成的矩阵, $\mathbf{f}$是误差$d_i$堆叠而成的向量。为了求解以上方程，我们使用Normal Equation：

  $$({\mathbf{J}}^T\mathbf{J})\Delta \mathbf{x}=-{\mathbf{J}}^T\mathbf{f}$$

  下面推导雅可比矩阵$\mathbf{J}$的计算过程，根据链式法则，上式等于：
  $${\mathbf{J}}_i=\frac{\partial f_i}{\partial {\mathbf{p}}_{\mathbf{i}}^{\prime }}\cdot \frac{\partial {\mathbf{p}}_i^{\prime }}{\partial \Delta \mathbf{x}}$$
  第一项$\frac{\partial f_i}{\partial {\mathbf{p}}_{\mathbf{i}}^{\prime }}$：这是距离相对于点坐标的梯度。对于点到面/线距离，这个梯度就是法向量$\mathbf{n}$。这个法向量在特征提取代码中已经计算，并存储在coeffSel中。
  第二项$\frac{\partial {\mathbf{p}}_i^{\prime }}{\partial \Delta \mathbf{x}}$：这是点坐标对位姿增量的偏导数。
  即：
  $${\mathbf{J}}_i={\mathbf{n}}^T\cdot \frac{\partial (\mathbf{R}{\mathbf{p}}_i+\mathbf{t})}{\partial \Delta \mathbf{x}}$$
  其中，$\mathbf{n}=[n_x,n_y,n_z{]}^T$ 是点 ${\mathbf{p}}^{\prime }$ 到其对应特征（线/面）的单位法向量。这个法向量的各个分量存储在 coeff 点的 $(x,y,z)$ 字段中。

  现在计算第二项，对平移($\Delta t_x,\Delta t_y,\Delta t_z$)的偏导：
  $$\frac{\partial ({\mathbf{Rp}}_{\mathbf{i}}+\mathbf{t})}{\partial \mathbf{t}}=\mathbf{I}$$

  所以，平移部分的雅可比就是法向量本身：
  $${\mathbf{J}}_{\mathbf{trans}}=\frac{\partial {\mathbf{f}}_{\mathbf{i}}}{\partial {\mathbf{p}}_{\mathbf{i}}^{\prime }}\cdot \frac{\partial {\mathbf{p}}_{\mathbf{i}}^{\prime }}{\partial \mathbf{t}}={\mathbf{n}}^{\mathbf{T}}\mathbf{I}={\mathbf{n}}^{\mathbf{T}}$$
  对角度($\Delta r,\Delta p,\Delta y$)的偏导：
  $${\mathbf{J}}_{\mathbf{rot}}={\mathbf{n}}^{\mathbf{T}}\cdot \frac{\partial {\mathbf{p}}_{\mathbf{i}}^{\prime }}{\partial \alpha }$$
  其中，$\alpha =[roll,pitch,yaw{]}^T$。

  $$\frac{\partial {\mathbf{p}}_i^{\prime }}{\partial \Delta \mathbf{\alpha }}=\frac{\partial (\mathbf{R}{\mathbf{p}}_i+\mathbf{t})}{\partial \Delta \mathbf{\alpha }}=\frac{\partial (\mathbf{R}{\mathbf{p}}_i)}{\partial \Delta \mathbf{\alpha }}$$

  $$\begin{array}{rl}\mathbf{R}& ={\mathbf{R}}_{\mathbf{y}}{\mathbf{R}}_{\mathbf{x}}{\mathbf{R}}_{\mathbf{z}}\\ & =\left[\begin{array}{ccc}\cos (y)& 0& \sin (y)\\ 0& 1& 0\\ -\sin (y)& 0& \cos (y)\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (x)& -\sin (x)\\ 0& \sin (x)& \cos (x)\end{array}\right]\left[\begin{array}{ccc}\cos (z)& -\sin (z)& 0\\ \sin (z)& \cos (z)& 0\\ 0& 0& 1\end{array}\right]\\ & =\left[\begin{array}{ccc}\cos (y)\cos (z)+\sin (x)\sin (y)\sin (z)& \sin (x)\sin (y)\cos (z)-\cos (y)\sin (z)& \cos (x)\sin (y)\\ \cos (x)\sin (z)& \cos (x)\cos (z)& -\sin (x)\\ \sin (x)\cos (y)\sin (z)-\sin (y)\cos (z)& \sin (x)\cos (y)\cos (z)+\sin (y)\sin (z)& \cos (x)\cos (y)\end{array}\right]\end{array}$$
  以roll为例，求${\mathbf{Rp}}_{\mathbf{i}}$对roll的偏导：
  $$\frac{\partial {\mathbf{Rp}}_{\mathbf{i}}}{\partial x}=\left[\begin{array}{c}\cos (x)\sin (y)\sin (z)p_x+\cos (x)\sin (y)\cos (z)p_y-\sin (x)\sin (y)p_z\\ -\sin (x)\sin (z)p_x-\sin (x)\cos (z)p_y-\cos (x)p_z\\ \cos (x)\cos (y)\sin (z)p_x+\cos (x)\cos (y)\cos (z)p_y-\sin (x)\cos (y)p_z\end{array}\right]$$
  然后再左乘${\mathbf{n}}^{\mathbf{T}}$，对应代码中的：

  float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x+ (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y+ (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;  
  

  同样，$\frac{\partial {\mathbf{Rp}}_{\mathbf{i}}}{\partial y}$和$\frac{\partial {\mathbf{Rp}}_{\mathbf{i}}}{\partial z}$可以轻松计算出来，不再赘述。

  得到雅可比矩阵后，就可以通过Normal Equation求解位姿增量$\Delta \mathbf{x}$，对应的代码如下：

  cv::transpose(matA, matAt);
  matAtA = matAt * matA;
  matAtB = matAt * matB;
  cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
  

  关于退化的处理：
  什么是退化：当机器人运动在某些方向上缺乏约束时（如长廊中，lidar无法确定前后的运动），$\mathbf{J}$矩阵会在对应的列上接近于0。这导致${\mathbf{J}}^{\mathbf{T}}\mathbf{J}$矩阵奇异（或接近奇异），此时求解的$\Delta \mathbf{x}$会变得不稳定，可能会导致在优化在无约束的方向上发散。对应的：方程${\mathbf{J}}^{\mathbf{T}}\mathbf{J}=-{\mathbf{J}}^{\mathbf{T}}\mathbf{f}$只有在$\mathbf{H}={\mathbf{J}}^{\mathbf{T}}\mathbf{J}$矩阵可逆（满秩）时才有唯一解。
  怎么检查是否退化？
  对$\mathbf{H}$矩阵进行特征值分解，如果它接近奇异，它至少会有一个非常小的特征值。
  对应的代码：

  cv::eigen(matAtA, matE, matV); // 对H矩阵特征值分解
  matV.copyTo(matV2);
  isDegenerate = false;
  float eignThre[6] = {100, 100, 100, 100, 100, 100};
  for (int i = 5; i >= 0; i--) {if (matE.at<float>(0, i) < eignThre[i]) { //判断是否存在特征值小于阈值for (int j = 0; j < 6; j++) {matV2.at<float>(i, j) = 0;}isDegenerate = true;} else {break;}matP = matV.inv() * matV2;if (isDegenerate){cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}
  }
  

  检查到退化怎么处理：
  cv::eigen(matAtA, matE, matV);: $\mathbf{H}=\mathbf{VE}{\mathbf{V}}^T$。$\mathbf{V}$ 是特征向量矩阵，$\mathbf{E}$ 是特征值对角矩阵。
  matV2.at<float>(i, j) = 0;：将 matV 中与小特征值（退化方向）对应的特征向量（matV的行）置零，得到 matV2。
  matP = matV.inv() * matV2;：(假设matV是正交的，matV.inv() $\approx$ matV.transpose())。这个矩阵 $\mathbf{P}$ 是一个投影矩阵。它会将一个向量投影到由“好”的特征向量（非退化方向）所张成的子空间上。
  matX = matP * matX2;：matX2 是未受约束的解 $\Delta \mathbf{x}$。通过左乘 $\mathbf{P}$，我们将解投影到安全、有约束的子空间上。这移除了解中沿着退化方向的分量，使优化保持稳定。

  最后更新位姿：

  transformTobeMapped[0] += matX.at<float>(0, 0);
  transformTobeMapped[1] += matX.at<float>(1, 0);
  transformTobeMapped[2] += matX.at<float>(2, 0);
  transformTobeMapped[3] += matX.at<float>(3, 0);
  transformTobeMapped[4] += matX.at<float>(4, 0);
  transformTobeMapped[5] += matX.at<float>(5, 0);
  

  判断是否收敛：

  float deltaR = sqrt(pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));
  float deltaT = sqrt(pow(matX.at<float>(3, 0) * 100, 2) +pow(matX.at<float>(4, 0) * 100, 2) +pow(matX.at<float>(5, 0) * 100, 2));
  if (deltaR < 0.05 && deltaT < 0.05) {return true; // converged
  }
  

  transformUpdate

• saveKeyFramesAndFactor

• correctPoses

• publishOdometry

• publishFrames

• imuRollInit和initialGuessRoll的区别
  imuRollInit来自IMU原始数据，是一个未经系统优化的独立的姿态
  initialGuessRoll来自lio-sam融合后的高频里程计，经过系统矫正的高精度位姿估计

5.3. gpsHandler

略

5.4. loopInfoHandler

这里和内部的回环检测的线程容易混淆，其实这里是给外部回环检测预留的接口，和内部回环检测时独立的。

5.5. loopClosureThread

这里是一个独立线程用于进行回环检测，通过icp进行匹配，然后添加到因子图中。

5.6. visualizeGlobalMapThread

略