diff --git a/vins_estimator/src/factor/imu_factor.h b/vins_estimator/src/factor/imu_factor.h index 6bd1b94..2c57c91 100644 --- a/vins_estimator/src/factor/imu_factor.h +++ b/vins_estimator/src/factor/imu_factor.h @@ -57,18 +57,20 @@ class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> pre_integration->repropagate(Bai, Bgi); } #endif - + // IMU残差 Eigen::Map> 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 sqrt_info = Eigen::LLT>(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); @@ -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> jacobian_pose_i(jacobians[0]); @@ -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> jacobian_speedbias_i(jacobians[1]); @@ -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> jacobian_pose_j(jacobians[2]); @@ -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> jacobian_speedbias_j(jacobians[3]); diff --git a/vins_estimator/src/factor/projection_factor.cpp b/vins_estimator/src/factor/projection_factor.cpp index 25a70ef..5cfe2c4 100644 --- a/vins_estimator/src/factor/projection_factor.cpp +++ b/vins_estimator/src/factor/projection_factor.cpp @@ -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 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(); @@ -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> jacobian_pose_i(jacobians[0]); @@ -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> jacobian_pose_j(jacobians[1]); @@ -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> jacobian_ex_pose(jacobians[2]); @@ -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 jacobian_feature(jacobians[3]);