文章目录

简介
- 重投影误差
- 卷帘快门相机时间差
带Td 的优化
- 残差
- - 优化变量
  - 残差计算
- 求解其Jacobian
- - 对 pts_camera_j 求导
  - 对 Pose_i 求导
  - 对 Pose_j 求导
  - 对外参求导
  - 对特征点深度 inv_dep_i 求导
  - 对时间差求导
- 代码实现
不带Td 优化
- 残差
- - 优化变量
  - 残差计算
- 求解Jacobian
- 代码实现

简介

逆深度参数化：[xyz]=1λ[uv1]\begin{bmatrix} x \ y \ z \end{bmatrix} = \frac{1}{\lambda}\begin{bmatrix} u \ v \ 1 \end{bmatrix}[x y z]=λ1[u v 1]
第i 帧中第一次被观测到转化到归一化平面：[ucivci1]\begin{bmatrix} u_{c_{i}}\ v_{c_{i}} \ 1\end{bmatrix}[uci vci 1]
转换到第j 帧：[xcjycjzcj1]=Tbc−1Twbj−1TwbiTbc[1λuci1λvci1λ1]\begin{bmatrix} x_{c_{j}}\ y_{c_{j}} \ z_{c_{j}} \ 1 \end{bmatrix} = T_{bc}^{-1}T_{wb_{j}}^{-1}T_{wb_{i}}T_{bc} \begin{bmatrix} \frac{1}{\lambda} u_{c_{i}} \ \frac{1}{\lambda} v_{c_{i}} \ \frac{1}{\lambda} \ 1\end{bmatrix}[xcj ycj zcj 1]=Tbc−1Twbj−1TwbiTbc[λ1uci λ1vci λ1 1]
视觉残差：
- 针孔： rc=[xcjzcj−ucjycjzcj−vcj]r_{c} = \begin{bmatrix} \frac{x_{c_{j}}}{z_{c_{j}}} - u_{c_{j}} \ \frac{y_{c_{j}}}{z_{c_{j}}} - v_{c_{j}}\end{bmatrix}rc=[zcjxcj−ucj zcjycj−vcj]
- 鱼眼：rC(zl^cj,X)=[b1→b2→](Plcj∣Plcj∣−Plˉcj)r_{\mathcal{C}}(\hat{z_{l}}^{c_{j}}, \mathcal{X}) = \begin{bmatrix} \overrightarrow{b_{1}} \ \overrightarrow{b_{2}} \end{bmatrix} (\frac{P_{l}^{c_{j}}}{\left | P_{l}^{c_{j}} \right |}-\bar{P_{l}}^{c_{j}})rC(zl^cj,X)=[b1 b2](∣Plcj∣Plcj−Plˉcj)
  - 因为视觉残差的自由度为2, 因此将视觉残差投影到正切平面上，b1,b2b_{1}, b_{2}b1,b2为正切平面上的任意俩个正交基。
  - 采用施密特正交化得到
求解Jacobian
- 视觉残差对重投影3D点fcjf_{c_{j}}fcj求导
- fcjf_{c_{j}}fcj 对各个优化变量的求导

重投影误差

对于同一特征点，i 时刻与 j 时刻的重投影误差
先将 i时刻特征点 camera坐标系通过外参＋i时刻全局位姿得到　特征点的全局位姿
然后将该特征点的全局位姿通过j时刻全局位姿＋外参转换到j时刻camera坐标系
转换后的两个差异之差即为重投影误差。

卷帘快门相机时间差

相加快门

快门是照相机用来控制感光片有效曝光时间的机构，Global Shutter（全局快门）与Rolling Shutter（卷帘快门）对应全局曝光和卷帘曝光模式，这两种曝光模式都是相机常见的曝光模式。
Global Shutter 特点
- 感光元件的所有像素点同时曝光一定时间，进而成像。
- 曝光时间比Rolling Shutter短，曝光整帧后输出像素数据。
- 对于高速运动的部分曝光后体现出模糊的现象。
- 一般使用CCD作为Sensor，对像素点输出的带宽的要求较高，CCD的造价也相对CMOS较高。
Rolling Shutter 特点
- 感光元件的所有像素点逐行轮流曝光一定时间，进而成像。
- 会出现“果冻现象”。
- 曝光时间比Global Shutter长，但是曝光一行输出一行。
- 对于高速运动的部分曝光后体现出弯曲的现象。
- 一般使用CMOS作为Sensor，对像素点输出的带宽的要求较低，CMOS的造价也相对较低。

本部分功能：

对imu-camera 时间戳不完全同步和 Rolling shutter 相机的支持
该部分的功能是优化 IMU 与相机硬件的固定时间差。
认为滑窗内 imu 与相机硬件的时间间隔一样。
输入的变量：
- i 时刻角点（j时刻角点一样，赋值一份）
  - 在归一化平面的坐标 pts_i
  - 角点在归一化平面的速度 velocity_i
  - 处理IMU数据时用到的时间同步误差 td_i
  - 角点在归一化平面行坐标 _row_i = _row_i - ROW / 2
每张图片以中心时间曝光作为其时间。

带Td 的优化

pts_i基于时间差及速度得到 pts_i_td
pts_j基于时间差及速度得到 pts_j_td

残差

优化变量

imu_i 的全局位姿 维度：7
imu_j 的全局位姿 维度：7
外参 维度：7
inv_dep_i 维度：1
时间差变量 para_Td[0] 维度：1

残差计算

角点i在归一化平面的row坐标
- pts_i - (td - td_i + TR / ROW * row_i) * velocity_i
- pts_camera_i = pts_i_td / inv_dep_i
通过外参转换到 imu坐标系
- pts_imu_i = qic * pts_camera_i + tic;
通过imu_i的全局位姿，将角点转换到世界坐标系
- pts_w = Qi * pts_imu_i + Pi;
将角点通过 imu_j 转换到 j时刻imu坐标系下位姿
- pts_imu_j = Qj.inverse() * (pts_w - Pj);
转换到 j 时刻相机坐标系下
- pts_camera_j = qic.inverse() * (pts_imu_j - tic);

求解其Jacobian

求解 jacobian时，即残差对优化变量求导
求导分两步，先对 pts_camera_j 求导，再对变量求导。

对 pts_camera_j 求导

残差对 pts_camera_j求导
非单位球面误差时：
reduce=[1./depj0−pts_camera_j(0)/(dep_j∗dep_j)01./depj−pts_camera_j(0)/(dep_j∗dep_j)]{ {reduce} = \begin{bmatrix} 1./dep_j & 0 & -{pts\_camera\_j(0) / (dep\_j * dep\_j)} \\ 0 & 1./dep_j & -{pts\_camera\_j(0) / (dep\_j * dep\_j)} \end{bmatrix}}reduce=[1./depj001./depj−pts_camera_j(0)/(dep_j∗dep_j)−pts_camera_j(0)/(dep_j∗dep_j)]

由于权重(信息矩阵)因素，故 reduce = sqrt_info * reduce

    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_ERRORdouble 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;
#elsereduce << 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

对 Pose_i 求导

pts_camera_j 对 Pose_i求导，对pts_camera_j偏导乘以其值
pts_camera_j 对 P_i的偏导： qic−1Qj−1{q_{ic}^{-1} Q_j^{-1}}qic−1Qj−1
pts_camera_j 对 Q_i的偏导： qic−1Qj−1−Qi(pts_imu_i)∧{q_{ic}^{-1} Q_j^{-1} -Q_i ({pts\_imu\_i})^\wedge}qic−1Qj−1−Qi(pts_imu_i)∧ （通过右扰动求导）

对 Pose_j 求导

pts_camera_j 对 Pose_j求导，对pts_camera_j偏导乘以其值
pts_camera_j 对 P_j的偏导： qic−1Qj−1{q_{ic}^{-1} Q_j^{-1}}qic−1Qj−1
pts_camera_j 对 Q_j的偏导： qic−1(pts_imu_j)∧{q_{ic}^{-1} ({pts\_imu\_j})^\wedge}qic−1(pts_imu_j)∧ （通过右扰动求导）

对外参求导

pts_camera_j 对 Pose_{ic}求导，对pts_camera_j偏导乘以其值
pts_camera_j 对 P_{ic}的偏导： qic−1(Qj−1Qi−I){q_{ic}^{-1} (Q_j^{-1}Q_i -I)}qic−1(Qj−1Qi−I)
pts_camera_j 对 Q_{ic}的偏导： qic−1(pts_imu_j−tic)∧+qic−1Qj−1Qiqic(pts_camera_i−tic)∧{q_{ic}^{-1}({pts\_imu\_j}-t_{ic})^\wedge + q_{ic}^{-1}Q_j^{-1} Q_iq_{ic}({pts\_camera\_i}-t_{ic})^\wedge}qic−1(pts_imu_j−tic)∧+qic−1Qj−1Qiqic(pts_camera_i−tic)∧ （通过右扰动求导）

对特征点深度 inv_dep_i 求导

pts_camera_j 对 inv_dep_i求导，对pts_camera_j偏导乘以其值
−qic−1Qj−1Qiqicpts_i_td/(inv_dep_i)2{-q_{ic}^{-1}Q_j^{-1} Q_iq_{ic} {pts\_i\_td} / (inv\_dep\_i)^2}−qic−1Qj−1Qiqicpts_i_td/(inv_dep_i)2

对时间差求导

对时间差求偏导时， pts_camera_j 和 pts_camera_j都有其变量，故：
pts_camera_j 对 dt 求导：reduce∗−qic−1Qj−1Qiqicpts_i_td∗velocity_y/(inv_dep_i){ reduce * -q_{ic}^{-1}Q_j^{-1} Q_iq_{ic} {pts\_i\_td} * velocity\_y/ (inv\_dep\_i) }reduce∗−qic−1Qj−1Qiqicpts_i_td∗velocity_y/(inv_dep_i)
pts_camera_j 对 dt 求导：sqrt_info∗velocity_y{sqrt\_info *velocity\_y }sqrt_info∗velocity_y

代码实现

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());
#elsedouble dep_j = pts_camera_j.z();residual = (pts_camera_j / dep_j).head<2>() - pts_j_td.head<2>();
#endifresidual = 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_ERRORdouble 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;
#elsereduce << 1. / dep_j, 0, -pts_camera_j(0) / (dep_j * dep_j),0, 1. / dep_j, -pts_camera_j(1) / (dep_j * dep_j);
#endifreduce = 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;
}

不带Td 优化

直接使用 pts_i 和 pts_j 求解

残差

优化变量

imu_i 的全局位姿维度：7
imu_j 的全局位姿维度：7
外参维度：7
inv_dep_i 维度：1

残差计算

pts_i 是i时刻归一化相机坐标系下的3D坐标
第i帧相机坐标系下的的逆深度
double inv_dep_i = parameters[3][0];
第i帧相机坐标系下的3D坐标
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;
第i帧IMU坐标系下的3D坐标
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;
世界坐标系下的3D坐标
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;
第j帧imu坐标系下的3D坐标
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);
第j帧相机坐标系下的3D坐标
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic);
残差固件
Eigen::MapEigen::Vector2d residual(residuals);
- 若球面相机
  residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
- 针孔相机
  double dep_j = pts_camera_j.z();
  residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();

求解Jacobian

由上述可知，二者残差几乎一致，因此Jacobian类似

代码实现

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 是i时刻归一化相机坐标系下的3D坐标//第i帧相机坐标系下的的逆深度double inv_dep_i = parameters[3][0];//第i帧相机坐标系下的3D坐标Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i;//第i帧IMU坐标系下的3D坐标Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic;//世界坐标系下的3D坐标Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi;//第j帧imu坐标系下的3D坐标Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj);//第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//针孔相机模型double dep_j = pts_camera_j.z();residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endifresidual = sqrt_info * residual;//reduce 表示残差residual对fci（pts_camera_j）的导数，同样根据不同的相机模型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_ERRORdouble 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);
#endifreduce = sqrt_info * reduce;// 残差项的Jacobian// 先求fci对各项的Jacobian，然后用链式法则乘起来// 对第i帧的位姿 pbi,qbi      2X7的矩阵 最后一项是0if (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();}// 对第j帧的位姿 pbj,qbjif (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();}// 对相机到IMU的外参 pbc,qbc (qic,tic)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();}// 对逆深度 \lambda (inv_dep_i)if (jacobians[3]){Eigen::Map<Eigen::Vector2d> jacobian_feature(jacobians[3]);
#if 1jacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i * -1.0 / (inv_dep_i * inv_dep_i);
#elsejacobian_feature = reduce * ric.transpose() * Rj.transpose() * Ri * ric * pts_i;
#endif}}sum_t += tic_toc.toc();return true;
}

vins estimator ProjectionFactor (Td) factor相关推荐

zoho邮箱收费和免费区别_您需要了解有关适用于ios和android的新zoho vault移动应用程序的所有信息...
zoho邮箱收费和免费区别 The secret phrase is the true standard of computerized validation and access. Any run ...
ZOHO：游刃于快时代的“慢公司”
编辑:阿由设计:紫菜作为一家国际化的软件技术公司,尽管已经贵为全球三大CRM品牌之一,ZOHO的低调与务实在业内有目共睹.与其他同类公司相比,ZOHO不光低调,还以"慢"闻名: ...
萌新改代码系列（一）--VINS+GPS
VINS与GPS组合距离上次写博客过去了快一年了,这一年来我一直忙于与SLAM方向几乎没有关系的科研工作,说来惭愧,最终也没研究出个啥.最近得空,就把我之前开源的代码整理了一下,把与GPS的组合部分 ...
从LVI-SAM来看激光与视觉的紧耦合系统
0.简介快过年了,这里打算以这一篇博文作为结尾,来迎向2022春节.同时也希望新的一年能够继续不断提升自我. 1. 系统介绍本文提出了一种紧耦合的雷达视觉惯导SLAM系统,可以实时高精度鲁棒的进行 ...
Robust Initialization of Monocular Visual-Inertial Estimation on Aerial Robots
Robust Initialization of Monocular Visual-Inertial Estimation on Aerial Robots VINS初始化论文详解 I.Motivat ...
【VINS论文翻译】VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator
回到目录写在前面港科大的VINS-Mono作为目前state of the art的开源VIO项目,是研究视觉与IMU紧耦合的必读算法,网上的论文解读与代码实现也非常丰富(感谢!).为更好地进行学 ...
VINS-Mono代码解读——状态估计器流程 estimator 写在初始化和非线性优化前
前言本文主要介绍VINS的状态估计器模块(estimator),主要在代码中/vins_estimator节点的相关部分实现. 这个模块可以说是VINS的最核心模块,从论文的内容上来说,里面的内容包 ...
VINS紧耦合优化公式及代码解析
1.首先确定待优化的状态变量对应代码,优化参数为: Vector3d Ps[(WINDOW_SIZE + 1)];(平移向量) Vector3d Vs[(WINDOW_SIZE + 1)];(速度) ...
VINS System::ProcessBackEnd()
vins 通过IMU和Image的两个线程处理,将imu数据存储到imu_buf中,image数据存储到image_buf中,接下来通过void System::ProcessBackEnd()函数来 ...

vins estimator ProjectionFactor (Td) factor

文章目录

简介

重投影误差

卷帘快门相机时间差

带Td 的优化

残差

优化变量

残差计算

求解其Jacobian

对 pts_camera_j 求导

对 Pose_i 求导

对 Pose_j 求导

对外参求导

对特征点深度 inv_dep_i 求导

对时间差求导

代码实现

不带Td 优化

残差

优化变量

残差计算

求解Jacobian

代码实现

vins estimator ProjectionFactor (Td) factor相关推荐

最新文章

热门文章

vins estimator ProjectionFactor (Td) factor

文章目录

简介

重投影误差

卷帘快门相机 时间差

带Td 的优化

残差

优化变量

残差计算

求解其Jacobian

对 pts_camera_j 求导

对 Pose_i 求导

对 Pose_j 求导

对 外参 求导

对特征点深度 inv_dep_i 求导

对时间差 求导

代码实现

不带Td 优化

残差

优化变量

残差计算

求解Jacobian

代码实现

vins estimator ProjectionFactor (Td) factor相关推荐

最新文章

热门文章

卷帘快门相机时间差

对外参求导

对时间差求导