2023面试高手

这个屌丝很懒，什么也没留下！

热门标签

ROS-3DSLAM（五）lvi-sam源代码阅读3_m.resetparams()优化

作者：2023面试高手 | 2024-02-19 20:40:37

踩

m.resetparams()优化

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;
}
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
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340

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;
    }

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

重置系统参数：resetParam.cpp

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

检查优化器过程是否失败：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;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

前置知识学习

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)
//一个下界 一个上界
//重新线性化处理？？？
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

ISAM2

ISAM2.h

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

NonLinearFactorGraph

NonLinearFactorGraph.h

//Factor Graph Constsiting of non-linear factors
1

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。
*/
1
2
3
4
5
6
7
8
9
10
11

priorFactor

priorFactor.h

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

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);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

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
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

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

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); }
}

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

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

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/2023面试高手/article/detail/116696