OPEN-VINS代码学习(一)
UpdaterHelper::get_feature_jacobian_representation
全局XYZ
// 全局XYZ表示
if (feature.feat_representation == LandmarkRepresentation::Representation::GLOBAL_3D) {
H_f.resize(3, 3);
H_f.setIdentity();
return;
}
作为规范参数化,三维点特征的全局位置仅由其在全局参考坐标系中的xyz坐标给出:
G
p
f
=
f
(
λ
)
=
[
G
x
G
y
G
z
]
where
λ
=
G
p
f
=
[
G
x
G
y
G
z
]
⊤
\begin{aligned} ^G\mathbf{p}_f &=\mathbf{f}(\boldsymbol{\lambda}) \\ &=\begin{bmatrix}^Gx\\^Gy\\^Gz\end{bmatrix} \\ \text{where}\quad\boldsymbol{\lambda}&={}^G\mathbf{p}_f=\begin{bmatrix}^Gx&^Gy&^Gz\end{bmatrix}^\top \end{aligned}
Gpfwhereλ=f(λ)=
GxGyGz
=Gpf=[GxGyGz]⊤
很明显,相对于特征参数的雅可比矩阵是:
∂
f
(
⋅
)
∂
λ
=
I
3
×
3
\begin{aligned}\frac{\partial\mathbf{f}(\cdot)}{\partial\boldsymbol{\lambda}}&=\mathbf{I}_{3\times3}\end{aligned}
∂λ∂f(⋅)=I3×3
全局逆深度
// 全局逆深度表示
if (feature.feat_representation == LandmarkRepresentation::Representation::GLOBAL_FULL_INVERSE_DEPTH) {
// 获取线性化特征点
Eigen::Matrix<double, 3, 1> p_FinG = (state->_options.do_fej) ? feature.p_FinG_fej : feature.p_FinG;
// 获取逆深度表示 (should match what is in Landmark.cpp)
double g_rho = 1 / p_FinG.norm();
double g_phi = std::acos(g_rho * p_FinG(2));
// double g_theta = std::asin(g_rho*p_FinG(1)/std::sin(g_phi));
double g_theta = std::atan2(p_FinG(1), p_FinG(0));
Eigen::Matrix<double, 3, 1> p_invFinG;
p_invFinG(0) = g_theta;
p_invFinG(1) = g_phi;
p_invFinG(2) = g_rho;
// 获取逆深度方位角
double sin_th = std::sin(p_invFinG(0, 0));
double cos_th = std::cos(p_invFinG(0, 0));
double sin_phi = std::sin(p_invFinG(1, 0));
double cos_phi = std::cos(p_invFinG(1, 0));
double rho = p_invFinG(2, 0);
// 构造雅可比矩阵
H_f.resize(3, 3);
H_f << -(1.0 / rho) * sin_th * sin_phi, (1.0 / rho) * cos_th * cos_phi, -(1.0 / (rho * rho)) * cos_th * sin_phi,
(1.0 / rho) * cos_th * sin_phi, (1.0 / rho) * sin_th * cos_phi, -(1.0 / (rho * rho)) * sin_th * sin_phi, 0.0,
-(1.0 / rho) * sin_phi, -(1.0 / (rho * rho)) * cos_phi;
return;
}
三维点特征的全局逆深度表示是通过以下方式给出的(类似于球坐标):
G
p
f
=
f
(
λ
)
=
1
ρ
[
cos
(
θ
)
sin
(
ϕ
)
sin
(
θ
)
sin
(
ϕ
)
cos
(
ϕ
)
]
where
λ
=
[
θ
ϕ
ρ
]
⊤
\begin{aligned} ^G\mathbf{p}_f& =\mathbf{f}(\boldsymbol{\lambda}) \\ &=\frac1\rho\begin{bmatrix}\cos(\theta)\sin(\phi)\\\sin(\theta)\sin(\phi)\\\cos(\phi)\end{bmatrix} \\ \text{where}\quad\boldsymbol{\lambda}&=\begin{bmatrix}\theta&\phi&\rho\end{bmatrix}^\top \end{aligned}
Gpfwhereλ=f(λ)=ρ1
cos(θ)sin(ϕ)sin(θ)sin(ϕ)cos(ϕ)
=[θϕρ]⊤
相对于特征参数的雅可比矩阵可以计算为:
f
(
⋅
)
∂
λ
=
[
−
1
ρ
sin
(
θ
)
sin
(
ϕ
)
1
ρ
cos
(
θ
)
cos
(
ϕ
)
−
1
ρ
2
cos
(
θ
)
sin
(
ϕ
)
1
ρ
cos
(
θ
)
sin
(
ϕ
)
1
ρ
sin
(
θ
)
cos
(
ϕ
)
−
1
ρ
2
sin
(
θ
)
sin
(
ϕ
)
0
−
1
ρ
sin
(
ϕ
)
−
1
ρ
2
cos
(
ϕ
)
]
\dfrac{\mathbf{f}(\cdot)}{\partial\boldsymbol{\lambda}}=\begin{bmatrix}-\frac{1}{\rho}\sin(\theta)\sin(\phi)&\frac{1}{\rho}\cos(\theta)\cos(\phi)&-\frac{1}{\rho^2}\cos(\theta)\sin(\phi)\\\frac{1}{\rho}\cos(\theta)\sin(\phi)&\frac{1}{\rho}\sin(\theta)\cos(\phi)&-\frac{1}{\rho^2}\sin(\theta)\sin(\phi)\\0&-\frac{1}{\rho}\sin(\phi)&-\frac{1}{\rho^2}\cos(\phi)\\\end{bmatrix}
∂λf(⋅)=
−ρ1sin(θ)sin(ϕ)ρ1cos(θ)sin(ϕ)0ρ1cos(θ)cos(ϕ)ρ1sin(θ)cos(ϕ)−ρ1sin(ϕ)−ρ21cos(θ)sin(ϕ)−ρ21sin(θ)sin(ϕ)−ρ21cos(ϕ)
锚定的XYZ
// 假设我们对于这个特征有一个锚定位姿
assert(feature.anchor_cam_id != -1);
// 对于锚定相机,我们需要考虑其定位和方向
Eigen::Matrix3d R_ItoC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->Rot();
Eigen::Vector3d p_IinC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->pos();
Eigen::Matrix3d R_GtoI = state->_clones_IMU.at(feature.anchor_clone_timestamp)->Rot();
Eigen::Vector3d p_IinG = state->_clones_IMU.at(feature.anchor_clone_timestamp)->pos();
Eigen::Vector3d p_FinA = feature.p_FinA;
// 如果我正在进行前端雅可比(FEJ)优化,我应该优化锚定状态(我们是否应该优化标定?)
// 如果需要的话,还应该获取特征的前端雅可比位置
if (state->_options.do_fej) {
// 在全局坐标系中的“最佳”特征
Eigen::Vector3d p_FinG_best = R_GtoI.transpose() * R_ItoC.transpose() * (feature.p_FinA - p_IinC) + p_IinG;
// 使用前端雅可比(FEJ)方法将最佳结果转换到我们的锚定坐标系中
R_GtoI = state->_clones_IMU.at(feature.anchor_clone_timestamp)->Rot_fej();
p_IinG = state->_clones_IMU.at(feature.anchor_clone_timestamp)->pos_fej();
p_FinA = (R_GtoI.transpose() * R_ItoC.transpose()).transpose() * (p_FinG_best - p_IinG) + p_IinC;
}
Eigen::Matrix3d R_CtoG = R_GtoI.transpose() * R_ItoC.transpose();
// 锚定姿态的雅可比矩阵
Eigen::Matrix<double, 3, 6> H_anc;
H_anc.block(0, 0, 3, 3).noalias() = -R_GtoI.transpose() * skew_x(R_ItoC.transpose() * (p_FinA - p_IinC));
H_anc.block(0, 3, 3, 3).setIdentity();
// 将锚定姿态的雅可比矩阵添加到我们的返回向量中
x_order.push_back(state->_clones_IMU.at(feature.anchor_clone_timestamp));
H_x.push_back(H_anc);
}
我们可以将一个3D点特征表示为某个“锚定”坐标系中(例如某个IMU本地坐标系,
{
I
a
G
R
,
G
p
I
a
}
\left\{\begin{smallmatrix}I_a\\G\end{smallmatrix}\mathbf{R},{^G\mathbf{p}}_{I_a}\right\}
{IaGR,GpIa}),通常是对应于第一次检测到该特征的相机帧的IMU姿态。
G
p
f
=
f
(
λ
,
I
a
G
R
,
G
p
I
a
,
I
C
R
,
C
p
I
)
=
G
I
a
R
⊤
I
C
R
⊤
(
λ
−
C
p
I
)
+
G
p
I
a
where
λ
=
C
a
p
f
=
[
C
a
x
C
a
y
C
a
z
]
⊤
\begin{aligned} ^G\mathbf{p}_f& =\mathbf{f}(\boldsymbol{\lambda},\begin{smallmatrix}I_a\\G\end{smallmatrix}\mathbf{R},{^G\mathbf{p}}_{I_a},{^C_I\mathbf{R}},^C\mathbf{p}_I) \\ &={}_G^{I_a}\mathbf{R}^{\top}{^C_I\mathbf{R}}^{\top}(\boldsymbol{\lambda}-{}^C\mathbf{p}_I)+{}^G\mathbf{p}_{I_a} \\ \text{where}\quad\boldsymbol{\lambda}&={}^{C_a}\mathbf{p}_f=\begin{bmatrix}^{C_a}x&^{C_a}y&^{C_a}z\end{bmatrix}^\top \end{aligned}
Gpfwhereλ=f(λ,IaGR,GpIa,ICR,CpI)=GIaR⊤ICR⊤(λ−CpI)+GpIa=Capf=[CaxCayCaz]⊤
由于锚定位姿参与了这种表示法,它的雅可比矩阵可以计算为:
∂
f
(
⋅
)
∂
G
I
a
R
=
−
G
I
a
R
⊤
⌊
I
C
R
⊤
(
C
a
p
f
−
C
p
I
)
×
⌋
∂
f
(
⋅
)
∂
G
p
I
a
=
I
3
×
3
\begin{aligned} \begin{aligned}\frac{\partial\mathbf{f}(\cdot)}{\partial_G^{I_a}\mathbf{R}}\end{aligned}& =-_G^{I_a}\mathbf{R}^\top\left\lfloor\begin{matrix}^C_I\mathbf{R}^\top(^{C_a}\mathbf{p}_f-^C\mathbf{p}_I)\times\end{matrix}\right\rfloor \\ \begin{aligned}\frac{\partial\mathbf{f}(\cdot)}{\partial^G\mathbf{p}I_a}\end{aligned}& =\mathbf{I}_{3\times3} \end{aligned}
∂GIaR∂f(⋅)∂GpIa∂f(⋅)=−GIaR⊤⌊ICR⊤(Capf−CpI)×⌋=I3×3
此外,如果进行外部标定,还需要以下相对于IMU-相机外参的雅可比矩阵:
// 获取标定的雅可比矩阵(用于锚点克隆)
if (state->_options.do_calib_camera_pose) {
Eigen::Matrix<double, 3, 6> H_calib;
H_calib.block(0, 0, 3, 3).noalias() = -R_CtoG * skew_x(p_FinA - p_IinC);
H_calib.block(0, 3, 3, 3) = -R_CtoG;
x_order.push_back(state->_calib_IMUtoCAM.at(feature.anchor_cam_id));
H_x.push_back(H_calib);
}
∂ f ( ⋅ ) ∂ I C R = − G I a R ⊤ I C R ⊤ ⌊ ( C a p f − C p I ) × ⌋ ∂ f ( ⋅ ) ∂ C p I = − G I a R ⊤ I C R ⊤ \begin{aligned}\frac{\partial\mathbf{f}(\cdot)}{\partial_I^C\mathbf{R}}&=-^{I_a}_G\mathbf{R}^{\top }{^C_I\mathbf{R}^{\top}}\left\lfloor{(^{C_a}\mathbf{p}_f-{}^C\mathbf{p}_I)\times}\right\rfloor\\\frac{\partial\mathbf{f}(\cdot)}{\partial^C\mathbf{p}_I}&=-^{I_a}_G\mathbf{R}^\top {^C_I\mathbf{R}^{\top}}\end{aligned} ∂ICR∂f(⋅)∂CpI∂f(⋅)=−GIaR⊤ICR⊤⌊(Capf−CpI)×⌋=−GIaR⊤ICR⊤
// 如果我们正在进行锚定的XYZ特征标定
if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_3D) {
H_f = R_CtoG;
return;
对于特征状态的雅可比矩阵可以表示为:
∂
f
(
⋅
)
∂
λ
=
G
I
a
R
⊤
I
C
R
⊤
\frac{\partial\mathbf{f}(\cdot)}{\partial\boldsymbol{\lambda}}=^{I_a}_{G}\mathbf{R}^\top{^C_I\mathbf{R}^\top}
∂λ∂f(⋅)=GIaR⊤ICR⊤
锚定的逆深度
// 如果我们正在进行完整的逆深度标定
if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_FULL_INVERSE_DEPTH) {
// 获取逆深度表示 (should match what is in Landmark.cpp)
double a_rho = 1 / p_FinA.norm();
double a_phi = std::acos(a_rho * p_FinA(2));
double a_theta = std::atan2(p_FinA(1), p_FinA(0));
Eigen::Matrix<double, 3, 1> p_invFinA;
p_invFinA(0) = a_theta;
p_invFinA(1) = a_phi;
p_invFinA(2) = a_rho;
// 使用锚定的逆深度
double sin_th = std::sin(p_invFinA(0, 0));
double cos_th = std::cos(p_invFinA(0, 0));
double sin_phi = std::sin(p_invFinA(1, 0));
double cos_phi = std::cos(p_invFinA(1, 0));
double rho = p_invFinA(2, 0);
// assert(p_invFinA(2,0)>=0.0);
// 锚定的三维位置相对于逆深度参数的雅可比矩阵
Eigen::Matrix<double, 3, 3> d_pfinA_dpinv;
d_pfinA_dpinv << -(1.0 / rho) * sin_th * sin_phi, (1.0 / rho) * cos_th * cos_phi, -(1.0 / (rho * rho)) * cos_th * sin_phi,
(1.0 / rho) * cos_th * sin_phi, (1.0 / rho) * sin_th * cos_phi, -(1.0 / (rho * rho)) * sin_th * sin_phi, 0.0,
-(1.0 / rho) * sin_phi, -(1.0 / (rho * rho)) * cos_phi;
H_f = R_CtoG * d_pfinA_dpinv;
return;
}
类似于全局逆深度情况,我们可以在锚定坐标系中使用带有方向的逆深度(类似于球面坐标),
{
I
a
G
R
,
G
p
I
a
}
\left\{\begin{smallmatrix}I_a\\G\end{smallmatrix}\mathbf{R},{^G\mathbf{p}}_{I_a}\right\}
{IaGR,GpIa},来表示一个三维点特征:
G
p
f
=
f
(
λ
,
G
I
a
R
,
G
p
I
a
,
I
C
R
,
C
p
I
)
=
G
I
a
R
⊤
I
C
R
⊤
(
C
a
p
f
−
C
p
I
)
+
G
p
I
a
=
G
I
a
R
⊤
I
C
R
⊤
(
1
ρ
[
cos
(
θ
)
sin
(
ϕ
)
sin
(
θ
)
sin
(
ϕ
)
cos
(
ϕ
)
]
−
C
p
I
)
+
G
p
I
a
where
λ
=
[
θ
ϕ
ρ
]
⊤
\begin{aligned} ^G\mathbf{p}_f& =\mathbf{f}(\boldsymbol{\lambda},{^{I_a}_G\mathbf{R}},{^G\mathbf{p}_{I_a}},{^C_I\mathbf{R}},{^C\mathbf{p}_I}) \\ &={_G^{I_a}\mathbf{R}^{\top}}{^C_I\mathbf{R}^{\top}}(^{C_a}\mathbf{p}_f-{}^C\mathbf{p}_I)+{}^G\mathbf{p}_{I_a} \\ &={^{I_a}_G\mathbf{R}^{\top}}{_I^{C}\mathbf{R}^{\top}}\Bigg(\frac1\rho\begin{bmatrix}\cos(\theta)\sin(\phi)\\\sin(\theta)\sin(\phi)\\\cos(\phi)\end{bmatrix}-{}^C\mathbf{p}_I\Bigg)+{}^G\mathbf{p}_{I_a} \\ \text{where}\quad\boldsymbol{\lambda}&=\begin{bmatrix}\theta&\phi&\rho\end{bmatrix}^\top \end{aligned}
Gpfwhereλ=f(λ,GIaR,GpIa,ICR,CpI)=GIaR⊤ICR⊤(Capf−CpI)+GpIa=GIaR⊤ICR⊤(ρ1
cos(θ)sin(ϕ)sin(θ)sin(ϕ)cos(ϕ)
−CpI)+GpIa=[θϕρ]⊤
对于特征状态的雅可比矩阵可以表示为:
∂
f
(
⋅
)
∂
λ
=
G
I
a
R
⊤
I
C
R
⊤
[
−
1
ρ
sin
(
θ
)
sin
(
ϕ
)
1
ρ
cos
(
θ
)
cos
(
ϕ
)
−
1
ρ
2
cos
(
θ
)
sin
(
ϕ
)
1
ρ
cos
(
θ
)
sin
(
ϕ
)
1
ρ
sin
(
θ
)
cos
(
ϕ
)
−
1
ρ
2
sin
(
θ
)
sin
(
ϕ
)
0
−
1
ρ
sin
(
ϕ
)
−
1
ρ
2
cos
(
ϕ
)
]
\frac{\partial\mathbf{f}(\cdot)}{\partial\boldsymbol{\lambda}}=^{I_a}_{G}\mathbf{R}^\top{^C_I\mathbf{R}^\top}\begin{bmatrix}-\frac{1}{\rho}\sin(\theta)\sin(\phi)&\frac{1}{\rho}\cos(\theta)\cos(\phi)&-\frac{1}{\rho^2}\cos(\theta)\sin(\phi)\\\frac{1}{\rho}\cos(\theta)\sin(\phi)&\frac{1}{\rho}\sin(\theta)\cos(\phi)&-\frac{1}{\rho^2}\sin(\theta)\sin(\phi)\\0&-\frac{1}{\rho}\sin(\phi)&-\frac{1}{\rho^2}\cos(\phi)\\\end{bmatrix}
∂λ∂f(⋅)=GIaR⊤ICR⊤
−ρ1sin(θ)sin(ϕ)ρ1cos(θ)sin(ϕ)0ρ1cos(θ)cos(ϕ)ρ1sin(θ)cos(ϕ)−ρ1sin(ϕ)−ρ21cos(θ)sin(ϕ)−ρ21sin(θ)sin(ϕ)−ρ21cos(ϕ)
相对于锚点姿态的雅可比矩阵为:
∂
f
(
⋅
)
∂
G
I
a
R
=
−
G
I
a
R
⊤
⌊
I
C
R
⊤
(
C
a
p
f
−
C
p
I
)
×
⌋
∂
f
(
⋅
)
∂
G
p
I
a
=
I
3
×
3
\begin{aligned} \begin{aligned}\frac{\partial\mathbf{f}(\cdot)}{\partial_G^{I_a}\mathbf{R}}\end{aligned}& =-_G^{I_a}\mathbf{R}^\top\left\lfloor\begin{matrix}^C_I\mathbf{R}^\top(^{C_a}\mathbf{p}_f-^C\mathbf{p}_I)\times\end{matrix}\right\rfloor \\ \begin{aligned}\frac{\partial\mathbf{f}(\cdot)}{\partial^G\mathbf{p}I_a}\end{aligned}& =\mathbf{I}_{3\times3} \end{aligned}
∂GIaR∂f(⋅)∂GpIa∂f(⋅)=−GIaR⊤⌊ICR⊤(Capf−CpI)×⌋=I3×3
锚定的逆深度(MSCKF版本)
// 如果我们正在使用MSCKF版本的逆深度表示
if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_MSCKF_INVERSE_DEPTH) {
// 获取逆深度表示 (should match what is in Landmark.cpp)
Eigen::Matrix<double, 3, 1> p_invFinA_MSCKF;
p_invFinA_MSCKF(0) = p_FinA(0) / p_FinA(2);
p_invFinA_MSCKF(1) = p_FinA(1) / p_FinA(2);
p_invFinA_MSCKF(2) = 1 / p_FinA(2);
// 使用MSCKF版本的逆深度表示
double alpha = p_invFinA_MSCKF(0, 0);
double beta = p_invFinA_MSCKF(1, 0);
double rho = p_invFinA_MSCKF(2, 0);
// 锚定的三维位置相对于逆深度参数
Eigen::Matrix<double, 3, 3> d_pfinA_dpinv;
d_pfinA_dpinv << (1.0 / rho), 0.0, -(1.0 / (rho * rho)) * alpha, 0.0, (1.0 / rho), -(1.0 / (rho * rho)) * beta, 0.0, 0.0,
-(1.0 / (rho * rho));
H_f = R_CtoG * d_pfinA_dpinv;
return;
}
请注意,在原始的MSCKF论文[32]中使用了更简化的逆深度表示。如果在特征被测量的相机坐标系中表示,这种表示方式不会出现奇异性。
G
p
f
=
f
(
λ
,
G
I
a
R
,
G
p
I
a
,
I
C
R
,
C
p
I
)
=
G
I
a
R
⊤
I
C
R
⊤
(
C
a
p
f
−
C
p
I
)
+
G
p
I
a
=
G
I
a
R
⊤
I
C
R
⊤
(
1
ρ
[
α
β
1
]
−
C
p
I
)
+
G
p
I
a
where
λ
=
[
α
β
ρ
]
⊤
\begin{aligned} ^G\mathbf{p}_f& =\mathbf{f}(\boldsymbol{\lambda},{^{I_a}_G\mathbf{R}},{^G\mathbf{p}_{I_a}},{^C_I\mathbf{R}},{^C\mathbf{p}_I}) \\ &=_G^{I_a}\mathbf{R}^{\top}{^C_I\mathbf{R}^{\top}}({}^{C_a}\mathbf{p}_f-{}^C\mathbf{p}_I)+{}^G\mathbf{p}_{I_a} \\ &=_G^{I_a}\mathbf{R}^{\top}{^C_I\mathbf{R}^{\top}}\left(\frac1\rho\begin{bmatrix}\alpha\\\beta\\1\end{bmatrix}-{}^C\mathbf{p}_I\right)+{}^G\mathbf{p}_{I_a} \\ \text{where}\quad\boldsymbol{\lambda}&=\begin{bmatrix}\alpha&\beta&\rho\end{bmatrix}^\top \end{aligned}
Gpfwhereλ=f(λ,GIaR,GpIa,ICR,CpI)=GIaR⊤ICR⊤(Capf−CpI)+GpIa=GIaR⊤ICR⊤
ρ1
αβ1
−CpI
+GpIa=[αβρ]⊤
相对于特征状态的雅可比矩阵:
∂
f
(
⋅
)
∂
λ
=
G
I
a
R
⊤
I
C
R
⊤
[
1
ρ
0
−
1
ρ
2
α
0
1
ρ
−
1
ρ
2
β
0
0
−
1
ρ
2
]
\dfrac{\partial\mathbf{f}(\cdot)}{\partial\boldsymbol{\lambda}}={^{I_a}_G\mathbf{R}^{\top }}{^C_I\mathbf{R}^{\top}}\begin{bmatrix}\frac{1}{\rho}&0&-\frac{1}{\rho^2}\alpha\\0&\frac{1}{\rho}&-\frac{1}{\rho^2}\beta\\0&0&-\frac{1}{\rho^2}\end{bmatrix}
∂λ∂f(⋅)=GIaR⊤ICR⊤
ρ1000ρ10−ρ21α−ρ21β−ρ21
锚定逆深度(MSCKF单深度版本)
/// 情景:使用初始方位角估计特征的单个深度值
if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_INVERSE_DEPTH_SINGLE) {
// 获取逆深度表示 (should match what is in Landmark.cpp)
double rho = 1.0 / p_FinA(2);
Eigen::Vector3d bearing = rho * p_FinA;
// 逆深度参数对锚定的三维位置的雅可比矩阵
Eigen::Vector3d d_pfinA_drho;
d_pfinA_drho << -(1.0 / (rho * rho)) * bearing;
H_f = R_CtoG * d_pfinA_drho;
return;
}
这种特征表示基于MSCKF表示和VINS-Mono中的单深度表示。与VINS-Mono中的实现相比,我们在处理特征的方位角时非常小心。在初始化过程中,我们首先初始化一个完整的三维特征,然后通过边缘化其方位角部分,将深度保留在状态向量中。边缘化的方位角在所有未来的线性化中保持不变。
然后在更新过程中,我们在每个时间步骤执行零空间投影,以消除特征对该方位角的依赖。为了做到这一点,我们需要至少两组
U
V
UV
UV测量数据来执行这个方位角零空间操作,因为在这个过程中我们失去了特征的两个维度。我们可以将特征测量函数定义如下:
G
p
f
=
f
(
λ
,
G
I
a
R
,
G
p
I
a
,
I
C
R
,
C
p
I
)
=
G
I
a
R
⊤
I
C
R
⊤
(
C
a
p
f
−
C
p
I
)
+
G
p
I
a
=
G
I
a
R
⊤
I
C
R
⊤
(
1
ρ
b
^
−
C
p
I
)
+
G
p
I
a
where
λ
=
[
ρ
]
\begin{aligned} ^G\mathbf{p}_f& =\mathbf{f}(\boldsymbol{\lambda},{^{I_a}_G\mathbf{R}},{^G\mathbf{p}_{I_a}},{^C_I\mathbf{R}},{^C\mathbf{p}_I}) \\ & =_G^{I_a}\mathbf{R}^{\top}{^C_I\mathbf{R}^{\top}}(^{C_a}\mathbf{p}_f-{}^C\mathbf{p}_I)+{}^G\mathbf{p}_{I_a} \\ & =_G^{I_a}\mathbf{R}^{\top}{_I^C\mathbf{R}^{\top}}\left(\frac1\rho\hat{\mathbf{b}}-{}^C\mathbf{p}_I\right)+{}^G\mathbf{p}_{I_a} \\ \text{where}\quad\boldsymbol{\lambda}&=\begin{bmatrix}\rho\end{bmatrix} \end{aligned}
Gpfwhereλ=f(λ,GIaR,GpIa,ICR,CpI)=GIaR⊤ICR⊤(Capf−CpI)+GpIa=GIaR⊤ICR⊤(ρ1b^−CpI)+GpIa=[ρ]
在上述情况中,我们定义了一个方位角
b
^
\hat{\mathbf{b}}
b^,它是初始化后特征的边缘化方位角。在收集到两个测量值之后,我们可以进行零空间投影,以消除相对于这个方位角变量的雅可比矩阵。
完整代码:
void UpdaterHelper::get_feature_jacobian_representation(std::shared_ptr<State> state, UpdaterHelperFeature &feature, Eigen::MatrixXd &H_f,
std::vector<Eigen::MatrixXd> &H_x, std::vector<std::shared_ptr<Type>> &x_order) {
// 全局XYZ表示
if (feature.feat_representation == LandmarkRepresentation::Representation::GLOBAL_3D) {
H_f.resize(3, 3);
H_f.setIdentity();
return;
}
// 全局逆深度表示
if (feature.feat_representation == LandmarkRepresentation::Representation::GLOBAL_FULL_INVERSE_DEPTH) {
// 获取线性化特征点
Eigen::Matrix<double, 3, 1> p_FinG = (state->_options.do_fej) ? feature.p_FinG_fej : feature.p_FinG;
// 获取逆深度表示 (should match what is in Landmark.cpp)
double g_rho = 1 / p_FinG.norm();
double g_phi = std::acos(g_rho * p_FinG(2));
// double g_theta = std::asin(g_rho*p_FinG(1)/std::sin(g_phi));
double g_theta = std::atan2(p_FinG(1), p_FinG(0));
Eigen::Matrix<double, 3, 1> p_invFinG;
p_invFinG(0) = g_theta;
p_invFinG(1) = g_phi;
p_invFinG(2) = g_rho;
// 获取逆深度方位角
double sin_th = std::sin(p_invFinG(0, 0));
double cos_th = std::cos(p_invFinG(0, 0));
double sin_phi = std::sin(p_invFinG(1, 0));
double cos_phi = std::cos(p_invFinG(1, 0));
double rho = p_invFinG(2, 0);
// 构造雅可比矩阵
H_f.resize(3, 3);
H_f << -(1.0 / rho) * sin_th * sin_phi, (1.0 / rho) * cos_th * cos_phi, -(1.0 / (rho * rho)) * cos_th * sin_phi,
(1.0 / rho) * cos_th * sin_phi, (1.0 / rho) * sin_th * cos_phi, -(1.0 / (rho * rho)) * sin_th * sin_phi, 0.0,
-(1.0 / rho) * sin_phi, -(1.0 / (rho * rho)) * cos_phi;
return;
}
//======================================================================
//======================================================================
//======================================================================
// 假设我们对于这个特征有一个锚定位姿
assert(feature.anchor_cam_id != -1);
// 对于锚定相机,我们需要考虑其定位和方向
Eigen::Matrix3d R_ItoC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->Rot();
Eigen::Vector3d p_IinC = state->_calib_IMUtoCAM.at(feature.anchor_cam_id)->pos();
Eigen::Matrix3d R_GtoI = state->_clones_IMU.at(feature.anchor_clone_timestamp)->Rot();
Eigen::Vector3d p_IinG = state->_clones_IMU.at(feature.anchor_clone_timestamp)->pos();
Eigen::Vector3d p_FinA = feature.p_FinA;
// 如果我正在进行前端雅可比(FEJ)优化,我应该优化锚定状态(我们是否应该优化标定?)
// 如果需要的话,还应该获取特征的前端雅可比位置
if (state->_options.do_fej) {
// 在全局坐标系中的“最佳”特征
Eigen::Vector3d p_FinG_best = R_GtoI.transpose() * R_ItoC.transpose() * (feature.p_FinA - p_IinC) + p_IinG;
// 使用前端雅可比(FEJ)方法将最佳结果转换到我们的锚定坐标系中
R_GtoI = state->_clones_IMU.at(feature.anchor_clone_timestamp)->Rot_fej();
p_IinG = state->_clones_IMU.at(feature.anchor_clone_timestamp)->pos_fej();
p_FinA = (R_GtoI.transpose() * R_ItoC.transpose()).transpose() * (p_FinG_best - p_IinG) + p_IinC;
}
Eigen::Matrix3d R_CtoG = R_GtoI.transpose() * R_ItoC.transpose();
// 锚定姿态的雅可比矩阵
Eigen::Matrix<double, 3, 6> H_anc;
H_anc.block(0, 0, 3, 3).noalias() = -R_GtoI.transpose() * skew_x(R_ItoC.transpose() * (p_FinA - p_IinC));
H_anc.block(0, 3, 3, 3).setIdentity();
// 将锚定姿态的雅可比矩阵添加到我们的返回向量中
x_order.push_back(state->_clones_IMU.at(feature.anchor_clone_timestamp));
H_x.push_back(H_anc);
// 获取标定的雅可比矩阵(用于锚点克隆)
if (state->_options.do_calib_camera_pose) {
Eigen::Matrix<double, 3, 6> H_calib;
H_calib.block(0, 0, 3, 3).noalias() = -R_CtoG * skew_x(p_FinA - p_IinC);
H_calib.block(0, 3, 3, 3) = -R_CtoG;
x_order.push_back(state->_calib_IMUtoCAM.at(feature.anchor_cam_id));
H_x.push_back(H_calib);
}
// 如果我们正在进行锚定的XYZ特征标定
if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_3D) {
H_f = R_CtoG;
return;
}
// 如果我们正在进行完整的逆深度标定
if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_FULL_INVERSE_DEPTH) {
// 获取逆深度表示 (should match what is in Landmark.cpp)
double a_rho = 1 / p_FinA.norm();
double a_phi = std::acos(a_rho * p_FinA(2));
double a_theta = std::atan2(p_FinA(1), p_FinA(0));
Eigen::Matrix<double, 3, 1> p_invFinA;
p_invFinA(0) = a_theta;
p_invFinA(1) = a_phi;
p_invFinA(2) = a_rho;
// 使用锚定的逆深度
double sin_th = std::sin(p_invFinA(0, 0));
double cos_th = std::cos(p_invFinA(0, 0));
double sin_phi = std::sin(p_invFinA(1, 0));
double cos_phi = std::cos(p_invFinA(1, 0));
double rho = p_invFinA(2, 0);
// assert(p_invFinA(2,0)>=0.0);
// 锚定的三维位置相对于逆深度参数的雅可比矩阵
Eigen::Matrix<double, 3, 3> d_pfinA_dpinv;
d_pfinA_dpinv << -(1.0 / rho) * sin_th * sin_phi, (1.0 / rho) * cos_th * cos_phi, -(1.0 / (rho * rho)) * cos_th * sin_phi,
(1.0 / rho) * cos_th * sin_phi, (1.0 / rho) * sin_th * cos_phi, -(1.0 / (rho * rho)) * sin_th * sin_phi, 0.0,
-(1.0 / rho) * sin_phi, -(1.0 / (rho * rho)) * cos_phi;
H_f = R_CtoG * d_pfinA_dpinv;
return;
}
// 如果我们正在使用MSCKF版本的逆深度表示
if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_MSCKF_INVERSE_DEPTH) {
// 获取逆深度表示 (should match what is in Landmark.cpp)
Eigen::Matrix<double, 3, 1> p_invFinA_MSCKF;
p_invFinA_MSCKF(0) = p_FinA(0) / p_FinA(2);
p_invFinA_MSCKF(1) = p_FinA(1) / p_FinA(2);
p_invFinA_MSCKF(2) = 1 / p_FinA(2);
// 使用MSCKF版本的逆深度表示
double alpha = p_invFinA_MSCKF(0, 0);
double beta = p_invFinA_MSCKF(1, 0);
double rho = p_invFinA_MSCKF(2, 0);
// 锚定的三维位置相对于逆深度参数
Eigen::Matrix<double, 3, 3> d_pfinA_dpinv;
d_pfinA_dpinv << (1.0 / rho), 0.0, -(1.0 / (rho * rho)) * alpha, 0.0, (1.0 / rho), -(1.0 / (rho * rho)) * beta, 0.0, 0.0,
-(1.0 / (rho * rho));
H_f = R_CtoG * d_pfinA_dpinv;
return;
}
/// 情景:使用初始方位角估计特征的单个深度值
if (feature.feat_representation == LandmarkRepresentation::Representation::ANCHORED_INVERSE_DEPTH_SINGLE) {
// 获取逆深度表示 (should match what is in Landmark.cpp)
double rho = 1.0 / p_FinA(2);
Eigen::Vector3d bearing = rho * p_FinA;
// 逆深度参数对锚定的三维位置的雅可比矩阵
Eigen::Vector3d d_pfinA_drho;
d_pfinA_drho << -(1.0 / (rho * rho)) * bearing;
H_f = R_CtoG * d_pfinA_drho;
return;
}
// 失败,无效的表示,未编程
assert(false);
}
UpdaterHelper::get_feature_jacobian_full
透视投影(方位角)测量模型
// Get current IMU clone state
std::shared_ptr<PoseJPL> clone_Ii = state->_clones_IMU.at(feature.timestamps[pair.first].at(m));
Eigen::Matrix3d R_GtoIi = clone_Ii->Rot();
Eigen::Vector3d p_IiinG = clone_Ii->pos();
// Get current feature in the IMU
Eigen::Vector3d p_FinIi = R_GtoIi * (p_FinG - p_IiinG);
// Project the current feature into the current frame of reference
Eigen::Vector3d p_FinCi = R_ItoC * p_FinIi + p_IinC;
Eigen::Vector2d uv_norm;
uv_norm << p_FinCi(0) / p_FinCi(2), p_FinCi(1) / p_FinCi(2);
// Distort the normalized coordinates (radtan or fisheye)
Eigen::Vector2d uv_dist;
uv_dist = state->_cam_intrinsics_cameras.at(pair.first)->distort_d(uv_norm);
// Our residual
Eigen::Vector2d uv_m;
uv_m << (double)feature.uvs[pair.first].at(m)(0), (double)feature.uvs[pair.first].at(m)(1);
res.block(2 * c, 0, 2, 1) = uv_m - uv_dist;
考虑在某个时间点
k
k
k从相机图像中检测到一个三维特征点,其在图像平面上的测量值
u
v
uv
uv(即对应的像素坐标)如下所示:
Z
m
,
k
=
h
(
x
k
)
+
n
k
=
h
d
(
z
n
,
k
,
ζ
)
+
n
k
=
h
d
(
h
p
(
C
k
p
f
)
,
ζ
)
+
n
k
=
h
d
(
h
p
(
h
t
(
G
p
f
,
G
C
k
R
,
G
p
C
k
)
)
,
ζ
)
+
n
k
=
h
d
(
h
p
(
h
t
(
h
r
(
λ
,
⋯
)
,
G
C
k
R
,
G
p
C
k
)
)
,
ζ
)
+
n
k
\begin{aligned} \mathbf{Z}_{m,k}&=\mathbf{h}(\mathbf{x}_k)\boldsymbol{+}\mathbf{n}_k \\ &=\mathbf{h}_d(\mathbf{z}_{n,k},\boldsymbol{\zeta})+\mathbf{n}_k \\ &=\mathbf{h}_d(\mathbf{h}_p(^{C_k}\mathbf{p}_f),\boldsymbol{\zeta})+\mathbf{n}_k \\ &=\mathbf{h}_d(\mathbf{h}_p(\mathbf{h}_t(^G\mathbf{p}_f,{^{C_k}_G\mathbf{R}},^G\mathbf{p}_{C_k})),\boldsymbol{\zeta})+\mathbf{n}_k\\ &=\mathbf{h}_d(\mathbf{h}_p(\mathbf{h}_t(\mathbf{h}_r(\boldsymbol{\lambda},\cdots),{^{C_k}_G\mathbf{R}},^G\mathbf{p}_{C_k})),\boldsymbol{\zeta})+\mathbf{n}_k \end{aligned}
Zm,k=h(xk)+nk=hd(zn,k,ζ)+nk=hd(hp(Ckpf),ζ)+nk=hd(hp(ht(Gpf,GCkR,GpCk)),ζ)+nk=hd(hp(ht(hr(λ,⋯),GCkR,GpCk)),ζ)+nk
其中,
n
k
\mathbf{n}_k
nk是测量噪声,通常假设为零均值的白噪声;
Z
m
,
k
\mathbf{Z}_{m,k}
Zm,k是归一化的无畸变
u
v
uv
uv测量值;
ζ
\zeta
ζ是相机的内参数,如焦距和畸变参数;
C
k
p
f
^{C_k}\mathbf{p}_f
Ckpf是特征点在当前相机坐标系
{
C
k
}
\left\{C_k\right\}
{Ck}中的位置;
G
p
f
^{G}\mathbf{p}_f
Gpf是特征点在全局坐标系
{
G
}
\left\{G\right\}
{G}中的位置;
{
G
C
k
R
,
G
p
C
k
)
}
\left\{{^{C_k}_G\mathbf{R}},^G\mathbf{p}_{C_k})\right\}
{GCkR,GpCk)}表示当前相机的姿态(位置和方向)相对于全局坐标系的变换;
λ
\lambda
λ是特征点的其他参数表示(位置之外的参数),如简单的xyz位置或具有方位角的逆深度。
在上述表达式中,我们将测量函数分解为多个函数的串联,这些函数将状态映射到图像平面上的原始 u v uv uv测量值。值得注意的是,由于我们将进行内外参数的校准,并使用不同的特征表示,上述相机测量模型是通用的。
UpdaterHelper::nullspace_project_inplace
- 目的
零空间投影就是为了去除误差模型对三维点的依赖. 换句话说(不严谨): 使观测模型与三维点无关, 把三维点边缘化掉
对于误差函数的线性展开: r = H X + n r=HX+n r=HX+n , ( r r r 为residual, H H H 为Jacobian, n n n 为nosie), 需要保证等号右边除了 H X HX HX 之外其他项都与 X X X 无关, 才能将 H H H 给到EKF 进行更新.
MSCKF 的状态量不包含三维特征点. 但是在构建重投影误差时, 需要三维点的坐标. 令待估计状态量
X
X
X, 观测对
X
X
X 的Jacobian 为
H
X
H_X
HX , 观测对三维点
P
P
P 的Jacobian 为
H
f
H_f
Hf , 观测噪声为
n
n
n , 观测误差的一阶泰勒展开如下:
r
=
z
−
h
(
X
,
P
)
≃
H
X
X
~
+
H
f
P
~
+
n
(
1
)
r=z-h(X,P)\simeq H_X\tilde{X}+H_f\tilde{P}+n\quad(1)
r=z−h(X,P)≃HXX~+HfP~+n(1)
由此引出一个问题: 上述泰勒近似模型的
P
~
\tilde{P}
P~与
X
X
X 不是无关的,
H
x
H_x
Hx 是不能直接用于EKF更新的.因此,我们想要消除
H
f
P
~
H_f\tilde{P}
HfP~
- 原理
Brief: 想要消除等式某一项,只需要在等式左右同时乘一个矩阵使得这项为0即可.
- 实现
对于式(1),采用上述方法计算出对应的
A
A
A ,对等式两边同时左乘
A
⊤
A^\top
A⊤ :
r
O
=
A
T
(
z
−
h
(
X
,
P
)
)
≃
A
T
H
X
X
~
+
A
T
n
=
H
O
X
~
+
n
O
(
2
)
r_O=A^T(z-h(X,P))\simeq A^TH_X\tilde{X}+A^Tn=H_O\tilde{X}+n_O\quad(2)
rO=AT(z−h(X,P))≃ATHXX~+ATn=HOX~+nO(2)
然后, 将
r
O
r_O
rO 作为EKF的residual,
H
O
H_O
HO作为EKF的Jacobian, 进行EKF Update.
式(2)提供了点 P P P 的所有观测帧之间的共视约束, 它体现了观测 z z z 能给到的全部有效信息,因此EKF的结果是最优的(除了线性展开带来的误差).
H f H_f Hf是列满秩的, 通常行数 m m m 大于列数 n n n (对于 X Y Z XYZ XYZ 表示, n n n=3 ), 它的零空间维度是 m − n m-n m−n .因此,投影后的残差项 r O r_O rO 是 ( m − n ) ∗ 1 (m-n)*1 (m−n)∗1 维的向量.
在代码实现时,其实并不需要显式的求解 A A A . EKF需要的三个量 r O r_O rO, H O H_O HO, n O n_O nO 的协方差矩阵 R O R_O RO 求解如下:
- ** r O r_O rO和 H O H_O HO : **
r r r 和 H x H_x Hx 在 H f H_f Hf 零空间的投影可以通过Givens Rotation快速计算得出.
- R o R_o Ro :
R O = A T R A ( 3 ) R_O=A^TRA\quad(3) RO=ATRA(3)
完整代码:
void UpdaterHelper::nullspace_project_inplace(Eigen::MatrixXd &H_f, Eigen::MatrixXd &H_x, Eigen::VectorXd &res) {
// 对所有变量应用H_f的左零空间
// 基于《矩阵计算》第四版(Golub和Van Loan)中的算法
// 参见第252页,算法5.2.4,了解这两个循环的工作原理
// 它们使用“matlab”索引表示法,因此我们需要从所有索引中减去1
Eigen::JacobiRotation<double> tempHo_GR;
for (int n = 0; n < H_f.cols(); ++n) {
for (int m = (int)H_f.rows() - 1; m > n; m--) {
// Givens矩阵G
tempHo_GR.makeGivens(H_f(m - 1, n), H_f(m, n));
// 将G乘以每个矩阵中对应的行(m-1, m)
// Note: 我们只将G应用于非零列[n:Ho.cols()-n-1],虽然它等效于将G应用于整个列[0:Ho.cols()-1].
(H_f.block(m - 1, n, 2, H_f.cols() - n)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
(H_x.block(m - 1, 0, 2, H_x.cols())).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
(res.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
}
}
// 如果H_f雅可比矩阵是一个3D位置,其最大秩为3,因此左零空间的大小为Hf.rows()-3
// NOTE: 需要使用eigen3进行评估,因为这里会出现别名问题!
// H_f = H_f.block(H_f.cols(),0,H_f.rows()-H_f.cols(),H_f.cols()).eval();
H_x = H_x.block(H_f.cols(), 0, H_x.rows() - H_f.cols(), H_x.cols()).eval();
res = res.block(H_f.cols(), 0, res.rows() - H_f.cols(), res.cols()).eval();
// 合理性检查
assert(H_x.rows() == res.rows());
}
UpdaterHelper::measurement_compress_inplace
- 目的
EKF中的 H H H 矩阵, 行数由观测数量决定,列数是状态量维度。行数会随着观测数量增长到很大(对于单个点 P P P , 观测数量为 M P M_P MP,每一个为2维 u v uv uv ,则行数为 2 M P M_P MP ),使得矩阵运算的计算量急速增大.
通常,观测数量远大于状态量维度, H H H 矩阵行数远大于列数. 通过把residual投影到 H H H 列空间基向量上实现降维, 可以把 H H H 行数减少到与列数相同,就可以节省EKF Update中矩阵运算的时间.
对于 H H H 是胖矩阵 (行数小于列数)的情况,就没有必要进行模型压缩了
- 原理:
QR分解
对
H
H
H 矩阵进行QR分解
H
=
[
Q
1
Q
2
]
[
T
H
0
]
(
4
)
\left.H=\left[\begin{array}{cc}Q_1&Q_2\end{array}\right.\right]\left[\begin{array}{c}T_H\\0\end{array}\right]\quad(4)
H=[Q1Q2][TH0](4)
其中
Q
1
Q_1
Q1 是
H
H
H列空间的基,
Q
2
Q_2
Q2 是
H
H
H 零空间的基,
T
H
T_H
TH 是上三角矩阵.则式(2)可得出:
r
O
=
[
Q
1
Q
2
]
[
T
H
0
]
X
~
+
n
O
(
5
)
r_O=[\begin{array}{cc}Q_1&Q_2\end{array}]\begin{bmatrix}T_H\\0\end{bmatrix}\tilde{X}+n_O\quad(5)
rO=[Q1Q2][TH0]X~+nO(5)
两边同乘
[
Q
1
T
Q
2
T
]
\begin{bmatrix}Q_1^T\\Q_2^T\end{bmatrix}
[Q1TQ2T]:
[
Q
1
T
r
O
Q
2
T
r
O
]
=
[
T
H
0
]
X
~
+
[
Q
1
T
n
O
Q
2
T
n
O
]
(
6
)
\begin{bmatrix}Q_1^Tr_O\\Q_2^Tr_O\end{bmatrix}=\begin{bmatrix}T_H\\0\end{bmatrix}\tilde{X}+\begin{bmatrix}Q_1^Tn_O\\Q_2^Tn_O\end{bmatrix}\quad(6)
[Q1TrOQ2TrO]=[TH0]X~+[Q1TnOQ2TnO](6)
通过上述方法, 将residual
r
O
r_O
rO 投影到
H
H
H 列空间的基上,可以保留观测的所有有效信息.而
Q
2
T
n
O
Q_2^Tn_O
Q2TnO 完全是噪声,可以直接抛弃.然后, 将以下残差模型用于EKF Update:
r
n
=
Q
1
T
r
O
=
T
H
X
~
+
n
n
(
7
)
r_n=Q_1^Tr_O=T_H\tilde{X}+n_n\quad(7)
rn=Q1TrO=THX~+nn(7)
其中噪声
n
n
n_n
nn的协方差矩阵为.
R
O
=
Q
1
T
R
Q
1
(
8
)
R_O=Q_1^TRQ_1\quad(8)
RO=Q1TRQ1(8)
Q
1
Q_1
Q1的求解方法参考UpdaterHelper::nullspace_project_inplace.
void UpdaterHelper::measurement_compress_inplace(Eigen::MatrixXd &H_x, Eigen::VectorXd &res) {
// Return if H_x is a fat matrix (there is no need to compress in this case)
if (H_x.rows() <= H_x.cols())
return;
// Do measurement compression through givens rotations
// Based on "Matrix Computations 4th Edition by Golub and Van Loan"
// See page 252, Algorithm 5.2.4 for how these two loops work
// They use "matlab" index notation, thus we need to subtract 1 from all index
Eigen::JacobiRotation<double> tempHo_GR;
for (int n = 0; n < H_x.cols(); n++) {
for (int m = (int)H_x.rows() - 1; m > n; m--) {
// Givens matrix G
tempHo_GR.makeGivens(H_x(m - 1, n), H_x(m, n));
// Multiply G to the corresponding lines (m-1,m) in each matrix
// Note: we only apply G to the nonzero cols [n:Ho.cols()-n-1], while
// it is equivalent to applying G to the entire cols [0:Ho.cols()-1].
(H_x.block(m - 1, n, 2, H_x.cols() - n)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
(res.block(m - 1, 0, 2, 1)).applyOnTheLeft(0, 1, tempHo_GR.adjoint());
}
}
// If H is a fat matrix, then use the rows
// Else it should be same size as our state
int r = std::min(H_x.rows(), H_x.cols());
// Construct the smaller jacobian and residual after measurement compression
assert(r <= H_x.rows());
H_x.conservativeResize(r, H_x.cols());
res.conservativeResize(r, res.cols());
}