本文详细解读了2021SC@SDUSC项目中imuHandler函数，涉及imu数据预积分、状态初始化、优化与重播过程。核心内容包括激光里程计与imu数据的同步、状态先验因子设置、ISAM2优化器重置与因子图更新。
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.youkuaiyun.com/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应该作为一个通用的键，同时与后面的信息，噪声之类的相关联。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
