Skip to content

Commit

Permalink
update factor learning
Browse files Browse the repository at this point in the history
  • Loading branch information
@ManiiXu
ManiiXu committed Jun 26, 2019
1 parent 893bdcf commit e174870
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 13 deletions.
12 changes: 9 additions & 3 deletions vins_estimator/src/factor/imu_factor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,18 +57,20 @@ class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
pre_integration->repropagate(Bai, Bgi);
}
#endif

// IMU残差
Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
Pj, Qj, Vj, Baj, Bgj);

//LLT分解
// LLT分解，residual还需乘以信息矩阵的sqrt_info
// 因为优化函数其实是d=r^T P^-1 r ，P表示协方差，而ceres只接受最小二乘优化
// 因此需要把P^-1做LLT分解，使d=(L^T r)^T (L^T r) = r'^T r
Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
//sqrt_info.setIdentity();
residual = sqrt_info * residual;

if (jacobians)
{
// 获取预积分的误差递推函数中pqv关于ba、bg的Jacobian
double sum_dt = pre_integration->sum_dt;
Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);
Expand All @@ -85,6 +87,7 @@ class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
/// ROS_BREAK();
}

// 第i帧的IMU位姿 pbi、qbi
if (jacobians[0])
{
Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
Expand All @@ -111,6 +114,7 @@ class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
//ROS_BREAK();
}
}
// 第i帧的imu速度、ba、bg
if (jacobians[1])
{
Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);
Expand Down Expand Up @@ -140,6 +144,7 @@ class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
//ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);
//ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);
}
// 第j帧的IMU位姿 pbj、qbj
if (jacobians[2])
{
Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);
Expand All @@ -159,6 +164,7 @@ class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
//ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8);
//ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8);
}
// 第j帧的IMU速度、ba、bg
if (jacobians[3])
{
Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);
Expand Down
32 changes: 22 additions & 10 deletions vins_estimator/src/factor/projection_factor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,29 +30,35 @@ bool ProjectionFactor::Evaluate(double const *const *parameters, double *residua
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]);

//i时刻相机坐标系下的map point的逆深度

//pts_i 是i时刻归一化相机坐标系下的3D坐标
//第i帧相机坐标系下的的逆深度
double inv_dep_i = parameters[3][0];
//i时刻相机坐标系下的map point坐标
//第i帧相机坐标系下的3D坐标
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
//i时刻IMU坐标系下的map point坐标
//第i帧IMU坐标系下的3D坐标
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
//世界坐标系下的map point坐标
//世界坐标系下的3D坐标
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
//在j时刻imu坐标系下的map point坐标
//第j帧imu坐标系下的3D坐标
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
//在j时刻相机坐标系下的map point坐标
//第j帧相机坐标系下的3D坐标
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
#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 表示残差residual对fci（pts_camera_j）的导数，同样根据不同的相机模型
if (jacobians)
{
Eigen::Matrix3d Ri = Qi.toRotationMatrix();
Expand All @@ -70,13 +76,17 @@ bool ProjectionFactor::Evaluate(double const *const *parameters, double *residua
- 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
#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])

// 残差项的Jacobian
// 先求fci对各项的Jacobian，然后用链式法则乘起来
// 对第i帧的位姿 pbi,qbi 2X7的矩阵 最后一项是0
if (jacobians[0])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);

Expand All @@ -87,7 +97,7 @@ bool ProjectionFactor::Evaluate(double const *const *parameters, double *residua
jacobian_pose_i.leftCols<6>() = reduce * jaco_i;
jacobian_pose_i.rightCols<1>().setZero();
}

// 对第j帧的位姿 pbj,qbj
if (jacobians[1])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[1]);
Expand All @@ -99,6 +109,7 @@ bool ProjectionFactor::Evaluate(double const *const *parameters, double *residua
jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
jacobian_pose_j.rightCols<1>().setZero();
}
// 对相机到IMU的外参 pbc,qbc (qic,tic)
if (jacobians[2])
{
Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_ex_pose(jacobians[2]);
Expand All @@ -110,6 +121,7 @@ bool ProjectionFactor::Evaluate(double const *const *parameters, double *residua
jacobian_ex_pose.leftCols<6>() = reduce * jaco_ex;
jacobian_ex_pose.rightCols<1>().setZero();
}
// 对逆深度 \lambda (inv_dep_i)
if (jacobians[3])
{
Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[3]);
Expand Down

0 comments on commit e174870

Please sign in to comment.