pts_i and pts_j: What specifically refers to the meaning? (L -th respectively waypoint at the i, j th observation camera normalized camera coordinate system to a coordinate of, P¯¯¯cil \ bar {} ^ {P} C_i the _l
P
ˉ
l
C
I and P¯¯¯cjl \ bar {P}} ^ {C_J the _l P ˉ L C J ); tangent_base: any two tangent plane orthogonal groups (assigned by calculating in the constructor);? static sqrt_info data members and sum_t: when and assigned it?
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<double, 2, 3> tangent_base;
static Eigen::Matrix2d sqrt_info;
static double sum_t;
};
. 1
2
. 3
. 4
. 5
. 6
. 7
. 8
. 9
10
. 11
12 is
13 is
come to the question, ProjectionFactor class Evaluate member functions:
optimization variables: [pwbi, qwbi], [ pwbj, qwbj], [pbc, qbc], λl [p ^ B_i {} W_, Q ^ {B_i W_}], [P ^ {b_j W_}, {Q ^ b_j W_}], [B_c P ^, 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 ], [lambda]
l respectively correspond to: [Pi, Qi], [Pj of, Qj], [TiC, QIC], inv_dep_i (? here [lambda] \ lambdaλ represents what meaning the inverse depth inv_dep_i) (w, bw, bw , b representative of the coordinates What system specifically referring to?)
解析中（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 the _l [lambda] L . 1 P ˉ L C I ==> Eigen, Vector3D pts_camera_i :: = pts_i / inv_dep_i; (Thus, pts_i represents P¯¯¯cil \ bar {} ^ {P} C_i the _l P ˉ l C I , for the l th waypoint the i-th camera coordinate in the camera coordinate system observed normalized); (* + of Rbc PBC) (R & lt B_c * + ^ P ^ B_c) (R & lt C B * P + C B
) ==> Eigen, Vector3D pts_imu_i :: = * pts_camera_i QIC TiC +;
? (Here can be seen, the IMU-based system is a b bb)
Rwbi * + ^ R & lt pwbi B_i W_ {*} + {P ^ W_ B_i R & lt}
B
I W * P + B I W ==> Eigen, Vector3D pts_w :: = Pi + Qi * pts_imu_i; Rbjw (* - pwbj) R & lt b_j ^ {} _W (* - P ^ {W_ b_j }) R & lt W B J (-p * B J W ) ==> Eigen, Vector3D pts_imu_j :: = Qj.inverse () * (pts_w - Pj of); Rcb (* - PBC) ^ R & lt C_B (* B_c ^ -p) R & lt B C (* -p C B
) ==> Eigen :: Vector3d pts_camera_j = qic.inverse () * (pts_imu_j - tic);
therefore, pts_camera_j represents C_J} {^ P Pcjl _lP
L
C
J , by P¯¯¯cil \ bar {P C_i the _l} ^ {} P ˉ L C I calculated come.
Parsing (25) the formula:
RC (zcjl, X-) = [b1⃗ b2⃗] (Pcjl||Pcjl||-P¯¯¯cjl) R_c (\ Hat {Z}} ^ {C_J the _l, X-) = \ the begin {bmatrix} \ vec {b_1} \\ \ vec {b_2} \ end {bmatrix} (\ frac {P ^ {c_j} _l} {|| P ^ {c_j} _l ||} - \ bar {P} ^ C_J the _l} {) R & lt
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;
Finally, computing the Jacobian, parsing (28):
Note: There are some errors on the dimensions of the matrix resolved here, should be 3 × * 3 \ times * 3 × *, rather than 3 × * 3 \ times * 3 × *.
In one example, the analysis of the code corresponding relation formula:
J [0] = 2. 7 × [∂rc∂pwbi, ∂rc∂qwbi] J [0] ^ {2 \ Times. 7} = [\ FRAC {\ partial {} R_c \ partial B_i P ^ {}} W_, \ FRAC {\ partial R_c} {\ partial B_i Q ^ {}} W_] J [0]
2. 7 ×
= [
∂p
B
I W ∂r C , ∂q B I W ∂r C ] code first defines a jaco_i of 3 × 6 3 \ times 63 × 6, then with a reduce2 × 3 2 \ times 32 × 3 multiplying give the 2 × 6 2 \ times as a result of 62 × 6 J [0] J [0] J left [0] is 6, the last column 0; follows: Eigen, the Matrix :: <Double,. 3,. 6> 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);
:: the Matrix Eigen, <Double, 2, 3> the reduce (2, 3);
the reduce = tangent_base * norm_jaco; here norm_jaco expression mean? Corresponding to the formula?
reduce = sqrt_info * reduce;
Eigen::Map的用法？
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> 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
and J [. 3]. 1 2 × J [. 3] ^ {2 \ J}. 1 Times [. 3]
2. 1 ×
similar calculation.
So far, visual constraints temporarily come to an end.
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];
Self :: Vector3D pts_camera_i = pts_i / inv_dep_i;
Self :: Vector3D pts_imu_i = qic * pts_camera_i + tic;
Self :: Vector3D pts_w = Qi * pts_imu_i + Pi;
Self :: Vector3D pts_imu_j Qj.inverse = () * (pts_w - Pj);
Self :: Vector3D pts_camera_j qic.inverse = () * (pts_imu_j - tic);
Self :: map <self :: Vector2d> resi dual (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<double, 2, 3> 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<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
Eigen::Matrix<double, 3, 6> 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<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]);
Eigen::Matrix<double, 3, 6> 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<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[2]);
Eigen::Matrix<double, 3, 6> 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<Eigen::Vector2d> 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;
}
---------------------