lio-sam阅读理解_publaserodometryincremental

8.刚才在看nav_msgs::Odomtry 里面有pose 也有 twist ，忽然感觉这个东西就和惯导很像，第一层是传感器数据，第二层是姿态数据。通过因子图把第一层的数据求出一个姿态增量，再叠加到第二层上。
icp匹配出来的在最后latestEstimate，然后赋值给了transformTobeMapped，那从那里到的第二层呢？这个transformTobeMapped 看起来是第二层的呀
在publishOdometry() 函数中，pubLaserOdometryIncremental 看起来发布的是里程计的增量。

问：cloudKeyPoses3D保存的是什么？
	cloudKeyPoses6D保存的是什么？
	
有这么几个变量：
    // 当前帧位姿
Eigen::Affine3f transPointAssociateToMap;
// 前一帧位姿
Eigen::Affine3f incrementalOdometryAffineFront;
// 当前帧位姿
Eigen::Affine3f incrementalOdometryAffineBack;
		

updateInitialGuess() 在这个函数中有想要的结果-->然后就挖到了cloud_info 这个消息格式，发现这个消息格式是贯穿整个工程的。然后就找到了imageProject.cpp 中的

    // 用当前激光帧起始时刻的imu里程计，初始化lidar位姿，后面用于mapOptmization
    cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x;//*这里是不是可以用wheelodom?* 优化无非是优化前端，和加变量优化后端？
    cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y;//订阅imu里程计，由imuPreintegration积分计算得到的每时刻imu位姿
    cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z;
    cloudInfo.initialGuessRoll  = roll;
    cloudInfo.initialGuessPitch = pitch;
    cloudInfo.initialGuessYaw   = yaw;
 问：initialGuess 相对位姿变换，平移增量后面怎么用到的？
答：
    Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;       //这个可以理解从initialGuess得到的位姿增量
            // 前一帧的位姿
            Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped); //这里是前一帧的最后位姿（map）
            // 当前帧的位姿
            Eigen::Affine3f transFinal = transTobe * transIncre;                     //估计出一个map坐标系下的全新位姿
   问：transFinal 这个有什么用？
     答：  // 更新当前帧位姿transformTobeMapped
            pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                          transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
                                                          
       后面就是提取特征点、加入局部map,降采样，scan2map 匹配更新位姿到transformTobeMapped，这里要去看一下transformTobeMapped怎么处理的？
答;在scan2map的过程中，对这个进行了优化，注意这里是累加的：
	        // 更新当前位姿 x = x + delta_x
    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);
  问：这样累加行不行？t-1 时刻 ，这个transformTobeMapped是map下的位姿，在t时刻，先做了一个预估initialGuess，得到了transformTobeMapped，又加上了点云匹配的matX，
  问题就在这里，之前以为是 有了map坐标系下的 lidar_odom IMU_odom  可能有GPS_odom，以及我们要加入的wheel_odom，这几个odom去做一个因子图。
  现在发现 lidar_odom和 IMU_odom不是并列关系，是一个紧耦合的关系，IMU_odom提供给了lidar_odom在t时刻的一个初始位姿，然后在这个基础上又通过点云匹配得到了了最终的lidar_odom，后面进行因子图优化。

transformTobeMapped  //yuphe 20230524 要测试 这里的transformTobeMapped 是t时刻的map位姿

后面又进行了   transformUpdate();
 // 用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll、pitch，约束z坐标
     
   我感觉要开一个专栏了。

按照惯导的说法，是在body系下面有个t-1的坐标系，然后加上了imu的数值，得到t时刻的一个预测值，然后估计imu的误差，得到t时刻在body系下的一个值。
那么，点云匹配的matX 这个是不是增量，或者把两帧之间的增量自己写一下，然后发布出来？
另外，我们使用的IMU并没有方差数据：
  // 如果起止时刻对应imu里程计的方差不等，返回
    if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))
        return;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59