-
进入了odometryHandler(),得到第一个lidar位姿T1. 先初始化系统, 丢掉比lidar位姿较早的imu数据(imuQueOpt里的), key=1(即此时为第一帧)
-
首先要取出两个lidar位姿之间(即两帧之间)的imu数据, 由于我们在初始化时(即key=1帧)已经把T1之前的imu数据给pop出去了,所以只要把T2之前的imu数据取出来就是两帧之间的imu数据了(这里要仔细想一下). 并且我们要取一个就pop一个(这和我们在初始化阶段pop掉T1之前的数据是一回事),这是为了保证在下一帧能顺利取出T2和T3之间的imu数据
-
将两帧之间的imu数据送入预积分器(优化用的imuIntegratorOpt_ ,后面还有另一个), 并构建因子图约束, 这时的图模型只有T1,T2以及T1和T2之间的△imu预积分约束. 模型有了就可以做优化了,至于怎么优化的不用我们管, gtsam会帮我们做这件事. 把优化过的最佳估计值记录下来,后面要用. prevState_是优化过的T2(就是lidar的位姿和imu得到的速度,), prevBias是优化过的零偏. 这两个变量很重要, 我们接下来要用到
-
下一步就是把另一个队列imuQueImu中早与lidar位姿T2的imu数据pop出去, 并且用刚得到的优化过的零偏prevBiasOdom(就是prevBias)复位另一个预积分器imuIntegratorImu_ ,这个预积分器会在下一次进到imuHandler()发挥作用. 这时才把doneFirstOpt置成true
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
std::lock_guard<std::mutex> lock(mtx);
double currentCorrectionTime = ROS_TIME(odomMsg);
// make sure we have imu data to integrate
// 确保imu队列中有数据
if (imuQueOpt.empty())
return;
// 获取里程计位姿
float p_x = odomMsg->pose.pose.position.x;
float p_y = odomMsg->pose.pose.position.y;
float p_z = odomMsg->pose.pose.position.z;
float r_x = odomMsg->pose.pose.orientation.x;
float r_y = odomMsg->pose.pose.orientation.y;
float r_z = odomMsg->pose.pose.orientation.z;
float r_w = odomMsg->pose.pose.orientation.w;
// 该位姿是否出现退化
bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;
// 把位姿转化成gtsam格式
gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));
// 0. initialize system
// 初始化系统
if (systemInitialized == false)
{
// 优化问题复位
resetOptimization();
// pop old IMU message
// 将之前的imu消息丢掉
while (!imuQueOpt.empty())
{
if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
{
lastImuT_opt = ROS_TIME(&imuQueOpt.front());
imuQueOpt.pop_front();
}