ROS-3DSLAM（五）lvi-sam源代码阅读3

2021SC@SDUSC

（五）lvi-sam源代码阅读3

lidar_odometry

imuPreintegration.cpp

本次分析主要是对imuHandler函数的分析。

里程计的回调函数 odometryHandler

/*
然后是里程计回调函数。

每隔100帧激光里程计，重置ISAM2优化器，添加里程计、速度、偏置先验因子，执行优化。

计算前一帧激光里程计与当前帧激光里程计之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态。

优化之后，执行重传播。获得 imu 真实的 bias，用来计算当前时刻之后的 imu 预积分。
*/
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
    // 当前帧激光里程计时间戳
    double currentCorrectionTime = ROS_TIME(odomMsg);

    // make sure we have imu data to integrate
    // 确保imu优化队列中有imu数据进行预积分
    //在imuHander函数中，会将imu数据push到队列中；数据是imu原始数据经过 旋转但没平移 到雷达坐标系下的数据
    if (imuQueOpt.empty())
        return;
     当前帧激光位姿，来自scan-to-map匹配、因子图优化后的位姿
    //位置 和 方向
    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;
    //四舍五入
    //
    int currentResetId = round(odomMsg->pose.covariance[0]);
    //用订阅的激光雷达里程计消息 初始化一个lidarPose 包括一个四元数和points
    gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));

    // correction pose jumped, reset imu pre-integration
     当前里程计位姿 id 保存在协方差的第一项，如果和当前 imu 位姿 id 不同，说明有跳变，重置优化
    //跟那个图有关系（imu和lidar的发布的频率对比图 id相同代表处在同一个时间段内？？？？？？？好像不是 根据之后的 暂时判断：有若干个雷达里程计帧属于同一个id）
    //id不同 可能是因为误差太大了


    //暂时根据imuHandler中，发布odometry来看，两针激光之间的很多帧imu数据，对每一帧imu，做预积分处理，这些imu帧属于同一个imu预积分项
    // imuPreintegrationResetId 代表的是预积分的id（即id相同的里程计中的imu的数据属于同一个预积分器）
    if (currentResetId != imuPreintegrationResetId)
    {
        //如果当前的里程计已经属于 另一个时间段了，就重置参数
        //这个可能不是什么正常现象？？？？？？？？？？？？？？
        resetParams();
        imuPreintegrationResetId = currentResetId;
        return;
    }


    // 0. initialize system
    // 系统初始化，第一帧
    if (systemInitialized == false)
    {
         // 重置优化参数，也就是重置 ISAM2 优化器
        // 将 relinearizeThreshold 赋值为 0.1，将relinearizeSkip 赋值为 1
        // new 分配出新的 graphFactors 和 graphValues
        resetOptimization();

        // pop old IMU message
        // 从imu优化队列中删除当前帧激光里程计时刻之前的imu数据（将过期，即时间戳<当前激光里程计的时间戳的数据都弹出）
        while (!imuQueOpt.empty())
        {


            //delta_t是个常量 = 0
            //if 当前opt队首的元素的时间戳 < 当前激光里程计时间戳
            if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
            {
                //将队首元素的时间戳记录下来，更新lastImuT_opt ，将元素弹出
                lastImuT_opt = ROS_TIME(&imuQueOpt.front());
                imuQueOpt.pop_front();
            }
            //直到时间戳>
            else
                break;
        }
        // initial pose
        // 添加 里程计位姿先验因子
        //lidarPose.compose(lidar2Imu); 将雷达位置转换到Imu坐标系下
        prevPose_ = lidarPose.compose(lidar2Imu);
        /*
            PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) :
            Base(model, key), prior_(prior) {
            }
        */
        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
        // 添加imu偏置先验因子
        prevBias_ = gtsam::imuBias::ConstantBias();
        /*
          ConstantBias() :
            biasAcc_(0.0, 0.0, 0.0), biasGyro_(0.0, 0.0, 0.0) {
            }
        */
        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
        // 假定起始为 0 速度，进行一次优化，用于刚开始的 scan-matching
        // 对于起始速度小于 10 m/s，角速度小于 180°/s，效果都非常好
        // 但是这总归是基于 0 起始速度估计的，起始估计并不是实际情况，因此清除图优化中内容
        optimizer.update(graphFactors, graphValues);
        //我的理解：暂时理解成，用一组虚假的数据做一次优化，进行初始化；但是毕竟是假的，所以要在因子图中删掉这些数据
        graphFactors.resize(0);
        graphValues.clear();

        //重置预积分器（这个代码块内是初始化）
        imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
        imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);

        //key=1 有一帧
        //系统初始化成功
        key = 1;
        systemInitialized = true;
        return;
    }


    /*
    暂时认为：
    根据imuhandler的那个图来看，
    激光雷达里程计帧后面跟着很多imu帧，这是一个连续的过程
    在imuhandler函数中，会将每一帧imu数据添加到队列imu和opt中
    在这个函数中，会将当前的激光雷达帧的之前的imu数据帧（上一帧激光后，在opt队列中）都进行预积分，然后进行预测（imuhandler中是 进行一次预积分，就做一次预测）
    resetParam可能是因为过程中发现误差过大，需要重新进行优化了
    100帧后重置就纯粹是因为内存不够？
    */

    // reset graph for speed
    // 之后会有一个++key
    // 每隔100帧激光里程计，重置ISAM2优化器，保证优化效率，防止内存溢出
    if (key == 100)
    {
        // get updated noise before reset
        // 根据最近的下标即 99，获得前一帧的位姿、速度、偏置噪声模型 作为初始化
        //marginalCovariance 返回值是一个矩阵
        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
        //优化系统开始前，也要进行重置操作
        //这里的重置是被迫的，

//在wiki中没有找到key类，暂时理解成x(0)类似于键值对的中的key对应后面的噪声
//内部源代码瞅着贼复杂，暂时先不看的（里面一堆typedef重命名看着乱七八糟的）

        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;
        //楼上的步骤系统初始化时候的步骤相同
    }


    // 正常步骤开始

    // 如论文中所述，初始化完成后就可以估计 IMU 偏差，机器人位姿，速度
    // 计算前一帧与当前帧之间的imu预积分量，用前一帧状态施加预积分量得到当前帧初始状态估计，
    // 添加来自mapOptimization的当前帧位姿，进行因子图优化，更新当前帧状态
    // 1. integrate imu data and optimize
    while (!imuQueOpt.empty())
    {
        // pop and integrate imu data that is between two optimizations
        // 提取前一帧与当前帧之间的imu数据，计算预积分
        sensor_msgs::Imu *thisImu = &imuQueOpt.front();
        //这一帧imu的时间戳
        double imuTime = ROS_TIME(thisImu);
        // 判断这一帧是否已经超过了当前激光雷达帧的时间戳
        if (imuTime < currentCorrectionTime - delta_t)
        {
            //没超过
            // 时间差初始值为 1/500 s，否则为与上一帧的时间差
            double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
            // 此处积分只用到了线加速度和角速度
            // 加入的是这个用来 因子图优化 的预积分器imuIntegratorOpt_,注意加入了上一步算出的dt
            //作者要求的9轴imu数据中欧拉角在本程序文件中没有任何用到,全在地图优化里用到的
            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;
            //从队列中删除已经处理的imu数据
            imuQueOpt.pop_front();
        }
        else
            break;
    }
    // add imu factor to graph
    // 加imu预积分因子

    //利用两帧之间的IMU数据完成了预积分后增加imu因子到因子图中,
    //注意后面容易被遮挡，imuIntegratorOpt_的值经过格式转换被传入preint_imu，
    // 因此可以推测imuIntegratorOpt_中的integrateMeasurement函数应该就是一个简单的积分轮子，????
    //传入数据和dt，得到一个积分量,数据会被存放在imuIntegratorOpt_中

    /*
    用法：dynamic_cast < type-id > ( expression ) 
    运算符把expression转换成type-id类型的对象。Type-id必须是类的指针、类的引用或者void *； 　　
    如果type-id是类指针类型，那么expression也必须是一个指针，如果type-id是一个引用，那么expression也必须是一个引用。 　　
    dynamic_cast主要用于类层次间的上行转换和下行转换，还可以用于类之间的交叉转换。
    */

   //楼下这些 参考imu的预积分公式中的一系列参数
    const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
    // 参数：前一帧位姿，前一帧速度，当前帧位姿，当前帧速度，前一帧偏置，预积分量
    gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);

    graphFactors.add(imu_factor);
    // add imu bias between factor
    // 添加imu偏置因子，前一帧偏置，当前帧偏置，观测值，噪声协方差；deltaTij()是积分段的时间
    graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
                     gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
    // add pose factor
    gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
    gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, correctionNoise);
    //添加位姿因子
    graphFactors.add(pose_factor);
    // insert predicted values

    //imuHandler中的imuInteratorImu_中的predict是每一帧imu数据加入预积分器后都会预测一次，而这里是将两帧激光雷达帧之间的imu帧全部加入后才预测（上面的while循环）
    // 用前一帧的状态、偏置，施加 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_);
    // optimize

    optimizer.update(graphFactors, graphValues);
    optimizer.update();
    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_);
    // 更新当前帧imu偏置
    prevBias_  = result.at<gtsam::imuBias::ConstantBias>(B(key));
    // Reset the optimization preintegration object.
    // 重置预积分器，设置新的偏置，这样下一帧激光里程计进来的时候，预积分量就是两帧之间的增量
    imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
    // check optimization
    // check optimization
    // 检测是否有失败：
    // 1. imu 的速度大于 30，则速度过大
    // 2. imu 的偏差大于 0.1，则偏差
    // 如果有上述失败，则重置参数
    if (failureDetection(prevVel_, prevBias_))
    {
        resetParams();
        return;
    }


    // 2. after optiization, re-propagate imu odometry preintegration
    // 为了维持实时性 imuIntegrateImu 就得在每次 odom 触发优化后立刻获取最新的 bias
    // 同时对imu测量值 imuQueImu 执行 bias 改变的状态重传播处理, 这样可以最大限度的保证实时性和准确性

    //prevStateOdom和prevBiasOdom是imuHandler里面用过的参数
    //感觉地位等同于prevState在这里面
    prevStateOdom = prevState_;
    prevBiasOdom  = prevBias_;
    // first pop imu message older than current correction data
    double lastImuQT = -1;
    // 从 imu 队列中删除当前激光里程计时刻之前的 imu 数据
    while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
    {   //弹出
        lastImuQT = ROS_TIME(&imuQueImu.front());
        imuQueImu.pop_front();
    }
    // repropogate
    // 对剩余的imu数据计算预积分
    // 因为bias改变了
    if (!imuQueImu.empty())
    {
        // reset bias use the newly optimized bias
        // 重置预积分器和最新的偏置，使用最新的偏差更新 bias 值
        imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
        // integrate imu message from the beginning of this optimization
        for (int i = 0; i < (int)imuQueImu.size(); ++i)
        {
            //重新进行预积分
            //与imuHandler中的预积分过程相同
            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;
}

odometryHandler中调用的函数：

重置优化器：resetOptimization.cpp

void resetOptimization()
    {
        //ISAM2的参数类
        gtsam::ISAM2Params optParameters;
        optParameters.relinearizeThreshold = 0.1;
        optParameters.relinearizeSkip = 1;
        //优化器
        optimizer = gtsam::ISAM2(optParameters);

        gtsam::NonlinearFactorGraph newGraphFactors;
        graphFactors = newGraphFactors;

        gtsam::Values NewGraphValues;
        graphValues = NewGraphValues;
    }

重置系统参数：resetParam.cpp

void resetParams()
    {
        //将上一帧imu数据的时间戳更新成-1（代表还没有imu数据进来）
        lastImuT_imu = -1;
        //false 代表 需要重新进行一次odo优化（跟imuHandler联系起来）
        doneFirstOpt = false;
        //系统关闭
        systemInitialized = false;
    }

检查优化器过程是否失败：failure Detection.cpp

bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur)
    {
        Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
        if (vel.norm() > 30)
        {
            ROS_WARN("Large velocity, reset IMU-preintegration!");
            return true;
        }

        Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());
        Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());
        if (ba.norm() > 0.1 || bg.norm() > 0.1)
        {
            ROS_WARN("Large bias, reset IMU-preintegration!");
            return true;
        }

        return false;
    }

前置知识学习

scan-to-map

Scan to map ，即激光雷达扫描数据直接与地图进行匹配，得到实际位置坐标[x,y,theta]。这种方式一边计算位置，一边把新扫描到的数据及时加入到先前地图中。

ISAM2优化器

https://blog.csdn.net/fuxingyin/article/details/51854415?locationNum=1&fps=1

iSAM2 将图优化问题转换成 **Bayes tree 的建立、更新和推理问题。**原始待优化的问题用 factor graph 表示，factor graph -> Bayes net 建立变量的条件概率，Bayes net -> Bayes tree 建立变量间的更新关系，Bayes tree 从 leaves 到 root 建立的过程，定义了变量逐级更新的 functions，Bayes tree 从 root 到 leaves 可以逐级更新变量值。iSAM2 整套架构建立在概率推理的基础上，和 iSAM 利用 QR 矩阵分解，当有 new factor 时，更新 R 矩阵差别蛮大。

畸变去除

小例子：

一般激光雷达驱动封装数据时，默认一帧激光雷达数据的所有激光束是在同一位姿下、瞬时获得的，也就是所有激光束都是从橙色点获得的数据，这样实际使用的数据和真实数据有明显差距，如10cm。所以，对于低帧率的激光雷达，运动畸变明显是个需要被解决的问题。

畸变去除原理：在一帧激光雷达数据中，为每个激光束的原点都找到一个近似的里程计位姿，并且认为该激光束是在该里程计位姿上进行数据采集的，得到这些基础数据之后，进行系列的坐标变换，把所有激光点的数据都变换到第一束激光原点的坐标系下，然后再封装数据，这样就能极大的减小畸变。

gtsam库学习

https://bitbucket.org/gtborg/gtsam/src/develop/

ISAM2Params

ISAM2Params.h

//类前定义了两个结构体：ISAM2GaussNewtonParams  ISAM2DoglegParams
/**
 * @addtogroup ISAM2
 * Parameters for ISAM2 using Gauss-Newton optimization.  Either this class or
 * ISAM2DoglegParams should be specified as the optimizationParams in
 * ISAM2Params, which should in turn be passed to ISAM2(const ISAM2Params&).
 */
//使用高斯-牛顿优化的 ISAM2 参数。 应将此类或 ISAM2DoglegParams 指定为 ISAM2Params 中的 optimizationParams，然后将其传递给 ISAM2(const ISAM2Params&)
//另一个是Dogleg，描述基本相同

//ISAM2Params类
//文件中使用的两个参数
RelinearizationThreshold relinearizeThreshold; //重线性化阈值，仅重新线性化线性增量幅度大于此阈值的变量（默认值：0.1）

int relinearizeSkip;  ///< Only relinearize any variables every
                      ///< relinearizeSkip calls to ISAM2::update (default:
                      ///< 10)
//一个下界 一个上界
//重新线性化处理？？？

ISAM2

ISAM2.h

//BayesTree 的增量更新功能 (ISAM2)，具有流体重新线性化。
//优化器

NonLinearFactorGraph

NonLinearFactorGraph.h

//Factor Graph Constsiting of non-linear factors

Values

values.h

/*
 * @brief A non-templated config holding any types of Manifold-group elements
 *
 *  Detailed story:
 *  A values structure is a map from keys to values. It is used to specify the value of a bunch
 *  of variables in a factor graph. A Values is a values structure which can hold variables that
 *  are elements on manifolds, not just vectors. It then, as a whole, implements a aggregate type
 *  which is also a manifold element, and hence supports operations dim, retract, and localCoordinates.
 * @brief 一个非模板化的配置，包含任何类型的 Manifold-group 元素
  * 值结构是从键到值的映射。 它用于指定因子图中一组变量的值。 Values 是一个值结构，它可以保存作为流形元素的变量，而不仅仅是向量。 然后，作为一个整体，它实现了一个聚合类型，该类型也是一个流形元素，因此支持操作 dim、retract 和 localCoordinates。
*/

priorFactor

priorFactor.h

    /** Constructor */
    PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) :
      Base(model, key), prior_(prior) {
    }
//先验因子

ImuFactor

ImuFactor.h

//ImuFactor imu因子

/**
 * ImuFactor is a 5-ways factor involving previous state (pose and velocity of
 * the vehicle at previous time step), current state (pose and velocity at
 * current time step), and the bias estimate. Following the preintegration
 * scheme proposed in [2], the ImuFactor includes many IMU measurements, which
 * are "summarized" using the PreintegratedIMUMeasurements class.
 * Note that this factor does not model "temporal consistency" of the biases
 * (which are usually slowly varying quantities), which is up to the caller.
 * See also CombinedImuFactor for a class that does this for you.
 *
 * @addtogroup SLAM
 
 * ImuFactor 是一个 5 向因子，涉及先前状态（车辆在前一时间步的姿态和速度）、当前状态（当前时间步的姿态和速度）和偏差估计。 遵循 [2] 中提出的预积分方案，ImuFactor 包括许多 IMU 测量值，这些测量值使用 PreintegratedIMUMeasurements 类进行“汇总”。
  * 请注意，此因素不会对偏差（通常是缓慢变化的数量）的“时间一致性”进行建模，这取决于调用者。
  * 另请参阅 CombinedImuFactor 以了解为您执行此操作的类。
 */

ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
      const PreintegratedImuMeasurements& preintegratedMeasurements);

imuFactor.cpp

//------------------------------------------------------------------------------
// ImuFactor methods
//------------------------------------------------------------------------------
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
    const PreintegratedImuMeasurements& pim) :
    Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
        pose_j, vel_j, bias), _PIM_(pim) {
}
//这里的Base，根据ImuFactor.h中的重定义为：typedef NoiseModelFactor2<POSE, POSE> Base;
//找到对应的函数：

gtsam/gtsam/nonlinear/NonlinearFactor.h ：
/**
   * Constructor
   * @param noiseModel shared pointer to noise model
   * @param j1 key of the first variable
   * @param j2 key of the second variable
   */
  NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) :
    Base(noiseModel, cref_list_of<2>(j1)(j2)) {}
//但是跟项目里的参数列表对不起来，但是大体意思一样了
//这里的Base，根据NonlinearFactor.h中的重定义为：typedef NoiseModelFactor Base;

template<typename CONTAINER>
  NoiseModelFactor(const SharedNoiseModel& noiseModel, const CONTAINER& keys) ://这里的key变成keys了
    Base(keys), noiseModel_(noiseModel) {}
//这里的Base，根据NonlinearFactor.h中的重定义为：typedef NonlinearFactor Base;

template<typename CONTAINER>
NonlinearFactor(const CONTAINER& keys) :
Base(keys) {}
//这里的Base，根据NonlinearFactor.h中的重定义为：typedef Factor Base;

gtsam/interface/factor.h
/** Construct factor from container of keys.  This constructor is used internally from derived factor
    *  constructors, either from a container of keys or from a boost::assign::list_of. */
    template<typename CONTAINER>
    explicit Factor(const CONTAINER& keys) : keys_(keys.begin(), keys.end()) {}
	//注意这个keys.begin和keys.end

predict函数

在上篇博客中没有找到该函数的原型，这次发现了一个相似度很高的函数：

gtsam/gtsam/navigation/PreintegrationBase.cpp

//------------------------------------------------------------------------------
NavState PreintegrationBase::predict(const NavState& state_i,
    const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
    OptionalJacobian<9, 6> H2) const {
  // TODO(frank): make sure this stuff is still correct
  Matrix96 D_biasCorrected_bias;
  Vector9 biasCorrected = biasCorrectedDelta(bias_i,
      H2 ? &D_biasCorrected_bias : 0);

  // Correct for initial velocity and gravity
  Matrix9 D_delta_state, D_delta_biasCorrected;
  Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
      p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
      H2 ? &D_delta_biasCorrected : 0);

  // Use retract to get back to NavState manifold
  Matrix9 D_predict_state, D_predict_delta;
  NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
  if (H1)
    *H1 = D_predict_state + D_predict_delta * D_delta_state;
  if (H2)
    *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias;
  return state_j;
}

X V B

函数中出现的X(0) B(0)…

gtsam/inference/Symbol.h

/**
  * 用于引用变量的字符和索引键。 将简单地转换为一个 Key，即一个大整数。 键用于从值中检索值，指定因素依赖的变量等。
  */

/** Create a symbol key from a character and index, i.e. x5. */
inline Key symbol(unsigned char c, std::uint64_t j) { return (Key)Symbol(c,j); }

/** Return the character portion of a symbol key. */
inline unsigned char symbolChr(Key key) { return Symbol(key).chr(); }

/** Return the index portion of a symbol key. */
inline std::uint64_t symbolIndex(Key key) { return Symbol(key).index(); }

namespace symbol_shorthand {
inline Key A(std::uint64_t j) { return Symbol('a', j); }
inline Key B(std::uint64_t j) { return Symbol('b', j); }
inline Key C(std::uint64_t j) { return Symbol('c', j); }
inline Key D(std::uint64_t j) { return Symbol('d', j); }
inline Key E(std::uint64_t j) { return Symbol('e', j); }
inline Key F(std::uint64_t j) { return Symbol('f', j); }
inline Key G(std::uint64_t j) { return Symbol('g', j); }
.
.
.
inline Key Z(std::uint64_t j) { return Symbol('z', j); }
}

通过综合分析后，认为X B V应该作为一个通用的键，同时与后面的信息，噪声之类的相关联。