VINS-Mono中的边缘化与滑窗 (2)—— 边缘化最老帧
文章目录
- 1. 添加和最老帧有关的参数块
- 1.1. 和最老帧的IMU约束有关的参数块
- 1.2. 和最老帧的视觉重投影约束有关的参数块
- 2. `MarginalizationInfo::preMarginalize()`边缘化预处理
- 2.1. 计算每个残差块的残差和雅克比
- 2.2. 缓存边缘化之前的参数值
- 3. 执行边缘化舒尔补
- 3.1. 统计参数块在边缘化变量中的顺序
- 3.1.1. 优先排列要边缘化掉的参数块
- 3.1.2. 继续排列剩下的边缘化保留的参数块
- 3.2. 多线程构造方程 H Δ x = g H\Delta x = g HΔx=g
- 3.2.1. 把残差块分配到每个线程中
- 3.2.2. 每个线程中计算H和g
- 3.2.3. 求解舒尔补
- 3.2.4. 对方程 H ′ Δ x b = g ′ H' \Delta x_b = g' H′Δxb=g′中的 H ′ , g ′ H', g' H′,g′分解得到雅克比矩阵和残差
- 4. 存储边缘化后结果,给下次非线性优化使用
- 4.1. 存储保留下的参数块信息
- 4.2. 赋值本次边缘化变量到全局变量中
- 5. 下次非线性优化使用上次的边缘化因子 MarginalizationFactor
- 5.1. 确定边缘化因子的残差维度和每个参数块维度
- 5.2. 重载`Evaluate`函数计算残差和雅克比
- 5.2.1. 残差计算方式
- 5.2.2. 雅克比计算方式
1. 添加和最老帧有关的参数块
1.1. 和最老帧的IMU约束有关的参数块
如图所示,第一帧的位姿 T 1 T_1 T1和速度零偏 B 1 B_1 B1是通过IMU预积分约束和滑窗中的最老帧存在约束关系的。
因此和最老帧有关的IMU预积分约束包含的参数块是 T 0 , B 0 , T 1 , B 1 T_0, B_0, T_1, B_1 T0,B0,T1,B1,要边缘化掉的参数块是 T 0 , B 0 T_0, B_0 T0,B0。
代码如下:
在MarginalizationInfo::addResidualBlockInfo
函数中,会用一些MarginalizationInfo
类变量将上面构建的ResidualBlockInfo
残差信息块存储起来:
变量 | 内容 |
---|---|
factors | 上面构建的IMU残差的残差块residual_block_info |
parameter_blocks | 这个残差块有关的参数内存地址,即 T 0 , B 0 , T 1 , B 1 T_0, B_0, T_1, B_1 T0,B0,T1,B1 |
parameter_block_size | (参数内存地址数值,参数global维度), 如(& T 0 T_0 T0, 7) (& B 0 B_0 B0, 9) (& T 1 T_1 T1, 7) (& B 1 B_1 B1, 9) |
parameter_block_idx | (要边缘化掉的参数的内存地址数值,索引暂时赋值0),如(& T 0 T_0 T0, 0) (& B 0 B_0 B0, 0) |
1.2. 和最老帧的视觉重投影约束有关的参数块
第1234帧的 位姿 T i T_i Ti 是通过视觉重投影和最老帧联系在一起的,此外视觉重投影中还包括IMU和相机外参 T i c T_{ic} Tic,还包括每个点的逆深度。
因此最老帧看到的每一个视觉点和其他帧构成的视觉重投影约束包含的参数块是: T 0 , T j , T i c , λ i T_0, T_j, T_{ic}, \lambda_i T0,Tj,Tic,λi,要边缘化掉的参数块是 T 0 , λ i T_0, \lambda_i T0,λi。
代码如下:
在MarginalizationInfo::addResidualBlockInfo
函数中,会用一些MarginalizationInfo
类变量将上面构建的ResidualBlockInfo
残差信息块存储起来:
变量 | 内容 |
---|---|
factors | 上面构建的视觉重投影残差的残差块residual_block_info |
parameter_blocks | 这个残差块有关的参数内存地址,如 T 0 , T 2 , T i c , λ 1 T_0, T_2, T_{ic}, \lambda_1 T0,T2,Tic,λ1 |
parameter_block_size | (参数内存地址数值,参数global维度), 如(& T 0 T_0 T0, 7) (& T 2 T_2 T2, 7) (& T i c T_{ic} Tic, 7) (& λ i \lambda_i λi, 1) |
parameter_block_idx | (要边缘化掉的参数的内存地址数值,索引暂时赋值0),如(& T 0 T_0 T0, 0) (& λ i \lambda_i λi, 0) |
2. MarginalizationInfo::preMarginalize()
边缘化预处理
2.1. 计算每个残差块的残差和雅克比
手动调用ResidualBlockInfo::Evaluate()
函数计算每个残差块的残差和雅克比,其内部本质上还是调用了我们定义的各种Factor因子里面的Evaluate
函数,比如IMU预积分因子、视觉重投影因子,我们在这些因子中的Evaluate
函数中定义了残差和雅克比如何计算,从而提供给ceres进行非线性优化使用。
代码如下:
将计算的结果存到残差块ResidualBlockInfo
的类成员变量中:
变量 | 内容 |
---|---|
residuals | 残差块的残差 |
jacobians | 残差块对和它有关的参数块的雅克比 |
2.2. 缓存边缘化之前的参数值
这一步是为了边缘化之后,下一次非线性优化执行的时候,计算边缘化约束的残差而使用的,所以这里就是把数据存储到类成员变量中。
代码如下:
3. 执行边缘化舒尔补
3.1. 统计参数块在边缘化变量中的顺序
边缘化的原则是:将要被边缘化的参数块放在状态变量的前面。比如上面的图,我们所有的状态变量是
T 0 , B 0 , T 1 , B 1 , T 2 , T 3 , T 4 , T i c , λ 1 , λ 2 , λ 3 T_0, B_0, T_1, B_1, T_2, T_3, T_4, T{ic}, \lambda_1, \lambda_2, \lambda_3 T0,B0,T1,B1,T2,T3,T4,Tic,λ1,λ2,λ3,要边缘化掉的参数块是 T 0 , B 0 , λ 1 , λ 2 , λ 3 T_0, B_0, \lambda_1, \lambda_2, \lambda_3 T0,B0,λ1,λ2,λ3。那么就把这些变量放在前面,然后在状态变量中的索引按照内存维度来排列。
3.1.1. 优先排列要边缘化掉的参数块
代码:
将要边缘化掉的参数块放在前面,对每一个参数块在边缘化的大向量 X X X中的内存索引进行排列,结果更新到MarginalizationInfo
的类成员变量parameter_block_idx
中。注意变量用localPose,即实际自由度;而golbalPose是参数块存储的维度;比如对位姿来说,localPose是6,globalPose是7。
上面的程序执行后,结果如下:
变量 |
---|
parameter_block_idx(& T 0 T_0 T0, 0), localPose = 6 |
parameter_block_idx(& B 0 B_0 B0, 6) , localPose = 9 |
parameter_block_idx(& λ 1 \lambda_1 λ1, 15), localPose = 1 |
parameter_block_idx(& λ 2 \lambda_2 λ2, 16), localPose = 1 |
parameter_block_idx(& λ 3 \lambda_3 λ3, 17), localPose = 1 |
m = 18 (6+9+1+1+1),要边缘化掉的参数的维度 |
3.1.2. 继续排列剩下的边缘化保留的参数块
代码:
把剩下的边缘化保留下的参数块排在后面,对这些参数块在边缘化的大向量 X X X中的内存索引进行排列,结果同样存储在上面的变量中。注意变量用localPose,即实际自由度。
假设这里排序的时候留下的参数块的排列顺序如下,我们这里故意颠倒了 T 3 , T 2 T_3, T_2 T3,T2的顺序,是为了后面说明这个顺序其实没有影响:
变量 |
---|
parameter_block_idx(& T 1 T_1 T1, 18), localPose = 6 |
parameter_block_idx(& B 1 B_1 B1, 24), localPose = 9 |
parameter_block_idx(& T 3 T_3 T3, 33), localPose = 6 |
parameter_block_idx(& T 2 T_2 T2, 39), localPose = 6 |
parameter_block_idx(& T 4 T_4 T4, 45), localPose = 6 |
parameter_block_idx(& T i c T_{ic} Tic, 51), localPose = 6 |
n = 57 - 18 = 39 保留下的参数的维度 |
因此最后状态变量的顺序如下,其中前5个是要被边缘化掉的。
T 0 T_0 T0 | B 0 B_0 B0 | λ 1 \lambda_1 λ1 | λ 2 \lambda_2 λ2 | λ 3 \lambda_3 λ3 | T 1 T_1 T1 | B 1 B_1 B1 | T 3 T_3 T3 | T 2 T_2 T2 | T 4 T_4 T4 | T i c T_{ic} Tic |
---|
3.2. 多线程构造方程 H Δ x = g H\Delta x = g HΔx=g
3.2.1. 把残差块分配到每个线程中
将之前存储在factor
中的所有残差块都平均分配到四个线程中,然后在每个线程中对残差块进行处理。
代码如下:
3.2.2. 每个线程中计算H和g
对最终每个线程计算的 H H H和 g g g都加起来,就得到最后总的 H H H和 g g g。
代码如下:
分析如下:
H = J T J H = J^T J H=JTJ,其中 J ( n , m ) J(n, m) J(n,m), n n n是边缘化中所有残差块的总维度, m m m是边缘化中所有参数块的总维度,所以最后 H ( m , m ) H(m,m) H(m,m)是和所有参数块的总维度相等的,和残差块的维度无关。
这里以 T 0 − λ 1 − T 3 − T i c T_0 - \lambda_1 - T_3 - T_{ic} T0−λ1−T3−Tic 构成的视觉重投影约束为例(注意视觉重投影要从IMU系到相机系转换,因此参数块包含外参),其在雅克比矩阵中的位置如下。注意该雅克比矩阵的行数是这个残差块的维度,视觉重投影中就是2;而对每一个参数块的雅克比的列数就是这个参数块的维度,比如 T 0 T_0 T0是6列, λ 1 \lambda_1 λ1是1列。
那么 H H H矩阵如下所示,可见 H H H矩阵中每一块的位置都只和状态变量的位置有关,而和残差无关。
3.2.3. 求解舒尔补
对 H H H矩阵按照要边缘化的参数块的位置进行分块,然后进行舒尔补得到只包含保留下来的参数块的方程 H ′ Δ x b = g ′ H' \Delta x_b = g' H′Δxb=g′。
代码:
3.2.4. 对方程 H ′ Δ x b = g ′ H' \Delta x_b = g' H′Δxb=g′中的 H ′ , g ′ H', g' H′,g′分解得到雅克比矩阵和残差
边缘化最终的目的就是得到像普通的IMU预积分约束和视觉重投影约束一样的结果:残差和关于哪些参数块的雅克比矩阵。而我们得到的是 H ′ Δ x b = g ′ H' \Delta x_b = g' H′Δxb=g′,如果通过正常的残差和雅克比矩阵的话,等效的方程就是 J T J Δ x = − J T e J^TJ \Delta x = -J^T e JTJΔx=−JTe,所以我们要从 H ′ H' H′和 g ′ g' g′中分离出残差和雅克比矩阵。
代码如下:
最终得到存储在MarginalizationInfo
类中的成员变量linearized_jacobians
和linearized_residuals
,代表边缘化得到的残差和雅克比。
注意思考:
1.残差的维度是多少?其实是和留下来的状态变量的维度一样的,因为我们对 H ′ H' H′矩阵进行的是类似开根号的分解,所以得到的 J J J矩阵还是一个方阵,其行数和列数都是保留下的状态变量的维度。
2.雅克比矩阵是对哪些参数块的雅克比?这个根据上面的解释很显然了,就是对边缘化留下来的状态变量参数块的雅克比。
变量 | 内容 |
---|---|
linearized_jacobians | 对留下来的状态的雅克比,即对 T 1 , B 1 , T 3 , T 2 , T 4 , T i c T_1, B_1, T_3, T_2, T_4, T_{ic} T1,B1,T3,T2,T4,Tic的雅克比 |
linearized_residuals | 和留下来的状态维数一样的残差,上面的例子就是 n = 39 |
4. 存储边缘化后结果,给下次非线性优化使用
4.1. 存储保留下的参数块信息
对于边缘化最老帧来说,边缘化之后,下一次再进行非线性优化的时候,第1帧就存到第0帧的位置,第2帧存到第1帧的位置,依次类推,因此要记录这次边缘化留下的参数块在下一次进行非线性优化的时候内存地址在哪里。
代码如下(注意下面这个addr_shift
只是一个局部变量,其真正起作用是被传入getParameterBlocks
函数中起作用):
然后在MarginalizationInfo::getParameterBlocks
函数中,会保存边缘化留下来的参数块的信息到MarginalizationInfo
类成员变量中:
变量 | 内容 |
---|---|
keep_block_size | 留下来的参数块的global维度,如留下 T 1 , B 1 , T 3 , T 2 , T 4 , T i c T_1, B_1, T_3, T_2, T_4, T_{ic} T1,B1,T3,T2,T4,Tic,则为7, 9, 7, 7, 7, 7 |
keep_block_idx | 留下来的参数块在边缘化的大向量 X X X中的索引位置,本例为 18, 24, 33, 39, 45, 51 |
keep_block_data | 留下来的参数块在边缘化之前的值,即parameter_block_data中对应的参数块的值 |
keep_block_addr | 函数返回值,即留下来的参数在下次优化的时候(经过滑窗地址已变)的内存地址 |
4.2. 赋值本次边缘化变量到全局变量中
变量 | 内容 |
---|---|
last_marginalization_info | 本次边缘化得到的结果,各种变量都在其中 |
last_marginalization_parameter_blocks | 本次边缘化保留下的参数块在下次优化的时所在的内存地址 |
注意思考:
问题:既然last_marginalization_info
保留了MarginalizationInfo
这个边缘化的巨无霸类,这个类中已经包含了keep_block_size
等记录本次边缘化保留下来的参数块的信息,为什么不把last_marginalization_parameter_blocks
参数块也定义在类中呢?
解答:其实主要是为了下次非线性优化的时候使用边缘化先验因子方便,因为在ceres中添加残差块的时候,通常要传入3大块参数,如下所示:
problem.AddResidualBlock(残差块因子, 鲁棒核函数, 和残差块因子有关的参数块1内存地址, 和残差块因子有关的参数块2内存地址, ...);
残差块因子:就是各种
Factor
,里面重载了Evaluate
函数,其中定义了如何计算残差,如何计算残差对优化参数块的雅克比矩阵;鲁棒核函数:排除外点用的;
优化参数块内存地址:这部分就是和残差块因子有关的优化参数块的地址,有多个参数块的话就要分别传入每个参数块的首地址。而我们定义的是
vector<double *> last_marginalization_parameter_blocks;
,可以直接传入一个,会自动隐式转换成多个==?==反正就是可以这样用,比写在类中更加方便。
5. 下次非线性优化使用上次的边缘化因子 MarginalizationFactor
5.1. 确定边缘化因子的残差维度和每个参数块维度
首先明确每次边缘化的残差和雅克比的维度都不一样:假设每次边缘化都是边缘化掉最老帧,那么关于IMU因子的边缘化残差维度和状态变量的维度是确定的。但是每次视觉重投影的边缘化,由于看到的路标点数目不确定,这些路标点被哪些帧看到也不确定,因此他们的参数块个数是不确定的。这就意味着我们无法在编译程序的时候就确定这个维度,所以就无法使用ceres中普通的确定维度的Factor
,即继承SizedCostFunction
的Factor,如下所示:
所以只能使用更加基础的CostFunction
,然后在类的构造函数中,每次运行到这里我都知道了上一次边缘化的参数块个数和残差维度,然后在类的构造函数中对CostFunction
的残差块维度和各个优化参数块的维度进行赋值,注意这里对优化参数块维度是使用globalSize,因为我们要的是本次优化参数块在内存中存储的维度,这也是我们边缘化的最后存储留下来的参数块的globalSize到变量keep_block_sieze
的原因。
代码如下:
5.2. 重载Evaluate
函数计算残差和雅克比
5.2.1. 残差计算方式
代码如下:
上面的程序中,需要理解三个地方:
为什么要把残差的
index
统一到0?为什么要把本次优化参数块和上次边缘化之前对应的参数块做差?
FEJ(First Estimated Jacobian)
是怎么回事?
解答这三个问题,要从头开始慢慢思考:
1. 边缘化得到的残差和雅克比是什么意义?
首先我们知道边缘化得到的残差维度就是边缘化留下来的所有参数块的localSize的和,然后我们还保存了边缘化之前这些参数块的值。这就相当于我们边缘化得到了一个对留下来的参数块的先验因子,如下图所示,我们边缘化掉的参数块变成了灰色,然后它们对留下来的参数块形成了一个先验因子。这个先验因子就好比是我们对这些保留下来的状态的一个约束,约束这次非线性优化的时候这些状态不能离先验因子表示的状态太远。
2.那么这里的先验因子是什么呢?是边缘化得到的残差吗?
按照后面的公式看并不是,代码中的公式如下:
dx.segment(idx, size) = x - x0; // 不需要local param的直接做差
Eigen::Map<Eigen::VectorXd>(residuals, n) = marginalization_info->linearized_residuals + marginalization_info->linearized_jacobians * dx;
(1)按照公式可以这么理解:
我们得到的先验因子其实就是边缘化之前的参数块的值,也就是程序中的keep_block_data
,它约束这我们这次进行非线性优化的结果不能离这个值太远。那么从边缘化过程中来看,我们得到的雅克比是边缘化得到的残差 e 0 \boldsymbol e_0 e0关于边缘化保留下来的状态的雅克比,对应到上面的程序中,就是对x0
的雅克比,即 J = ∂ e 0 ∂ x 0 \boldsymbol J = \frac{\partial{\boldsymbol {e}_0}}{\partial{\boldsymbol {x}_0}} J=∂x0∂e0。但是现在我们优化的是x
,是这次的状态变量,而x0
只是上次优化的结果,在这次优化过程中它是一个常数。但是我们也说了,x0
是作为x
的先验因子,这意味着x
和x0
不会相差太远。那么之前边缘化时候的雅克比 J = ∂ e ∂ x 0 \boldsymbol J = \frac{\partial{\boldsymbol e}}{\partial{\boldsymbol {x}_0}} J=∂x0∂e,就约等于 J = ∂ e ∂ x − x 0 \boldsymbol J = \frac{\partial{\boldsymbol e}}{\partial{\boldsymbol x - \boldsymbol {x}_0}} J=∂x−x0∂e。
注意:前面的一个偏导 x 0 \boldsymbol {x}_0 x0是状态变量,因此 x 0 \boldsymbol {x}_0 x0是变量;后面的一个偏导 x \boldsymbol {x} x是状态变量,因此 x \boldsymbol {x} x是变量,而 x 0 \boldsymbol {x}_0 x0只是一个常量;所以就相当于后面的一个偏导用 x \boldsymbol x x来近似了前面一个偏导的 x 0 \boldsymbol {x}_0 x0。从而当此次非线性优化的过程中,如果状态变量 x \boldsymbol x x离先验状态 x 0 \boldsymbol {x}_0 x0发生了 x − x 0 \boldsymbol x - \boldsymbol {x}_0 x−x0的变化,那么边缘化的残差 e \boldsymbol e e应该产生的增量就是使用一阶泰勒展开,为 J ( x − x 0 ) \boldsymbol J (\boldsymbol x - \boldsymbol{x}_0) J(x−x0)。那么总的边缘化残差就是上次边缘化的时候的残差 e 0 \boldsymbol e_0 e0加上增量,即 e = e 0 + J ( x − x 0 ) \boldsymbol e = \boldsymbol {e}_0 + \boldsymbol J (\boldsymbol x - \boldsymbol{x}_0) e=e0+J(x−x0)。这其实就是 FEJ(First Estimated Jacobian)
。
(2)再来思考,上次边缘化的残差 e 0 \boldsymbol e_0 e0代表什么意义呢?
可以这么想,每一次的优化结果都是一个最小二乘结果,也就是每一个状态变量都非常接近真实值,但又都不是真实值。因为如果一旦迁就了某个状态变量让它变成真实值,那么其他状态变量就会更加的偏离真实值,最后造成所有状态变量偏离真实值的误差平方和更大了。所以说这个上次边缘化的残差 e 0 \boldsymbol e_0 e0就是代表上次边缘化给出的先验状态因子 x 0 \boldsymbol {x}_0 x0离真实状态的误差,这是由于最小二乘造成的,是为了兼顾总的误差最小而导致每个状态变量都有一个误差。
3.再回到我们的问题,为什么要把残差的index
统一到0?
其实就是因为现在我们的先验状态因子只有对上次边缘化保留下来的状态的先验因子,而我们的keep_block_idx
存储的是上次边缘化开始之前,包含被边缘化的参数块和保留下来的参数块的所有参数块的内存索引排序。以上图为例,之前我们推导排序结果如下:
parameter_block_idx中保留下的参数块的索引 |
---|
m = 18 (6+9+1+1+1),要边缘化掉的参数的维度 |
parameter_block_idx(& T 1 T_1 T1, 18), localPose = 6 |
parameter_block_idx(& B 1 B_1 B1, 24), localPose = 9 |
parameter_block_idx(& T 3 T_3 T3, 33), localPose = 6 |
parameter_block_idx(& T 2 T_2 T2, 39), localPose = 6 |
parameter_block_idx(& T 4 T_4 T4, 45), localPose = 6 |
parameter_block_idx(& T i c T_{ic} Tic, 51), localPose = 6 |
n = 57 - 18 = 39 保留下的参数的维度 |
也就是上次边缘化中,我们保留下的参数块只有39个维度,也就是我们的残差是39维度,并且在边缘化中的状态向量 X X X中的内存索引是从18—56(注意最后一个索引是56,总数是57)。
而此次非线性优化在内存中存储上次边缘化保留下来的状态变量到Eigen::VectorXd
的时候,我们应该是从0—38存储,所以说我们要把边缘化掉的那些参数块的索引偏移减掉。
4.另外注意,在边缘化的误差中,我们的状态变量的排列顺序有影响吗?
比如上面边缘化我们对保留下来的状态变量的排列顺序是 T 1 , B 1 , T 3 , T 2 , T 4 , T i c T_1, B_1, T_3, T_2, T_4, T_{ic} T1,B1,T3,T2,T4,Tic,而一般我们在非线性优化中对状态变量应该都是按照关键帧的顺序进行顺序排列的,也就是应该是 T 1 , B 1 , T 2 , T 3 , T 4 , T i c T_1, B_1, T_2, T_3, T_4, T_{ic} T1,B1,T2,T3,T4,Tic,即边缘化中我们把保留下来的状态变量中有些顺序颠倒了,这在本次的非线性优化中有影响吗?即这样是否会造成我们本次非线性优化对状态变量计算的残差和雅克比位置不对应?
其实是没有影响的, 因为我们传入参数块(状态变量)的时候传入的是它们的地址,这也就保证了本次非线性优化拿到的某个参数块和我们保留的边缘化之前的某个参数块一定是对应的,这个对应关系是由keep_block_data
和last_marginalization_parameter_blocks
的顺序是一一对应而决定的。因此只要参数块的内存地址是正确的,那我们本次优化计算的边缘化因子的残差就和参数块是对应的,从而这两块参数在最后本次非线性优化的整个 H H H矩阵中的位置就可以正确的找到,因为ceres就是通过参数块的内存地址来构造 H H H矩阵。
5.2.2. 雅克比计算方式
其实前面就用FEJ
来解释过了,在本次的非线性优化过程中,认为 x \boldsymbol x x离我边缘化得到的先验 x 0 \boldsymbol {x}_0 x0不会太远,因此使用上次边缘化的时候误差对 x 0 \boldsymbol {x}_0 x0的雅克比近似本次非线性优化过程中的雅克比即可。
但是要注意的是,程序中的索引对齐是为了得到边缘化的雅克比矩阵中对当前参数块的偏导。因为边缘化的雅克比维度是 n × n n \times n n×n,其中行数就是和残差的维度对应的,而残差的维度就是保留下来的状态变量的维度。而列数是和求导的状态变量的维度对应的,其实也是保留下来的状态变量的维度。以上面的例子为例,边缘化的雅克比形式如下:
J = [ ∂ e ∂ T 1 ∣ ∂ e ∂ B 1 ∣ ∂ e ∂ T 3 ∣ ∂ e ∂ T 2 ∣ ∂ e ∂ T 4 ∣ ∂ e ∂ T i c ] \boldsymbol J = \left[ \begin{matrix} \frac{\partial {\boldsymbol e}}{\partial {\boldsymbol {T}_1}} \quad | \quad \frac{\partial {\boldsymbol e}}{\partial {\boldsymbol {B}_1}} \quad | \quad \frac{\partial {\boldsymbol e}}{\partial {\boldsymbol {T}_3}} \quad | \quad \frac{\partial {\boldsymbol e}}{\partial {\boldsymbol {T}_2}} \quad | \quad \frac{\partial {\boldsymbol e}}{\partial {\boldsymbol {T}_4}} \quad | \quad \frac{\partial {\boldsymbol e}}{\partial {\boldsymbol {T}_{ic}}} \end{matrix} \right] J=[∂T1∂e∣∂B1∂e∣∂T3∂e∣∂T2∂e∣∂T4∂e∣∂Tic∂e]
所以为了得到对某一个参数块的雅克比,需要在列上寻找对应的索引。
代码如下:
VINS-Mono中的边缘化与滑窗 (2)—— 边缘化最老帧相关推荐
- 用Java实现Stream流处理中的滑窗
2019独角兽企业重金招聘Python工程师标准>>> 简单地说,滑窗算法是一种移动固定大小的窗口(子列表)来遍历数据结构的方法,主要是基于固定步骤的序列流数据. 如果我们想通过使用 ...
- 一起做激光反光板(六)-基于滑窗的EKF-SLAM及外参自动标定公式推导
在第四篇中已经提到,如果场景中反光板不够多,容易造成EKF系统效果不好的问题,且我们还想用上其他的点云信息,保证在反光板不够的情况下仍能够正确的收敛. 我们考虑扩充观测信息: (1)角点和线段特征,加 ...
- codeforces 293E Close Vertices 点分治+滑窗+treap
http://codeforces.com/contest/293/problem/E 题意:求树上合法点对的个数.合法条件为:路径长度<=W,路径边数<=L. 显然是点分治.求解的时候第 ...
- 风控特征—时间滑窗统计特征体系
" 本文介绍了风控业务中构建时间滑窗特征的一些实践经验,是一篇既能让读者快速上手特征工程又能加深其业务理解的深度好文." 作者:求是汪在路上 来源:知乎专栏 风控模型算法. 编辑: ...
- 风控特征:时间滑窗统计特征体系
风控业务背景 俗话说, 路遥知马力,日久见人心.在风控中也是如此,我们常从时间维度提取借款人在不同时间点的特征,以此来判断借款人的风险.在实践中,这类特征通常会占到80%以上.由于是通过时间切片和聚合 ...
- 【CodeForces - 144C】Anagram Search(尺取,滑窗问题,处理字符串计数)
题干: A string t is called an anagram of the string s, if it is possible to rearrange letters in t so ...
- 基于滑窗捕获的伪卫星系统抗远近效应方法研究
[摘 要]通过滑窗捕获方法来对抗伪卫星系统中的远近效应,在伪卫星基站发射端进行分时隙发送信号,同时在接收端通过滑窗的方式增加一维时域的捕获.实验结果表明,在远近效应较强的情况下,滑窗捕获相关峰均比值 ...
- 力扣刷题笔记:1438. 绝对差不超过限制的最长连续子数组(滑窗模板题,选择有序列表SortedList()数据类型就不会超时)
题目: 1438.绝对差不超过限制的最长连续子数组 给你一个整数数组 nums ,和一个表示限制的整数 limit,请你返回最长连续子数组的长度,该子数组中的任意两个元素之间的绝对差必须小于或者等于 ...
- VOC数据集制作 滑窗切分图片 单通道标签调色板
做PASCAL VOC语义分割数据集的时候, 1.标注工具使用labelme,我是在anaconda下新建了一个名字为labelme的虚拟环境,将labelme安装在了这个环境下面,具体的安装一堆教程 ...
最新文章
- poj1511(SPFA算法)
- 修改mysql数据库的编码格式
- linux c之命名管道简单使用
- 华为回应美新规:不涉及产品买卖;微软 GitHub 帐户疑被黑;GCC 10.1 发布 | 极客头条...
- LeetCode之二分查找
- flex 实现图片播放 方案二 把临时3张图片预加载放入内存
- @Autowired注解位置、@Autowired与@Resource的区别与注入流程
- mongodb 的 GridFS 详细分析(二)
- 数学建模-TOPSIS法
- 软件开发技术文档的编写模块
- dreamweaver cs6 html教程,Dreamweaver cs6安装详细图文教程
- 双显卡(Intel+Nvidia)笔记本配置cuda开发环境
- 招投标系统源码 java招投标系统 招投标系统简介 招投标系统功能设计
- eNSP实验日记二划分Vlan
- 【Axure交互教程】 可模糊搜索的多选效果
- qs使用方式+axios|| uni-app + qs及其基础封装
- 关于MOTOROLA O202C无线座机 来电响一声故障
- 系统间对接 各个方案
- 一本价值不太大的书——《正在爆发的互联网革命》读后的印象
- DNF台版云服务器搭建(非商用,可联机)
热门文章
- 交换机依靠什么进行数据转发?
- 儿童计算机知识科普ppt,儿童科普通信知识.ppt
- 易语言 用精易的网页_访问 请求https的时候返回不了数据
- 100集华为HCIE安全培训视频教材整理 | 防火墙出口选路(二)
- dcap mysql_Scrapy抓取关键字(支持百度、搜狗等)
- 在进行CMOS设置时也会出现死机
- 计算机专业实习报告范文3000字,计算机毕业实习报告3000字范文{3篇}
- 【tinyriscv verilator】分支移植到正点原子达芬奇开发板
- EZDML的SQL数据查询功能介绍
- 利用swiper插件做点击相应slide块居中