LIO-SAM 详读代码笔记--2.imuPreintegration

发布于:2023-01-20 ⋅ 阅读:(586) ⋅ 点赞:(0)

先把系统topic 数据流图搬过来,能更加清晰的理解这个模块。

图2.ros topic 发布与订阅关系图关系图(还是叫图2,因为是同一个图)

这个cpp文件主要有两个类,一个叫IMUPreintegration类,一个叫TransformFusion类。从数据流向和依赖程度的角度分析,应该先分析IMUPreintegration类,但是TransformFusion类写在该模块的前头,且这两个模块的数据流相互依赖,这里先分析TransformFusion类。

TransformFusion类

讲解之前,二话不说,先上该类的成员变量列表。

变量名 类型 注释 备注
mtx std::mutex 因为ros回调函数是异步运行,在回调函数操作成员变量或者队列时,要先上锁 -
subImuOdometry ros::Subscriber 订阅"odometry/imu_incremental" 虽然名字上有incremental ,但里程计,他就是里程计,代表着该时刻固定的frame 的位姿信息,由下面的imuPreintegration 发出, 虽然 由 gtsam 的预积分器算出来,但完全可以理解为不与预积分有关系,单纯文imu 数据的位姿积分解算 在这个类中 其中一个主要的目的就是 在优化过后的lidar 里程计基础上,叠加上lidar时刻到至今时刻的位姿差,以发出更高频率的里程计定位数据 -
subLaserOdometry ros::Subscriber 订阅"lio_sam/mapping/odometry" 就是经过MapOptimization优化后的里程计信息,接收周期比慢一个量级,与 "odometry/imu_incremental" 的 frameid 应该是一致的,都是 odomframe,child_frame_id 都是odom_lidar

注意,这里/mapping/odometry和/mapping/odometry_incremental的区别,前者是激光里程计部分直接优化得到的激光位姿,后者根据运动模型 计算的超前一个lidar 周期的里程计信息。

-
pubImuOdometry ros::Publisher 发布imu 里程计 "odometry/imu" 结合了lidar 里程计的IMU 里程计# IMU pre-preintegration odometry, same frequency as IMU -
pubImuPath ros::Publisher 消息名为 "lio_sam/imu/path" ,在图2 中由 rviz 订阅,其发布imu 里程计 "odometry/imu" 对应的轨迹,发布频率是10hz,每次发布都只是一小段,即lidar时刻到当前imu 时刻的位姿数组 -
lidarOdomAffine Eigen::Affine3f 就是把"lio_sam/mapping/odometry" 的消息转化为位姿变换矩阵,代表但前lidar时刻 (机器人)的位姿 -
lidarOdomAffine Eigen::Affine3f 就是把"lio_sam/mapping/odometry" 的消息转化为位姿变换矩阵,代表但前lidar时刻 的位姿 -
imuOdomAffineFront Eigen::Affine3f 在loam 系列代码里看到这一对 front /back 的变换矩阵,就应该想到作差 相对位姿变换,这个变换矩阵是由lidar 时刻的最近的imu时刻的"odometry/imu_incremental" 的里程计消息转化而来,代表那个时刻的位姿 -
imuOdomAffineBack Eigen::Affine3f 这个变换矩阵是由当前时刻imu时刻的"odometry/imu_incremental" 的里程计消息转化而来,代表那个时刻的位姿,作用是 与imuOdomAffineFront 作差后,附加到lidarOdomAffine变换上,得到结合地图优化的位姿的更高频率的位姿信息 -
tfListener tf::TransformListener 从tf 树中得到 lidar2Baselink 的外参关系 -
lidar2Baselink tf::StampedTransform 如果imu里程计与lidar 里程计的坐标系不是同一个坐标系,需要转化到同一个坐标系两个里程计数据才能融合 -
lidarOdomTime float lidar时刻的时间戳,每次lidar 里程计来了都更换一次,imu 里程计队列imuOdomQueue的数据,都保留这个时刻之后的数据 -
imuOdomQueue deque imu 里程计队列,都保留lidar时刻开始之后的数据,便于寻找lidar时刻imu的里程计,和当前时刻里程计,作差,附加到lidar 里程计中去 -

纵观TransformFusion类,该类只干一件事,就是融合lidar里程计(经过mapOptimization 优化过的)和imu 积分的里程计,以比lidar里程计更高的频率分布出去,一个是odometry/imu,一个是odom 到 baselink 的tf 关系,这个就是整个系统的主要输出。

主要的处理逻辑在imu 里程计的回调函数中,imuOdometryHandler,在函数里面监听的是"odometry/imu_incremental"(在下面imuPreintegration中发出,是imu 独立解算的里程计),把imu里程计放到imuodomQueue里面保存,然后把lidarOdomTime之前的数据扔掉,之后用最新和最旧时刻的mu里程计的计算差异,如图3所示,再加上lidarOdomAffine的基础进行发布。

另外还会发布一个path,用来rviz显示,名字叫lio-sam/imu/path。

/**
 * 激光雷达里程计就保存当前的雷达里程计的数据到lidarOdomAffine里面,
 * 把时间戳存到lidarOdomTime里面去,没干别的,
 * 具体的主要逻辑在imu 里程计的回调函数中处理。
 * 
 * */
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
        std::lock_guard<std::mutex> lock(mtx);

        lidarOdomAffine = odom2affine(*odomMsg);

        lidarOdomTime = odomMsg->header.stamp.toSec();
    }

/**
 * 主要的处理逻辑在imu 里程计的回调函数中,`imuOdometryHandler`,
 * 在函数里面监听的是/imu/odometry_incremental(在下面imuPreintegration中发出,
 * 是imu 独立解算的里程计),把imu里程计放到imuodomQueue里面保存,
 * 然后把lidarOdomTime之前的数据扔掉,之后用最新和最旧时刻的mu里程计的计算差异,
 * 再加上lidarOdomAffine的基础进行发布。
 * */
    void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
        // static tf  这个地方究竟是在干什么?
        static tf::TransformBroadcaster tfMap2Odom;
        static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
        tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));

        std::lock_guard<std::mutex> lock(mtx);

        imuOdomQueue.push_back(*odomMsg);

        // get latest odometry (at current IMU stamp)
        if (lidarOdomTime == -1)
            return;
        while (!imuOdomQueue.empty())
        {
            if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
                imuOdomQueue.pop_front();
            else
                break;
        }
        Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());//前
        Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());// 后
        Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
        Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
        
        // publish latest odometry
        nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
        laserOdometry.pose.pose.position.x = x;
        laserOdometry.pose.pose.position.y = y;
        laserOdometry.pose.pose.position.z = z;
        laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
        pubImuOdometry.publish(laserOdometry);

        // publish tf
        static tf::TransformBroadcaster tfOdom2BaseLink;
        tf::Transform tCur;
        tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
        if(lidarFrame != baselinkFrame)
            tCur = tCur * lidar2Baselink;
        tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
        // 发布 odom 到 baselink 的tf 关系
        tfOdom2BaseLink.sendTransform(odom_2_baselink);

        // publish IMU path
        static nav_msgs::Path imuPath;  // 静态数据,Path 实际是pose数组,
        static double last_path_time = -1;
        double imuTime = imuOdomQueue.back().header.stamp.toSec();
        // 控制发送频率 10HZ
        if (imuTime - last_path_time > 0.1)
        {
            last_path_time = imuTime;
            geometry_msgs::PoseStamped pose_stamped;
            pose_stamped.header.stamp = imuOdomQueue.back().header.stamp;
            pose_stamped.header.frame_id = odometryFrame;
            pose_stamped.pose = laserOdometry.pose.pose;
            imuPath.poses.push_back(pose_stamped);
            // lidar 时间戳1秒钟以内的数据,以前的数据删掉
            while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
                imuPath.poses.erase(imuPath.poses.begin());
            if (pubImuPath.getNumSubscribers() != 0)
            {
                imuPath.header.stamp = imuOdomQueue.back().header.stamp;
                imuPath.header.frame_id = odometryFrame;
                pubImuPath.publish(imuPath);
            }
        }
    }

 

IMUPreintegration 类

成员变量列表:

变量名 类型 注释 备注
mtx std::mutex 因为ros回调函数是异步运行,在回调函数操作成员变量或者队列时,要先上锁 -
subImu ros::Subscriber //订阅"imu_raw",imu源数据 -
subOdometry ros::Subscriber

订阅 "lio_sam/mapping/odometry_incremental" 这个消息是从MapOptimization 发布,经过地图优化且通过匀速运动模型 计算的朝一个lidar 优化周期的里程计信息

-
pubImuOdometry ros::Publisher 发布 "odometry/imu_incremental" 实际 TransformFusion是其中一个订阅者,在一个订阅者是imageProjection 虽然 frame_id = odometryFrame;child_frame_id = "odom_imu"; 但代码里实现应该是“odom_lidar”,因为IMU的数据转换成与Lidar坐标系同方向的数据,最后又右乘一个变换(imu2Lidar 实际不是) ;即T_ol = T_oi*T_il -
systemInitialized bool 初始化标志,因子图优化器初始化完成后,就设置为true,或者积分出来的数据超过阈值,需要重新因子图优化器初始化 -
预积分因子模型噪声模型参数 -
priorPoseNoise gtsam::noiseModel::Diagonal::shared_ptr 位姿噪声模型 -
priorVelNoise gtsam::noiseModel::Diagonal::shared_ptr 速度噪声模型 -
priorBiasNoise gtsam::noiseModel::Diagonal::shared_ptr 偏差噪声模型 -
correctionNoise gtsam::noiseModel::Diagonal::shared_ptr 激光雷达位姿噪声模型 -
correctionNoise2 gtsam::noiseModel::Diagonal::shared_ptr 激光雷达位姿噪声模型,协方差是否为单位阵,决定使用哪个噪声模型 -
noiseModelBetweenBias gtsam::Vector 两帧的bias 形成一个betweenFactor, 也需要提供一个噪声模型 -
*imuIntegratorOpt_ gtsam::PreintegratedImuMeasurements 预积分器,这个预积分提供预积分数据给因子图优化器 -
*imuIntegratorImu_ gtsam::PreintegratedImuMeasurements 预积分器,这个预积分提供预积分数据给里程计累加用 -
imuQueOpt std::deque 预积分器对应的imu积分数据队列 -
imuQueImu std::deque 预积分器对应的imu积分数据队列 -
prevPose_ gtsam::Pose3 上一帧lidar数据到来时因子图优化后的位姿 -
prevVel_ gtsam::Vector3 上一帧lidar数据到来时因子图优化后的速度 -
prevState_ gtsam::NavState 上一帧lidar数据到来时因子图优化后的状态,包含pose、vel -
prevBias_ gtsam::imuBias::ConstantBias 上一帧lidar数据到来时因子图优化后的IMU偏差 -
prevStateOdom gtsam::NavState 上一帧lidar数据到来时因子图优化后的状态,包含pose、vel;从prevState_ 赋值 -
prevBiasOdom gtsam::imuBias::ConstantBias 上一帧lidar数据到来时因子图优化后的IMU偏差;从prevBias_ 赋值 -
doneFirstOpt bool 只有经过因子图优化后,才进行imuIntegratorImu_预积分累积里程计 -
lastImuT_imu double 上一次的imu 的时间戳,当前帧imu 来时,用当前imu时间戳 减去 这个值,得到dt,imu积分时需要这个值 -
lastImuT_opt double 意思同上,就是对应不同的预积分器而已 -
optimizer gtsam::ISAM2 因子图优化器,这里用ISAM2作为优化器的后端 -
graphFactors gtsam::NonlinearFactorGraph 因子图容器,用于添加各种因子 -
graphValues gtsam::Values 因子图中各个节点,也就是待优化的变量 -
delta_t const double 常量,距离lidar 时间戳的距离,这里固定为0 -
key int 计数器,每100次 重新初始化 因子图优化器,这个跟gtsam的实现有关系,每次优化保留一些东西在贝叶斯树里头,多了会影响速度 -
key int 计数器,每100次 重新初始化 因子图优化器,这个跟gtsam的实现有关系,每次优化保留一些东西在贝叶斯树里头,多了会影响速度 -
imu2Lidar gtsam::Pose3 原文注释:T_bl: tramsform points from lidar frame to imu frame 这个注释跟命名感觉不对劲,应该表示成Lidar2Imu ;在params.yaml 文件中 # Extrinsics: T_lb (lidar -> imu) 个人觉得也有问题 应该表示成T_lb (imu -> lidar) 但是 包括下边的两个变量的命名 应该是反着来的 -
lidar2Imu gtsam::Pose3 原文注释:T_bl: T_lb: tramsform points from imu frame to lidar frame 这个注释跟命名感觉不对劲,应该表示成imu2Lidar ,这个注释没有说到点子上,在整个系统中这两个变量只有在里程计变换中用到,而里程计变换的计算跟坐标系上的点坐标变换是有区别,具体看代码处的注释 -

关于IMU原始数据,送入imuhandle中:

1.1 imuhandle:

1.1.1 imu原始数据,会先被坐标系转换,通过调用头文件里的imuConverter函数,转换到一个“雷达中间系”中,其实和真正的雷达系差了一个平移。这里的操作就像对IMU 的数据进行修正,让IMU的坐标系与Lidar 坐标系的方向一致,然后 IMU 与 Lidar外参就只有位移关系了。乍一看,感觉作者故意写复杂了,但是个人猜想,1.由于IMU数据里面没有位移数据,不能进行直接变换到Lidar坐标系;2.速度变化很小,进行位移变换后是否影响积分精度。

1.1.2 转换后,会存入两个队列,一个imuQueOpt队列,在另一个回调函数odometryHandler会被处理,用于预积分因子源数据,用来优化系统状态(R,P,V,B)和imu的bias。一个imuQueImu队列,imuQueImu是这里真正要用的数据,每到一个imu数据,就用imuIntegratorImu_这个预积分器,把这一帧imu数据累积进去,然后预测当前时刻的状态:currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom); 其中prevStateOdom就是之前系统所保持的状态。

1.1.3 把currentState,通过imu2Lidar(其实是Lidar2Imu T_il)T_ol = T_oi * T_il,把IMU 积分得到的里程计转成真正的雷达里程计,然后发布出去。发布的话题就叫odometry/imu_incremental(实际是lidar 里程计),这也就是imageProjection.cpp的总结部分的第2点部分提到的“imu”里程计。虽然作者的变量命名很疑惑,但是代码实现得还是很老实的。

void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
    {
        std::lock_guard<std::mutex> lock(mtx);
        // 这个函数这个地方写的非常绕,非值得注意的是,这里是把imu的数据转换到lidar坐标系下,
        // 但是又不完全转,只是保持与lidar坐标系的方向一致,差一个位移
        // 这里的操作就像对IMU 的数据进行修正,让IMU的坐标系与Lidar 坐标系的方向一致,
        // 然后 IMU 与 Lidar外参就只有位移关系了。
        sensor_msgs::Imu thisImu = imuConverter(*imu_raw);  
        // 转换后的数据压入队列中
        imuQueOpt.push_back(thisImu);
        imuQueImu.push_back(thisImu);
        // 如果因子图还没有优化过,就直接返回了
        if (doneFirstOpt == false)
            return;

        // 获取该IMU数据的时间戳
        double imuTime = ROS_TIME(&thisImu);
        // 与上一帧IMU 数据时间戳作差,
        double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
        lastImuT_imu = imuTime;

        // 把当前帧的IMU 数据添加入预积分器
        // integrate this single imu message
        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);

        // 以上一帧lidar时刻因子图优化出来的状态为基准,预积分器预测出当前IMU时刻的位姿状态
        // predict odometry
        gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);

        // publish odometry
        nav_msgs::Odometry odometry;
        odometry.header.stamp = thisImu.header.stamp;
        odometry.header.frame_id = odometryFrame;
        // 这里实际应该是“odom_lidar”
        odometry.child_frame_id = "odom_imu";

        // transform imu pose to ldiar
        gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
        // imu2Lidar 变量的值给得应该表示成T_il,整个式子 则为 T_ol = T_oi*T_il 
        gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);

        odometry.pose.pose.position.x = lidarPose.translation().x();
        odometry.pose.pose.position.y = lidarPose.translation().y();
        odometry.pose.pose.position.z = lidarPose.translation().z();
        odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
        odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
        odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
        odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
        
        odometry.twist.twist.linear.x = currentState.velocity().x();
        odometry.twist.twist.linear.y = currentState.velocity().y();
        odometry.twist.twist.linear.z = currentState.velocity().z();
        odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
        odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
        odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
        pubImuOdometry.publish(odometry);
    }

1.2. odomHandle:这部分订阅的是/lio_sam/mapping/odometry_incremental,这个话题是由mapOptmization.cpp发布的,就是经过了地图匹配优化后的位姿信息,里程计就是从odom到lidar的变换关系而不是两帧激光之间的变换,它和imu里程计性质类似,就是相对世界坐标系的位姿。

1.2.1 初始化系统:

从imuQueOpt队列里,删掉当前这帧激光里程计时间上之前的数据,取两个Lidar时刻之间的IMU 数据进行预积分累积,然后把雷达的pose变换到IMU “坐标系”(这里的IMU坐标经过了修正,方向与Lidar坐标系保持一致,外参只差一个位移),然后初始化因子图优化器,相关优化变量prevPose等。因为预积分器需要bias ,而bias每次优化都会发生变化,所以需要重新设置,所以把两个预积分器imuIntegratorImu_,imuIntegratorOpt_给reset一下。(之后简称imu积分器和opt积分器)

1.2.2 清理缓存:

100帧激光里程计数据了,就把优化器给reset一下(用前面的先验协方差给reset一下)。每100 次 重新设置优化器,这个是gtsam 的实现要求的,在每次优化的过程中, gtsam 会缓存一些数据在optimizer贝叶斯树中,随着优化次数增大,该树会越来越大,执行效率就会变慢,所以要周期性清理。 在清理之前,需要把优化变量的协方差给保存起来,给新的优化器使用,在最开始的时候 每个因子的噪声模型都是人工经验值,这里需要把协方差矩阵传递下去,给新的优化器

1.2.3 正式处理:

1.2.3.1 把两个Lidar 时刻的IMU数据拿出来,送入opt积分器。这样得到两个时刻之间的IMU预积分,构建imu因子。

1.2.3.2 然后把关联Xkey-1 和Xkey的IMU 因子加入因子图 以及 激光里程计提供的pose先验因子,还有Bias BetweenFactor,整体做一个优化。优化的结果就是当前Lidar时刻的状态量(R,P,V,B)。

1.2.3.3 把优化的结果(主要是bias),重置opt积分器和imu积分器。由于imuIntegratorImu_被重置了BIAS ,需要对之前的IMU 数据进行重新积分,并删掉使用过的数据,这就是在imuHandler 中积分却不删数据的原因。

/**
 * @brief 激光雷达里程计的回调函数
 * 
 * @param odomMsg 
 */
    void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
    {
        std::lock_guard<std::mutex> lock(mtx);
        // 获取lidar 时间戳
        double currentCorrectionTime = ROS_TIME(odomMsg);

        // make sure we have imu data to integrate
        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;
        // 是否退化,什么情况下会退化? 初始情况下odomMsg->pose.covariance[0] == 1 
        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));

        // 初始化
        // 0. initialize system
        if (systemInitialized == false)
        {
            // 重置Isam2 优化器
            积分
        // 优化器
        // optimizer = gtsam::ISAM2(optParameters);
        // 因子图容器
        // graphFactors = newGraphFactors;
        // 待优化的变量
        // graphValues = NewGraphValues;
            resetOptimization();

            // pop old IMU message
            while (!imuQueOpt.empty())
            {
                // imuQueOpt数据中小于lidar时刻的数据全部pop 出去
                if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
                {
                    lastImuT_opt = ROS_TIME(&imuQueOpt.front());
                    imuQueOpt.pop_front();
                }
                else
                    break;
            }
            // 因子图是贝叶斯公式推导过来的,P(x|z) = a*P(z|x)P(x) 其中a 为系数,P(z|x)为似然函数,P(x) 则为先验
            // 所以 因子图里都要添加进 先验P(x)
            // 这里把 因子图中第一个优化变量设为先验
            // initial pose
            prevPose_ = lidarPose.compose(lidar2Imu);// compose 是什么运算??转到IMU 系 todo:T_oi = T_ol * T_li
            gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
            graphFactors.add(priorPose);
            // initial velocity
            prevVel_ = gtsam::Vector3(0, 0, 0);
            gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
            graphFactors.add(priorVel);
            // initial bias
            prevBias_ = gtsam::imuBias::ConstantBias();
            gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
            graphFactors.add(priorBias);
            // add values
            graphValues.insert(X(0), prevPose_);
            graphValues.insert(V(0), prevVel_);
            graphValues.insert(B(0), prevBias_);
            // optimize once
            optimizer.update(graphFactors, graphValues);
            graphFactors.resize(0);
            graphValues.clear();

            //预积分器需要bias ,而bias每次优化都会发生变化,所以需要重新设置
            imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
            imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
            
            key = 1;
            systemInitialized = true;
            return;
        }

        // 每100 次 重新设置优化器,这个是gtsam 的实现要求的,在每次优化的过程中,
        // gtsam 会缓存一些数据在贝叶斯树中,随着优化次数增大,该树会越来越大,执行效率就会变慢,
        // 所以要周期性清理
        // reset graph for speed
        if (key == 100)
        {
            // 在最开始的时候 每个因子的噪声模型都是人工经验值,这里需要把协方差矩阵传递下去,给新的优化器
            // get updated noise before reset
            gtsam::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 graph
            resetOptimization();
            // 新的因子图需要新的先验
            // add pose
            gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
            graphFactors.add(priorPose);
            // add velocity
            gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
            graphFactors.add(priorVel);
            // add bias
            gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
            graphFactors.add(priorBias);
            // add values
            graphValues.insert(X(0), prevPose_);
            graphValues.insert(V(0), prevVel_);
            graphValues.insert(B(0), prevBias_);
            // optimize once 预热一次
            optimizer.update(graphFactors, graphValues);
            graphFactors.resize(0);
            graphValues.clear();

            key = 1;
        }

        // 从这里开始 真正处理数据因子图优化了
        // 1. integrate imu data and optimize
        while (!imuQueOpt.empty())
        {
            // pop and integrate imu data that is between two optimizations
            sensor_msgs::Imu *thisImu = &imuQueOpt.front();
            double imuTime = ROS_TIME(thisImu);
            // 取两个lidar 时间戳之间的IMU 数据提供给IMU积分器进行积分,
            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(); // 用一个 删一个,每一次优化周期内,没有上一周期的IMU数据
            }
            else
                break;
        }
        // add imu factor to graph
        const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
        // IMU 因子 提供 K-1 时刻的状态变量 与K 时刻的状态变量,与预积分器 
        // 在gtsam::ImuFactor 中定义好了 残差计算和雅可比的计算
        gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
        graphFactors.add(imu_factor);
        // BIAS 残差 用gtsam::BetweenFactor 因子,表示两帧lidar时刻的BIAS 应该保持一致,对因子图进行约束
        // add imu bias between factor
        graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
                         gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
        // lidar 位姿约束因子只跟当前 状态量有关系,属于一个先验因子
        // add pose factor
        gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
        gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
        graphFactors.add(pose_factor);
        // 因子相当于给因子图优化问题提供了观测值,
        // 还需要给优化变量提供初始值
        // 以上一时刻的状态为基准,从预积分器中预测当前的状态值,作为因子图优化变量的初值
        // insert predicted values
        gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
        graphValues.insert(X(key), propState_.pose());
        graphValues.insert(V(key), propState_.v());
        graphValues.insert(B(key), prevBias_);
        // optimize
        optimizer.update(graphFactors, graphValues);
        optimizer.update();
        // 优化完之后,因子图把相关约束 添加到优化器optimizer的贝叶斯树中,所以在因子图上可以删除
        graphFactors.resize(0);
        graphValues.clear();
        // Overwrite the beginning of the preintegration for the next step.
        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));
        // Reset the optimization preintegration object.
        imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
        // check optimization
        if (failureDetection(prevVel_, prevBias_))
        {
            resetParams();
            return;
        }

        //因子图优化完之后,得到当前优化状态,需要设置imuIntegratorImu_ 的BIAS 重新预积分

        // 2. after optiization, re-propagate imu odometry preintegration
        prevStateOdom = prevState_;
        prevBiasOdom  = prevBias_;
        // first pop imu message older than current correction data
        double lastImuQT = -1;
        while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
        {
            lastImuQT = ROS_TIME(&imuQueImu.front());
            imuQueImu.pop_front();
        }
        // repropogate
        if (!imuQueImu.empty())
        {
            // reset bias use the newly optimized bias
            imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
            // integrate imu message from the beginning of this optimization
            for (int i = 0; i < (int)imuQueImu.size(); ++i)
            {
                sensor_msgs::Imu *thisImu = &imuQueImu[i];
                double imuTime = ROS_TIME(thisImu);
                double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);

                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);
                lastImuQT = imuTime;
            }
        }

        ++key;
        doneFirstOpt = true;
    }

本文含有隐藏内容,请 开通VIP 后查看