文章目录
- 李群、李代数
- 李群、李代书与坐标变换的对应关系
- SE(3)上的李代数求导数
-
- SLAM中的使用
- 重投影误差
- 误差项构建
- 对应C++代码
- 雅克比矩阵
- frame_i的Ti2w从IMU坐标系变换到world坐标系
- frame_j的Ti2w从IMU坐标系变换到world坐标系
- Tc2i从camera坐标系变换到imu坐标系(相机外参)
- frame_i中的逆深度inv_dep_i
李群、李代数
数学概念什么的不做介绍了,本人不是数学专业也解释不清楚。咱的目的是怎么即使不是很能理解还能够把这个东西理解下来,并且还能看懂和编程使用。就是像考试怎么不会还能把它做对!
反正下面是本人不知道对不对的理解方式。
- 视觉SLAM十四讲第四讲
- 等等
李群、李代书与坐标变换的对应关系
SE(3)上的李代数求导数
首先需要补充一个运算符号^
:将向量转换为反对称矩阵
就是说对扰动求导数,SLAM中的主要应用就是这个其他的也不用很懂,记住下面的左乘、右乘的公式其实就行了,然后用公式推导3-4个雅克比矩阵基本上就会用了。但是《视觉SLAM十四讲》中只有左乘扰动的推导过程,其他的地方咱也没有搜到合适的右乘扰动的讲解。
左乘扰动、右乘扰动
参考这个链接来吧,建议直接背公式套进去就行了。
怎么选取用左or右扰动?
大致就是说转换到world坐标系或者global坐标系这些固定不变的坐标系使用右乘扰动,其他的loacl坐标系下都是左乘扰动。
借鉴参考这个https://zhuanlan.zhihu.com/p/108478399
SLAM中的使用
这里以VINS_MONO中的重投影误差的约束为例解释使用。
projection_factor.cpp
重投影误差
误差项构建
对应C++代码
bool ProjectionFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians)
函数中
TicToc tic_toc;
Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); //共同观测的frame_i位姿
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]); //共同观测的frame_j位姿
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]); //相机外参Tcamera2imu
Eigen::Quaterniond qic(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);
double inv_dep_i = parameters[3][0]; //相机逆深度
// 将frame_i下的3d点投影到frame_j坐标系下
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; //使用逆深度将frame_i的point归一化坐标恢复到真实3D坐标
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; //将frame_i的point转化到imu坐标系
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; //将frame_i的point转化到world坐标系
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); //将frame_i的point转化到frame_j的imu坐标系下
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); //将frame_i的point转化到frame_j的camera坐标系下
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(); //投影到frame_j坐标系下point的深度
residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>(); //归一化坐标的重投影误差
#endif
residual = sqrt_info * residual; // sqrt_info = 焦距/1.5*I,信息矩阵,重投影误差的可靠性相等
雅克比矩阵
本质就是对误差项的各个变量求偏导数呗
咱也不知道为什么VINS_MONO的扰动都是右乘扰动。一开始拿《视觉SLAM14讲》的公式去推导怎么都不对QAQ
frame_i的Ti2w从IMU坐标系变换到world坐标系
- 公式推导
- 对应C++代码
if (jacobians[0]) //共同观测的frame_i位姿
{
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();
}
frame_j的Ti2w从IMU坐标系变换到world坐标系
- 公式推导
- 对应C++代码
if (jacobians[1]) //共同观测的frame_j位姿
{
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();
}
Tc2i从camera坐标系变换到imu坐标系(相机外参)
这个太多了,不想写了,搜索参考崔华坤《VINS论文推导及代码解析》很详细了。注意扰动的二阶项近似为0就是了。
if (jacobians[2]) //相机外参Tcamera2imu
{
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();
}
frame_i中的逆深度inv_dep_i
- 公式推导
不涉及李群、李代数,即1/inv_dep_i
的导数为1/(inv_dep_i*inv_dep_i)
。 - 对应C++代码
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
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)