latex 点乘_多视几何——三角化求解3D空间点坐标

a268d39362fc20cd21a36a45a1396e60.png

摘要

多视几何中,相机在两个位置观测到同一个空间点,希望通过两次相机位姿和图像观测点坐标求得三维空间点坐标,我们称这个过程为三角化(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);
}

efd33c0e3e96073d949f5ae523064705.png

2. 数学推导

先介绍一个重要的向量叉乘向矩阵运算转换的性质[4],

其中,

表示向量
对应的反对称矩阵(skew-symmetric matrix)。

对于,

为三维空间点在世界坐标系下的齐次坐标
为世界坐标系到相机坐标系的变换
为归一化平面坐标,
为深度值,有:

借助式(2-1)展开,有:

其中:

第一行

,第二行
,再相加,即可得到第三行,因此,其线性相关,保留前两行即可,有
因此,已知一个归一化平面坐标
和变换
,可以构建两个关于
的线性方程组。有两个以上的图像观测,即可求出

上述方程没有非零解,使用SVD求最小二乘解,解可能不满足齐次坐标形式(第四个元素为1),因此,

求得空间点坐标,但是这个解几何意义不明确[1],属于代数最小误差解。不等价于最小重投影误差,也不是最小化3D点距离误差。详细解释参考[1]中PPT。

3. 扩展

在VINS-Mono中给出了归一化平面坐标

, 如果只是给出像素坐标
,并且已知相机内参
,求解3D点坐标方式类似。

其中,

的形式与式(2-3)的形式一致,同理可以得到式(2-4)的方程。

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

表情包
插入表情
评论将由博主筛选后显示,对所有人可见 | 还能输入1000个字符
相关推荐
©️2020 CSDN 皮肤主题: 深蓝海洋 设计师:CSDN官方博客 返回首页