看过了main函数，我们来看mapOptmization的正题：
MO.run();
void run(){
if (newLaserCloudCornerLast && std::abs(timeLaserCloudCornerLast - timeLaserOdometry) < 0.005 &&
newLaserCloudSurfLast && std::abs(timeLaserCloudSurfLast - timeLaserOdometry) < 0.005 &&
newLaserCloudOutlierLast && std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&
newLaserOdometry)
//函数开始时，会检查是否收到了新的激光雷达角点云数据 (newLaserCloudCornerLast)、
表面点云数据 (newLaserCloudSurfLast)、离群点云数据 (newLaserCloudOutlierLast)
和里程计数据 (newLaserOdometry)。同时，还会检查这些数据的时间戳与最新的里程计数据的
时间戳之间的差异是否小于0.005秒。这是为了确保所有数据都是在相近的时间点采集的，
从而可以认为是同步的。
{
newLaserCloudCornerLast = false; newLaserCloudSurfLast = false; newLaserCloudOutlierLast = false; newLaserOdometry = false;
//如果所有数据都已就绪，会将所有新数据的标志位重置为 false，表示这些数据已经被处理。
std::lock_guard<std::mutex> lock(mtx);
//使用 std::lock_guard 和 std::mutex 来保护共享数据，确保线程安全。
//这是为了防止多个线程同时访问和修改共享数据，可能会导致数据不一致。
if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {
//extern const double mappingProcessInterval = 0.3;
//检查自上次处理以来是否已经过了设定的时间间隔 (mappingProcessInterval)。
//这是为了避免过于频繁的处理，从而节省计算资源。
timeLastProcessing = timeLaserOdometry;
//更新时间，为下一次判断做准备
transformAssociateToMap();
extractSurroundingKeyFrames();
downsampleCurrentScan();
scan2MapOptimization();
saveKeyFramesAndFactor();
correctPoses();
publishTF();
publishKeyPosesAndFrames();
clearCloud();
}
}
}下面我们来看第一个函数，transformAssociateToMap();
transformAssociateToMap();
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;
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]));
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);
}我们要尝试理解transformAssociateToMap();就要先理解：transformSum，transformBefMapped，transformAftMapped，transformTobeMapped分别是什么？
transformSum是当前帧雷达里程计的结果，transformBefMapped是上一帧雷达里程计的结果。
这在transformUpdate()函数中有所体现。transformAftMapped是上一帧优化后的位姿，transformTobeMapped是当前帧优化后的位姿。
第一次运行时，除transformSum，其他均初始化为0。
for (int i = 0; i < 6; i++) {
transformBefMapped[i] = transformSum[i];
transformAftMapped[i] = transformTobeMapped[i];
}所以要得到当前帧优化后的位姿，就要用上一帧优化后的位姿乘上一帧雷达里程计结果的逆，再乘当前帧雷达里程计的结果。公式如下：
参考自LOAM中的坐标变化公式推导_loam公式推导-CSDN博客
从xyz1到transformIncre的过程就对应当前帧雷达里程计结果的逆乘（上一帧雷达里程计的平移量-当前帧雷达里程计的平移量）。公式如下：
我的理解就是，当前的平移量差值△r，是在上一次位姿优化后，由于位姿变换产生的，我们先把它变换回上一次的位姿，也就是左乘 当前帧雷达里程计结果的逆，然后再乘transformTobeMapped，变换到当前帧优化后的位姿上。
transformSum[0] = -pitch;
transformSum[1] = -yaw;
transformSum[2] = roll;
//那为什么公式中没有求逆的过程呢？对于旋转矩阵来说，yaw-pitch-roll的逆就是按roll-pitch-yaw顺序旋转。transformTobeMapped 的公式推导就是矩阵乘法的展开式，类似于PluginIMURotation的公式推导，用恒等关系解决，具体推导见：LOAM中的坐标变化公式推导_loam公式推导-CSDN博客
extractSurroundingKeyFrames();
void extractSurroundingKeyFrames(){
if (cloudKeyPoses3D->points.empty() == true)
return;
}
only use recent key poses for graph building只使用临近帧的位姿去建图。
初始的cloudKeyPoses3D就是空的，它的值会在saveKeyFramesAndFactor()函数中更新，所以可以把cloudKeyPoses3D简单理解为上一帧的点云。
！！但是其实cloudKeyPoses3D不是点云，它只是一个上一帧机器人的位置，但是这个位置可以代替一帧点云，这样运行时就大大减少了内存使用！！
thisPose3D.x = latestEstimate.translation().y();
thisPose3D.y = latestEstimate.translation().z();
thisPose3D.z = latestEstimate.translation().x();
thisPose3D.intensity = cloudKeyPoses3D->points.size(); // this can be used as index
cloudKeyPoses3D->push_back(thisPose3D);
当 extractSurroundingKeyFrames() 中执行 return 后，run() 函数中的代码会继续执行，并不会因为 extractSurroundingKeyFrames() 返回就中断 run() 的执行。
具体来说，extractSurroundingKeyFrames() 函数中如果 cloudKeyPoses3D->points.empty() 为 true，会直接返回，跳过当前函数的其他内容。但因为这是 extractSurroundingKeyFrames() 内部的 return，仅仅是提前结束了该函数的执行，不会影响 run() 中其后的代码执行。
所以在 extractSurroundingKeyFrames() 执行 return 后，run() 函数会继续执行 downsampleCurrentScan()、scan2MapOptimization() 等后续函数。所以我们按cloudKeyPoses3D就是空的，先看return之后的代码，也就是下一个函数downsampleCurrentScan()；。
当cloudKeyPoses3D->points.empty() == false，我们来看下面的代码：
如果需要回环检测：loopClosureEnableFlag == true
void extractSurroundingKeyFrames(){
if (cloudKeyPoses3D->points.empty() == true)
return;
if (loopClosureEnableFlag == true){
//extern const bool loopClosureEnableFlag = false;
//默认值是false,也就是默认不进行回环检测。
// only use recent key poses for graph building
if (recentCornerCloudKeyFrames.size() < surroundingKeyframeSearchNum){
// queue is not full (the beginning of mapping or a loop is just closed)
//extern const int surroundingKeyframeSearchNum = 50;
// submap size (when loop closure enabled)
// clear recent key frames queue
recentCornerCloudKeyFrames. clear();
recentSurfCloudKeyFrames. clear();
扫一扫
1778
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
