Linear triangulation methods 又称为线性三角形法,其作用是通过同一个三维空间点在两个视图中的平面坐标(也可以认为是像素坐标),求解这个三维空间点的世界坐标。通常的求解方式有两种,一种是数值求解;一种是几何求解。
首先给出问题:有一个三维空间点X,X分别被两帧图像观测到,这两帧图像拍摄时刻的相机姿态分别为P_cam1_w和P_cam2_w,将X在cam1坐标系下的单位向量表示,将X在cam2坐标系下的单位向量表示
;P_cam1_w和P_cam2_w为3行4列的矩阵表示,由3行3列的旋转矩阵和3行1列的平移矩阵组成。
1. 数值求解方案
2. 几何求解方案
公式 1
公式 2
最终由公式1和公式2得出:
3. 参考代码实现
#include <opengv/triangulation/methods.hpp>
#include <Eigen/Eigenvalues>
opengv::point_t
opengv::triangulation::triangulate(
const relative_pose::RelativeAdapterBase & adapter,
size_t index )
{
translation_t t12 = adapter.gett12();
rotation_t R12 = adapter.getR12();
Eigen::Matrix<double,3,4> P1 = Eigen::Matrix<double,3,4>::Zero();
P1.block<3,3>(0,0) = Eigen::Matrix3d::Identity();
Eigen::Matrix<double,3,4> P2 = Eigen::Matrix<double,3,4>::Zero();
P2.block<3,3>(0,0) = R12.transpose();
P2.block<3,1>(0,3) = -R12.transpose()*t12;
bearingVector_t f1 = adapter.getBearingVector1(index);
bearingVector_t f2 = adapter.getBearingVector2(index);
Eigen::MatrixXd A(4,4);
A.row(0) = f1[0] * P1.row(2) - f1[2] * P1.row(0);
A.row(1) = f1[1] * P1.row(2) - f1[2] * P1.row(1);
A.row(2) = f2[0] * P2.row(2) - f2[2] * P2.row(0);
A.row(3) = f2[1] * P2.row(2) - f2[2] * P2.row(1);
Eigen::JacobiSVD< Eigen::MatrixXd > mySVD(A, Eigen::ComputeFullV );
point_t worldPoint;
worldPoint[0] = mySVD.matrixV()(0,3);
worldPoint[1] = mySVD.matrixV()(1,3);
worldPoint[2] = mySVD.matrixV()(2,3);
worldPoint = worldPoint / mySVD.matrixV()(3,3);
return worldPoint;
};
opengv::point_t
opengv::triangulation::triangulate2(
const relative_pose::RelativeAdapterBase & adapter,
size_t index )
{
translation_t t12 = adapter.gett12();
rotation_t R12 = adapter.getR12();
bearingVector_t f1 = adapter.getBearingVector1(index);
bearingVector_t f2 = adapter.getBearingVector2(index);
bearingVector_t f2_unrotated = R12 * f2;
Eigen::Vector2d b;
b[0] = t12.dot(f1);
b[1] = t12.dot(f2_unrotated);
Eigen::Matrix2d A;
A(0,0) = f1.dot(f1);
A(1,0) = f1.dot(f2_unrotated);
A(0,1) = -A(1,0);
A(1,1) = -f2_unrotated.dot(f2_unrotated);
Eigen::Vector2d lambda = A.inverse() * b;
Eigen::Vector3d xm = lambda[0] * f1;
Eigen::Vector3d xn = t12 + lambda[1] * f2_unrotated;
point_t point = ( xm + xn )/2;
return point;
};
https://github.com/ethz-asl/opengv/blob/master/src/triangulation/methods.cpp
4.延申思考
相对于数值求解方案,X在两个相机中的表示除了单位向量,也可以是像素坐标,此时P矩阵就是内参矩阵和转换矩阵的联合表达。
已知条件为:空间点的世界坐标X,X在两个视图中的平面坐标,
,以及两个视图的投影矩阵
,
。
求解过程:
1. 根据相机模型可以得出:
,
2. 对上面公式同时进行叉乘计算:
,
3. 根据向量本身的叉乘为零向量,得:
,
4. 将向量叉乘表示为矩阵形式为:
5. 将按行展开,
代表矩阵第一行:
6. 展开上面的公式:
7. 可以发现上面的式3和式1式2是线性相关的,只取式1式2可以表示成
8. 此时相当于求解最小二乘问题,后面将讲解一下如何使用奇异值分解求解AX=0的最小二乘问题。
---------------------------------------------------------------------------------------------------------
---------------------------------------------------------------------------------------------------------
---------------------------------------------------------------------------------------------------------
对于AX=b的最小二乘求解:
但是当b=0时,像上面的求解空间点的世界坐标,X总是零向量,此时可以使用SVD分解进行求解,下面论证下,SVD求解最小二乘的可行性:证明的最小二乘解是A经过SVD分解后的V的第一列向量。
1. A经过SVD分解后,可以得:
(式1)
其中:
(式2)
而且:
2. 令:
(式3)
则:
3. 若设:
则:
(式4)
将式1代入式4得:
(式5)
因为是正交矩阵,所以V的列向量是
的一组标准正交基,故
可以由
表示:
(式6)
其中,
不全为零
将式6代入式5得:
因为所以当
时
有最小值
此时X等于V的第一列向量