锐客网

  1. 首页 > it技术 > >

VINS-Fusion代码阅读（四）

pts_i和pts_j：具体指什么含义？（分别为第l个路标点在第i, j个相机归一化相机坐标系中的观察到的坐标，Pˉˉˉcil \bar{P}^{c_i}_l
P
ˉ

l
c
i
?

?
和 Pˉˉˉcjl \bar{P}^{c_j}_l
P
ˉ

l
c
j
?

?
）；
tangent_base：正切平面上的任意两个正交基（在构造函数中通过计算？被赋值）；
静态数据成员sqrt_info和sum_t：在何时被赋值的呢？
class ProjectionFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1>
{
public:
ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j);
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;
void check(double **parameters);
Eigen::Vector3d pts_i, pts_j;
Eigen::Matrix tangent_base;
static Eigen::Matrix2d sqrt_info;
static double sum_t;
};
1
2
3
4
5
6
7
8
9
10
11
12
13
步入正题，ProjectionFactor类中的Evaluate成员函数：
优化变量：[pwbi,qwbi],[pwbj,qwbj],[pbc,qbc],λl [p^w_{b_i}, q^w_{b_i}], [p^w_{b_j}, q^w_{b_j}], [p^b_c, q^b_c], \lambda_l[p
b
i
?

w
?
,q
b
i
?

w
?
],[p
b
j
?

w
?
,q
b
j
?

w
?
],[p
c
b
?
,q
c
b
?
],λ
l
?

分别对应：[Pi, Qi], [Pj, Qj], [tic, qic], inv_dep_i
（此处的λ \lambdaλ表示什么含义？逆深度inv_dep_i）（w,b w, bw,b代表的坐标系具体指什么？）
解析中（27）式：
Pcjl=Rcb{Rbjw[Rwbi(Rbc1λlPˉˉˉcil+pbc)+pwbi?pwbj]?pbc} P^{c_j}_l=R^c_b\{R^{b_j}_w[R^w_{b_i}(R^b_c\frac{1}{\lambda_l}\bar{P}^{c_i}_l+p^b_c)+p^w_{b_i}-p^w_{b_j}]-p^b_c\}P
l
c
j
?

?
=R
b
c
?
{R
w
b
j
?

?
[R
b
i
?

w
?
(R
c
b
?

λ
l
?

1
?

P
ˉ

l
c
i
?

?
+p
c
b
?
)+p
b
i
?

w
?
?p
b
j
?

w
?
]?p
c
b
?
}
1λlPˉˉˉcil \frac{1}{\lambda_l}\bar{P}^{c_i}_l
λ
l
?

1
?

P
ˉ

l
c
i
?

?
==>Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
（因此，pts_i表示Pˉˉˉcil \bar{P}^{c_i}_l
P
ˉ

l
c
i
?

?
，为第l个路标点在第i个相机归一化相机坐标系中的观察到的坐标）；
(Rbc?+pbc) (R^b_c*+p^b_c)(R
c
b
?
?+p
c
b
?
) ==>Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
（此处可以看出来，imu与b bb系是一个系？）
Rwbi?+pwbi R^w_{b_i}*+p^w_{b_i}R
b
i
?

w
?
?+p
b
i
?

w
?
==>Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
Rbjw(??pwbj) R^{b_j}_w(*-p^w_{b_j})R
w
b
j
?

?
(??p
b
j
?

w
?
) ==>Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
Rcb(??pbc) R^c_b(*-p^b_c)R
b
c
?
(??p
c
b
?
) ==>Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
因此，pts_camera_j表示Pcjl P^{c_j}_lP
l
c
j
?

?
，是由Pˉˉˉcil \bar{P}^{c_i}_l
P
ˉ

l
c
i
?

?
计算得来的。
解析中（25）式：
rc(z?cjl,X)=[b1? b2? ](Pcjl∣∣Pcjl∣∣?Pˉˉˉcjl) r_c(\hat{z}^{c_j}_l,X)=\begin{bmatrix}\vec{b_1}\\ \vec{b_2} \end{bmatrix}(\frac{P^{c_j}_l}{||P^{c_j}_l||}-\bar{P}^{c_j}_l)r
c
?
(
z
^

l
c
j
?

?
,X)=[
b
1
?

?

b
2
?

?

?
](
∣∣P
l
c
j
?

?
∣∣
P
l
c
j
?

?

?
?
P
ˉ

l
c
j
?

?
)
residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
residual = sqrt_info * residual;
最后，计算Jacobian，解析中（28）式：
注：此处解析上有一些矩阵维数上的错误，应该为3×? 3\times *3×?，而非3×? 3\times *3×?。
以其中一个为例，分析公式与代码对应关系：
J[0]2×7=[?rc?pwbi,?rc?qwbi] J[0]^{2\times 7}=[\frac{\partial r_c}{\partial p^w_{b_i}}, \frac{\partial r_c}{\partial q^w_{b_i}}]J[0]
2×7
=[
?p
b
i
?

w
?

?r
c
?

?
,
?q
b
i
?

w
?

?r
c
?

?
]
代码中首先定义了一个jaco_i为3×6 3\times 63×6，然后用一个reduce2×3 2\times 32×3去乘，得到的2×6 2\times 62×6的结果作为J[0] J[0]J[0]的左边6列，最后一列为0；具体如下：
Eigen::Matrix jaco_i;
RcbRbjw R^c_bR^{b_j}_wR
b
c
?
R
w
b
j
?

?

jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();
?RcbRbjwRwbi(Rbc1λlPˉˉˉcil+pbc) -R^c_bR^{b_j}_wR^w_{b_i}(R^b_c\frac{1}{\lambda_l}\bar{P}^{c_i}_l+p^b_c)?R
b
c
?
R
w
b
j
?

?
R
b
i
?

w
?
(R
c
b
?

λ
l
?

1
?

P
ˉ

l
c
i
?

?
+p
c
b
?
)
jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);
Eigen::Matrix reduce(2, 3);
reduce = tangent_base * norm_jaco; 此处norm_jaco表达什么含义？对应公式？
reduce = sqrt_info * reduce;
Eigen::Map的用法？
Eigen::Map> jacobian_pose_i(jacobians[0]);
jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
jacobian_pose_i.rightCols<1>().setZero();
J[1]2×7 J[1]^{2\times 7}J[1]
2×7
、J[2]2×7 J[2]^{2\times 7}J[2]
2×7
和J[3]2×1 J[3]^{2\times 1}J[3]
2×1
的计算类似。
至此，视觉约束暂时告一段落。

bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
TicToc tic_toc;
Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]);
Eigen::Quaterniond Qj(parameters[1][6], parameters[1][3], parameters[1][4], parameters[1][5]);
Eigen::Vector3d tic(parameters[2][0], parameters[2][1], parameters[2][2]);
Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);
double inv_dep_i = parameters[3][0];
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
Eigen::Map residual(residuals);
#ifdef UNIT_SPHERE_ERROR
residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif
residual = sqrt_info * residual;
if (jacobians)
{
Eigen::Matrix3d Ri = Qi.toRotationMatrix();
Eigen::Matrix3d Rj = Qj.toRotationMatrix();
Eigen::Matrix3d ric = qic.toRotationMatrix();
Eigen::Matrix reduce(2, 3);
#ifdef UNIT_SPHERE_ERROR
double norm = pts_camera_j.norm();
Eigen::Matrix3d norm_jaco;
double x1, x2, x3;
x1 = pts_camera_j(0);
x2 = pts_camera_j(1);
x3 = pts_camera_j(2);
norm_jaco << 1.0 / norm - x1 * x1 / pow(norm, 3), - x1 * x2 / pow(norm, 3), - x1 * x3 / pow(norm, 3),
- x1 * x2 / pow(norm, 3), 1.0 / norm - x2 * x2 / pow(norm, 3), - x2 * x3 / pow(norm, 3),
- x1 * x3 / pow(norm, 3), - x2 * x3 / pow(norm, 3), 1.0 / norm - x3 * x3 / pow(norm, 3);
reduce = tangent_base * norm_jaco;
#else
reduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),
0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
#endif
reduce = sqrt_info * reduce;
if (jacobians[0])
{
Eigen::Map> jacobian_pose_i(jacobians[0]);
【VINS-Fusion代码阅读（四）】 Eigen::Matrix jaco_i;
jaco_i.leftCols<3>() = ric.transpose() * Rj.transpose();
jaco_i.rightCols<3>() = ric.transpose() * Rj.transpose() * Ri * -Utility::skewSymmetric(pts_imu_i);
jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
jacobian_pose_i.rightCols<1>().setZero();
}
if (jacobians[1])
{
Eigen::Map> jacobian_pose_j(jacobians[1]);
Eigen::Matrix jaco_j;
jaco_j.leftCols<3>() = ric.transpose() * -Rj.transpose();
jaco_j.rightCols<3>() = ric.transpose() * Utility::skewSymmetric(pts_imu_j);
jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
jacobian_pose_j.rightCols<1>().setZero();
}
if (jacobians[2])
{
Eigen::Map> jacobian_ex_pose(jacobians[2]);
Eigen::Matrix jaco_ex;
jaco_ex.leftCols<3>() = ric.transpose() * (Rj.transpose() * Ri - Eigen::Matrix3d::Identity());
Eigen::Matrix3d tmp_r = ric.transpose() * Rj.transpose() * Ri * ric;
jaco_ex.rightCols<3>() = -tmp_r * Utility::skewSymmetric(pts_camera_i) + Utility::skewSymmetric(tmp_r * pts_camera_i) +
Utility::skewSymmetric(ric.transpose() * (Rj.transpose() * (Ri * tic + Pi - Pj) - tic));
jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;
jacobian_ex_pose.rightCols<1>().setZero();
}
if (jacobians[3])
{
Eigen::Map jacobian_feature(jacobians[3]);
#if 1
jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i);
#else
jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i;
#endif
}
}
sum_t += tic_toc.toc(http://www.my516.com);
return true;
}
---------------------
转载于:https://www.cnblogs.com/hyhy904/p/11058039.html

    推荐阅读


    上一篇：抽象代数学习笔记（15）环

    下一篇：CSS3字体火焰燃烧效果