![在这里插入图片描述](https://img-blog.csdnimg.cn/20210620200534146.png?#pic_center)
![在这里插入图片描述](https://img-blog.csdnimg.cn/1c77143d8d184bd18a2b28a5b036cb26.png?#pic_center)
目录
- 1. 代码
- 2. ceres解析求导
- 3. ceres李代数加法代码实现
- 4. 预积分约束残差计算
- 5. 预积分雅克比计算
- 6. 视觉重投影约束
- 7. 滑动窗口边缘化
1. 代码
- 代码位置:
vins_estimator->src->estimator.cpp
if(result)
{
solver_flag = NON_LINEAR;
solveOdometry();
slideWindow();
f_manager.removeFailures();
ROS_INFO("Initialization finish!");
last_R = Rs[WINDOW_SIZE];
last_P = Ps[WINDOW_SIZE];
last_R0 = Rs[0];
last_P0 = Ps[0];
}
void Estimator::solveOdometry()
{
if (frame_count < WINDOW_SIZE)
return;
if (solver_flag == NON_LINEAR)
{
TicToc t_tri;
f_manager.triangulate(Ps, tic, ric);
ROS_DEBUG("triangulation costs %f", t_tri.toc());
optimization();
}
}
2. ceres解析求导
解析求导相比于自动求导,会使程序的速度加快。特别是对于里程计模块这种对于实时性要求很高的程序而言。
需要手动求出每次优化的残差和雅可比
![在这里插入图片描述](https://img-blog.csdnimg.cn/31205b9bfd2649c2a0227d14ad5d9c4c.png)
ceres官网
![在这里插入图片描述](https://img-blog.csdnimg.cn/41d0cd9236004bf7ab6177aa5dbf1555.png?#pic_center)
因为在vins中我们知道参数块的大小,所以我在vins中使用SizeCostFunction
- 第一步是新建一个类继承SizeCostFunction
ceres::SizeCostFunction<1,1> : 前一个1:残差的维度; 后一个1:参数块的维度,因为这里只有一个参数块,所以只有一个1 - 第二步:定义完类之后需要重载Evaluate虚函数
double const* const* parameters对应各个参数块.由于各个参数块都定义为double数组,如果有好多个参数块就会被定义为数组的数组,也就是这边的双指针. - residuals是一个向量,也就是一个一维数组,所以这里定义为double *
- jacobians的大小是残差*参数块总数的矩阵,对于矩阵来讲它也是一个二维数组,所以定义为double * *
const double x = parameters[0][0]
LOSS Function 核函数
![在这里插入图片描述](https://img-blog.csdnimg.cn/82934468e83f4e09a8d0d9742341b87a.png)
http://ceres-solver.org/nnls_modeling.html#instances
3. ceres李代数加法代码实现
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
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; };
};
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;
}
![在这里插入图片描述](https://img-blog.csdnimg.cn/fe38f2f1304340f1a25882bec0933281.png)
template <typename Derived>
static Eigen::Quaternion<typename Derived::Scalar> deltaQ(const Eigen::MatrixBase<Derived> &theta)
{
typedef typename Derived::Scalar Scalar_t;
Eigen::Quaternion<Scalar_t> dq;
Eigen::Matrix<Scalar_t, 3, 1> half_theta = theta;
half_theta /= static_cast<Scalar_t>(2.0);
dq.w() = static_cast<Scalar_t>(1.0);
dq.x() = half_theta.x();
dq.y() = half_theta.y();
dq.z() = half_theta.z();
return dq;
}
4. 预积分约束残差计算
预积分约束:
![在这里插入图片描述](https://img-blog.csdnimg.cn/d0d0ef3972ad4c1dadb77d3c3cf64e11.png)
- 如果仅通过IMU积分得到
b
k
b_k
bk到
b
k
+
1
b_{k+1}
bk+1的位姿,速度和旋转,那么(25)式是成立的.
- 但实际还会收到最小化重投影误差的约束
- 所以最小化 [ 两帧间位姿(速度/旋转)计算出的增量 - IMU预积分得到的增量 ],残差如公式(25)下部分 所示。
![在这里插入图片描述](https://img-blog.csdnimg.cn/4414458ce3bd4eb08c0e11f21084c099.png?#pic_center)
IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
Eigen::Matrix<double, 15, 1> evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
{
Eigen::Matrix<double, 15, 1> residuals;
Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);
Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);
Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG);
Eigen::Vector3d dba = Bai - linearized_ba;
Eigen::Vector3d dbg = Bgi - linearized_bg;
Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg);
Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg;
Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg;
residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p;
residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec();
residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v;
residuals.block<3, 1>(O_BA, 0) = Baj - Bai;
residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi;
return residuals;
}
因为ceres不像g2o,没有增加信息矩阵的接口.
所以不能直接使用
e
T
∗
p
∗
e
e^T*p*e
eT∗p∗e,而是将p分解为
L
∗
L
T
L*L^T
L∗LT,让式子等于
e
T
∗
L
∗
L
T
∗
e
e^T*L*L^T*e
eT∗L∗LT∗e. 认为
L
T
∗
e
L^T*e
LT∗e即为新的残差
Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
residual = sqrt_info * residual;
5. 预积分雅克比计算
![在这里插入图片描述](https://img-blog.csdnimg.cn/48097ff3e7464ecdb9bdf4b9fe79e672.png?#pic_center)
![在这里插入图片描述](https://img-blog.csdnimg.cn/1a44cdaa5d6e495f80c3b5e3f0208bd7.png?#pic_center)
- 对于雅可比矩阵的第一个分块(jacobians[0] 15X7)
![在这里插入图片描述](https://img-blog.csdnimg.cn/b1c648b2b5da43ed906c07fe17fdabfe.png?#pic_center)
jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));
![在这里插入图片描述](https://img-blog.csdnimg.cn/29d0f50256a14d20963b44ec92517f93.png?#pic_center)
jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));
![在这里插入图片描述](https://img-blog.csdnimg.cn/e37f4af0145e444392f4ca0553357fa9.png?#pic_center)
![在这里插入图片描述](https://img-blog.csdnimg.cn/8818fdefd9954017aaeb92da8f1f8a4e.png?#pic_center)
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
整体代码
if (jacobians[0])
{
Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
jacobian_pose_i.setZero();
jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));
#if 0
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();
#else
Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
#endif
jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));
jacobian_pose_i = sqrt_info * jacobian_pose_i;
if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)
{
ROS_WARN("numerical unstable in preintegration");
}
}
- 对于雅可比矩阵的第三个分块(jacobians[2] 15X7)
![在这里插入图片描述](https://img-blog.csdnimg.cn/41e4a19a53684160bed848efb7bfb707.png?#pic_center)
- 对于雅可比矩阵的第二个分块(jacobians[1] 15X9)
![36:07](https://img-blog.csdnimg.cn/3c8c7219289a41deb9def82bc7f5014a.png?#pic_center)
jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;
![在这里插入图片描述](https://img-blog.csdnimg.cn/98ab405d8dc44a60815fa4ecbf9c4859.png?#pic_center)
jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;
![在这里插入图片描述](https://img-blog.csdnimg.cn/4f9230b8b5954058a3f0c828361efe17.png?#pic_center)
![在这里插入图片描述](https://img-blog.csdnimg.cn/cd63eb4f7efd476e99127bd14f32cc3b.png?#pic_center)
jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();
jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();
![#pic_center](https://img-blog.csdnimg.cn/ab23721eed36482290de3be20a7939ad.png?#pic_center)
jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q).bottomRightCorner<3, 3>() * dq_dbg;
- 对于雅可比矩阵的第四个分块(jacobians[3] 15X9)
![在这里插入图片描述](https://img-blog.csdnimg.cn/7533cc0a9c8e4ba5864780e76ce5fdfc.png?#pic_center)
6. 视觉重投影约束
![在这里插入图片描述](https://img-blog.csdnimg.cn/3b9317a9630b4e8595eaeae7600950cd.png?#pic_center)
- 第i帧是滑窗中第一个观测到这个3D点的帧
- 3D点的状态量维持的是逆深度,而不是传统的三维向量
- 因为在
J
T
∗
J
∗
△
x
=
−
J
∗
f
(
x
)
J^T*J*△x = -J*f(x)
JT∗J∗△x=−J∗f(x)中,
J
T
∗
J
J^T*J
JT∗J的维度与
△
x
△x
△x维度相同,而
x
x
x在VIO问题中维护的是滑窗中的位姿,外参以及(占比重最大的)3D点,对3D点只存储逆深度(1个维度),可以让x的维度相比维护3D点坐标(3个维度)缩小2/3,加快求解速度
- 为什么用逆深度不用深度:对于极远的点(例如天空),正深度为无穷大,逆深度则为一个很小的值.而且不会存在离相机很近的点,不存在逆深度值很大的情况
- 因为不是存储三维坐标,所以逆深度需要和第i帧绑定
- 优化变量:第i帧IMU的位姿,第j帧IMU的位姿,相机到IMU外参,3D点逆深度
![在这里插入图片描述](https://img-blog.csdnimg.cn/38448fe8d792480fb988801f12be1157.png?#pic_center)
λ
\lambda
λ表示逆深度,
1
/
λ
1/\lambda
1/λ表示深度
推导一下:
![在这里插入图片描述](https://img-blog.csdnimg.cn/95e7de02e74d4273948ba4eeacc1978e.png?#pic_center)
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]);
残差的计算:
如果把上面的一长串带入到残差,需要把一长串分为x,y,z三个部分。这会非常的复杂,所以我们采用链式求导法则。
以x为例,我们把残差关于待优化变量的导数分解为残差对于第j帧相机坐标系下的点的导数 × 第j帧相机坐标系下点对于各个优化变量的导数
![在这里插入图片描述](https://img-blog.csdnimg.cn/49b4c7cf05694c0ea2b1b75bcee52ed0.png?#pic_center)
![在这里插入图片描述](https://img-blog.csdnimg.cn/36d9d302c18e42ccb21b001207dc4f72.png#pic_center)
![在这里插入图片描述](https://img-blog.csdnimg.cn/23f1d02c934c4340ba0338118b9484ce.png?#pic_center)
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();
}
![在这里插入图片描述](https://img-blog.csdnimg.cn/f3670c6f49ce429cb4ae3b48f0f89a0d.png?#pic_center)
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();
}
![在这里插入图片描述](https://img-blog.csdnimg.cn/ffe33eb5b1ce4d8f8e84e3e752dd7898.png?#pic_center)
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();
}
![在这里插入图片描述](https://img-blog.csdnimg.cn/f97d4c74364f46f984db500afc97d227.jpeg#pic_center)
if (jacobians[3])
{
Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[3]);
jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i);
}
7. 滑动窗口边缘化
![在这里插入图片描述](https://img-blog.csdnimg.cn/0c01e956352f4cc5b6a5391bc11cc5f2.png?#pic_center)
由于需要边缘化,所以我们要手动计算H矩阵,而不能靠ceres帮我们自动计算
![在这里插入图片描述](https://img-blog.csdnimg.cn/e3999072e2bf4cebbd57cf3ebf6c5fd8.png?#pic_center)
https://blog.csdn.net/heyijia0327/article/details/52822104
-
p是位姿,m是地图点,连线表示约束
-
X
p
1
X_{p1}
Xp1与
X
m
1
X_{m1}
Xm1的贡献是{p1,p1},{p1,m1},{m1,p1},{m1,m1};
X
p
1
X_{p1}
Xp1与
X
p
2
X_{p2}
Xp2的贡献是{p1,p1},{p1,p2},{p2,p1},{p2,p2}
![在这里插入图片描述](https://img-blog.csdnimg.cn/7a5af4b447b24541ada7dd32a763dad7.png?#pic_center)
-
Λ
a
Λa
Λa ={p1,p1},
Λ
b
Λb
Λb = {p1,p2-m6},
Λ
c
Λc
Λc={p2-m6,p2-m6}
上式=
=
![在这里插入图片描述](https://img-blog.csdnimg.cn/fd4ddff090b6427fb714f13993010be8.png?#pic_center)
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)