摘要
多视几何中,相机在两个位置观测到同一个空间点,希望通过两次相机位姿和图像观测点坐标求得三维空间点坐标,我们称这个过程为三角化(Triangulate),如图1。
VINS-Mono / VINS-Fusion中triangulatePoint()
函数通过三角化求解空间点坐标,代码所体现的数学描述不是很直观,查找资料,发现参考文献[1]对这个问题进行详细解释,记录笔记以备忘。
1. VINS-Mono中相关代码
void FeatureManager::triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1,
Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d)
{
Eigen::Matrix4d design_matrix = Eigen::Matrix4d::Zero();
design_matrix.row(0) = point0[0] * Pose0.row(2) - Pose0.row(0);
design_matrix.row(1) = point0[1] * Pose0.row(2) - Pose0.row(1);
design_matrix.row(2) = point1[0] * Pose1.row(2) - Pose1.row(0);
design_matrix.row(3) = point1[1] * Pose1.row(2) - Pose1.row(1);
Eigen::Vector4d triangulated_point;
triangulated_point =
design_matrix.jacobiSvd(Eigen::ComputeFullV).matrixV().rightCols<1>();
point_3d(0) = triangulated_point(0) / triangulated_point(3);
point_3d(1) = triangulated_point(1) / triangulated_point(3);
point_3d(2) = triangulated_point(2) / triangulated_point(3);
}
2. 数学推导
先介绍一个重要的向量叉乘向矩阵运算转换的性质[4],
其中,
对于,
借助式(2-1)展开,有:
其中:
第一行
上述方程没有非零解,使用SVD求最小二乘解,解可能不满足齐次坐标形式(第四个元素为1),因此,
求得空间点坐标,但是这个解几何意义不明确[1],属于代数最小误差解。不等价于最小重投影误差,也不是最小化3D点距离误差。详细解释参考[1]中PPT。
3. 扩展
在VINS-Mono中给出了归一化平面坐标
其中,
4. 总结
在多视几何的公式推导中,多次使用"共线向量叉乘等于0"的性质,将等式的某一项置为0,式(2-2)中应用了该性质,在本质矩阵推导中,也应用了该性质,如本质矩阵的前两步推导:
在多视几何公式推导中,另一个常用的性质是"互相垂直的两个向量点乘为零",在此不再展开。
参考文献
[1] Lecture 7.2 Triangulation https://www.uio.no/studier/emner/matnat/its/UNIK4690/v16/forelesninger/lecture_7_2-triangulation.pdf