2021@SDUSC
Friday, December 24, 2021 - Monday, December 27, 2021
1, Background:
Continue to analyze the factor folder this week.
This week, I analyzed the visual residuals of the code part and the residuals of feature points under the projection of the camera: projection_factor.cpp and projection_td_factor.cpp and pose_local_parameterization.cpp and the corresponding header file.
The first is the principle analysis:
Returning to the general framework diagram of marginalization we analyzed before, we can see that this part of the code is closely related to ceres and uses ceres Solver to deal with the problem.
After reading and analyzing, I found that the core code of this part is projection_factor
projetcion_td_factor is its extension, pose_local_parameterization is a small function in ceres.
2, Code analysis:
1projection_factor.h
For projection_ For factor, it inherits the SizedCostFunction in the Ceres library.
It represents the point PTS on the unit camera plane in frame i_ i is the point and matching point PTS re projected onto the unit camera plane of frame j_ Projection error of j. You need to implement the constructor and overload the Evaluate function.
#pragma once
#include <ros/assert.h>
#include <ceres/ceres.h>
#include <Eigen/Dense>
#include "../utility/utility.h"
#include "../utility/tic_toc.h"
#include "../parameters.h"
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;
};
2 projection_factor.cpp
The constructor passes in a constant, which is a pair of matching feature points. The data passed in here is the coordinates of the matching points on the camera plane.
However, the re projection error is the error on the tangent plane of the unit sphere. Why is it a spherical plane
Therefore, it is necessary to find the orthogonal basis of the tangent plane by Schmidt orthogonalization.
#include "projection_factor.h"
Eigen::Matrix2d ProjectionFactor::sqrt_info;
double ProjectionFactor::sum_t;
ProjectionFactor::ProjectionFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j) : pts_i(_pts_i), pts_j(_pts_j)
{
#ifdef UNIT_SPHERE_ERROR
Eigen::Vector3d b1, b2;
Eigen::Vector3d a = pts_j.normalized();
Eigen::Vector3d tmp(0, 0, 1);
if(a == tmp)
tmp << 1, 0, 0;
b1 = (tmp - a * (a.transpose() * tmp)).normalized();
b2 = a.cross(b1);
tangent_base.block<1, 3>(0, 0) = b1.transpose();
tangent_base.block<1, 3>(1, 0) = b2.transpose();
#endif
};
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]);
//pts_i is the 3D coordinate in the normalized camera coordinate system at time i
//Inverse depth in frame i camera coordinate system
double inv_dep_i = parameters[3][0];
//3D coordinates in frame i camera coordinate system
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
//3D coordinates in IMU coordinate system of frame i
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
//3D coordinates in world coordinate system
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
//3D coordinates in frame j imu coordinate system
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
//3D coordinates in frame j camera coordinate system
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
Eigen::Map<Eigen::Vector2d> 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;
//reduce represents the derivative of residual to fci (pts_camera_j), also according to different camera models
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);
//The following is the derivation and calculation of Jacobian matrix
#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;
// Jacobian of residual term
// First find the Jacobian of fci for each item, and then multiply it by the chain rule
// For the pose PBI of frame i, the last term of qbi 2x7 matrix is 0
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();
}
// Pose pbj,qbj for frame j
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();
}
// External parameters pbc,qbc (qic,tic) from camera to IMU
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();
}
// Inverse depth \ lambda (inv_dep_i)
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();
return true;
}
The second half of this part involves the knowledge of disturbance model, which is also an incomprehensible concept.
In short, the concept here is to solve the four different terms of Jacobian matrix.
3 pose_local_parameterization.h
This header file declares and extends the processing of parameters in ceres solver.
It mainly declares four functions. The functions of these four functions are:
- Plus: the addition of translation and rotation is mainly rotation, which needs to deal with the operation of quaternion. Note that this wig means generalized addition, and the purpose is to update the optimization variables
- ComputeJacobian: calculate Jacobian matrix
- GlobalSize: indicates the degree of freedom of the parameter
- LocalSize: indicates Δ Degrees of freedom in tangent space where x is located
This is a related parameterized function in the ceres library.
#pragma once
#include <eigen3/Eigen/Dense>
#include <ceres/ceres.h>
#include "../utility/utility.h"
class PoseLocalParameterization : public ceres::LocalParameterization
{
virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;
virtual bool ComputeJacobian(const double *x, double *jacobian) const;
virtual int GlobalSize() const { return 7; };
virtual int LocalSize() const { return 6; };
};
4pose_local_parameterization.cpp
This place is the specific implementation of the header file introduced above.
#include "pose_local_parameterization.h"
bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
Eigen::Map<const Eigen::Vector3d> _p(x);
Eigen::Map<const Eigen::Quaterniond> _q(x + 3);
Eigen::Map<const Eigen::Vector3d> dp(delta);
Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));
Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);
p = _p + dp;
q = (_q * dq).normalized();
return true;
}
bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const
{
Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
j.topRows<6>().setIdentity();
j.bottomRows<1>().setZero();
return true;
}
5 projection_td_factor.h
It can be seen that this td and the projection without td_ Factor is very similar.
The difference is that there are several more variables.
Because the header file is only a declaration, we need to find the difference in the cpp code.
Eigen::Vector3d velocity_i, velocity_j;//Velocity of corner in normalized plane
double td_i, td_j;//Time synchronization error used in processing IMU data
double row_i, row_j;//Ordinate of corner image coordinates
#pragma once
#include <ros/assert.h>
#include <ceres/ceres.h>
#include <Eigen/Dense>
#include "../utility/utility.h"
#include "../utility/tic_toc.h"
#include "../parameters.h"
//Support for incomplete synchronization of IMU camera timestamp and Rolling shutter camera
class ProjectionTdFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1, 1>
{
public:
ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j,
const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j,
const double _td_i, const double _td_j, const double _row_i, const double _row_j);
virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;
void check(double **parameters);
Eigen::Vector3d pts_i, pts_j;//Coordinates of corners in the normalized plane
Eigen::Vector3d velocity_i, velocity_j;//Velocity of corner in normalized plane
double td_i, td_j;//Time synchronization error used in processing IMU data
Eigen::Matrix<double, 2, 3> tangent_base;
double row_i, row_j;//Ordinate of corner image coordinates
static Eigen::Matrix2d sqrt_info;
static double sum_t;
};
6 projection_td_factor.cpp
The conclusion is still that the initialization information is different... The remaining structures are exactly the same.
#include "projection_td_factor.h"
Eigen::Matrix2d ProjectionTdFactor::sqrt_info;
double ProjectionTdFactor::sum_t;
ProjectionTdFactor::ProjectionTdFactor(const Eigen::Vector3d &_pts_i, const Eigen::Vector3d &_pts_j,
const Eigen::Vector2d &_velocity_i, const Eigen::Vector2d &_velocity_j,
const double _td_i, const double _td_j, const double _row_i, const double _row_j) :
pts_i(_pts_i), pts_j(_pts_j),
td_i(_td_i), td_j(_td_j)
{
velocity_i.x() = _velocity_i.x();
velocity_i.y() = _velocity_i.y();
velocity_i.z() = 0;
velocity_j.x() = _velocity_j.x();
velocity_j.y() = _velocity_j.y();
velocity_j.z() = 0;
row_i = _row_i - ROW / 2;
row_j = _row_j - ROW / 2;
#ifdef UNIT_SPHERE_ERROR
Eigen::Vector3d b1, b2;
Eigen::Vector3d a = pts_j.normalized();
Eigen::Vector3d tmp(0, 0, 1);
if(a == tmp)
tmp << 1, 0, 0;
b1 = (tmp - a * (a.transpose() * tmp)).normalized();
b2 = a.cross(b1);
tangent_base.block<1, 3>(0, 0) = b1.transpose();
tangent_base.block<1, 3>(1, 0) = b2.transpose();
#endif
};
bool ProjectionTdFactor::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];
double td = parameters[4][0];
Eigen::Vector3d pts_i_td, pts_j_td;
pts_i_td = pts_i - (td - td_i + TR / ROW * row_i) * velocity_i;
pts_j_td = pts_j - (td - td_j + TR / ROW * row_j) * velocity_j;
Eigen::Vector3d pts_camera_i = pts_i_td / 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<Eigen::Vector2d> residual(residuals);
#ifdef UNIT_SPHERE_ERROR
residual = tangent_base * (pts_camera_j.normalized() - pts_j_td.normalized());
#else
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.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]);
jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i_td * -1.0 / (inv_dep_i * inv_dep_i);
}
if (jacobians[4])
{
Eigen::Map<Eigen::Vector2d> jacobian_td(jacobians[4]);
jacobian_td = reduce * ric.transpose() * Rj.transpose() * Ri * ric * velocity_i / inv_dep_i * -1.0 +
sqrt_info * velocity_j.head(2);
}
}
sum_t += tic_toc.toc();
return true;
}
3, Questions and answers:
1 projection in this_ td_ Factor and projection_ What is the relationship of factor
But looking at this part of the code, we can't find the similarities and differences between the two. We just find the extra parameters in td.
You also need to explore the similarities and differences where the two classes are called.
for (auto &it_per_frame : it_per_id.feature_per_frame)
{//Traverse the information of the current feature in each frame
imu_j++;
if (imu_i == imu_j)
continue;
Vector3d pts_j = it_per_frame.point;//! Get the second feature point
if (ESTIMATE_TD)//In case of synchronization error
{
ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td, t_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);
}
else//Without synchronization error
{
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
}
f_m_cnt++;
}
This part of the code is already obvious, which is used to deal with two frames in different situations: with synchronization error and without synchronization error.
Corresponding to different problem s, add residual functions.
2 Schmidt orthogonalization
Given the basis vector, a set of orthogonalized bases can be obtained by this algorithm.
3 disturbance model
This part involves the knowledge of Lie groups and Lie algebras; Don't delve into it for the time being.
Mainly to deal with the derivative of the matrix
But this part of the analysis is also the key part of the code. Remember to make it up if you have time.
4, References:
projection_factor code analysis: VINS-MONO
Schmidt orthogonalization
Derivation and perturbation model of Lie algebra
VINS-MONO -- backend optimization
Local parameterization parameterization analysis