SLAM中的三角化方案

wzj5530

已于 2022-04-13 19:17:34 修改

阅读量4.3k

点赞数 1

分类专栏： SLAM Math 文章标签：自动驾驶矩阵线性代数视觉检测人工智能

于 2017-12-13 17:41:14 首次发布

本文链接：https://blog.csdn.net/u011089570/article/details/78791123

版权

SLAM 同时被 2 个专栏收录

7 篇文章 2 订阅

订阅专栏

Math

4 篇文章 1 订阅

订阅专栏

Linear triangulation methods 又称为线性三角形法，其作用是通过同一个三维空间点在两个视图中的平面坐标（也可以认为是像素坐标），求解这个三维空间点的世界坐标。通常的求解方式有两种，一种是数值求解；一种是几何求解。

首先给出问题：有一个三维空间点X，X分别被两帧图像观测到，这两帧图像拍摄时刻的相机姿态分别为P_cam1_w和P_cam2_w，将X在cam1坐标系下的单位向量表示 $f_{1}$ ，将X在cam2坐标系下的单位向量表示 $f_{2}$ ；P_cam1_w和P_cam2_w为3行4列的矩阵表示，由3行3列的旋转矩阵和3行1列的平移矩阵组成。

1. 数值求解方案

2. 几何求解方案

$L_{1} = L_{2} cos(\gamma ) + \begin{vmatrix} t_{12} \end{vmatrix}cos(\alpha )$

$L_{2} = L_{1} cos(\gamma ) + \begin{vmatrix} t_{12} \end{vmatrix}cos(\beta )$

$cos(\gamma ) = f_{1}.dot(f_{2})$

$cos(\alpha ) = t_{12}.dot(f_{1}) / \begin{vmatrix}t_{12} \end{vmatrix}$

$cos(\beta ) = -t_{12}.dot(f_{2}) / \begin{vmatrix}t_{12} \end{vmatrix}$

$L_{1} - L_{2} cos(\gamma ) = \begin{vmatrix} t_{12} \end{vmatrix}cos(\alpha )$

$L_{1} - L_{2}f_{1}.dot(f_{2}) = t_{12}.dot(f_{1})$ 公式 1

$L_{1} cos(\gamma ) - L_{2} = - \begin{vmatrix} t_{12} \end{vmatrix}cos(\beta )$

$L_{1} f_{1}.dot(f_{2}) - L_{2} =t_{12}.dot(f_{2})$ 公式 2

最终由公式1和公式2得出：

$\begin{bmatrix} f_{1}.dot(f_{1}), & -f_{1}.dot(f_{2}) \\ f_{1}.dot(f_{2}),& -f_{2}.dot(f_{2}) \end{bmatrix} \begin{bmatrix} L_{1}\\ L_{2} \end{bmatrix} = \begin{bmatrix} t_{12}.dot(f_{1})\\ t_{12}.dot(f_{2}) \end{bmatrix}$

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在两个视图中的平面坐标 $x_{1}$ ， $x_{2}$ ，以及两个视图的投影矩阵 $P_{1}$ ， $P_{2}$ 。

求解过程：

1. 根据相机模型可以得出：

$x_{1}=P_{1}X$ ， $x_{2}=P_{2}X$

2. 对上面公式同时进行叉乘计算：

$x_{1}\bigotimes x_{1}=x_{1}\bigotimes P_{1}\mathbf{X}$ ， $x_{2}\bigotimes x_{2}=x_{2}\bigotimes P_{2}\mathbf{X}$

3. 根据向量本身的叉乘为零向量，得：

$\mathbf{0}=x_{1}\bigotimes P_{1}\mathbf{X}$ ， $\mathbf{0}=x_{2}\bigotimes P_{2}\mathbf{X}$

4. 将向量叉乘表示为矩阵形式为：

$\begin{bmatrix} & 0 & -1 & y\\ & 1 & 0 & -x\\ & -y & x & 0 \end{bmatrix} \mathbf{P}\mathbf{X} = \mathbf{0}$

5. 将 $\mathbf{P}\mathbf{X}$ 按行展开， $P^{1}$ 代表矩阵第一行：

$\begin{bmatrix} & 0 & -1 & y\\ & 1 & 0 & -x\\ & -y & x & 0 \end{bmatrix} \begin{bmatrix} P^{1}\mathbf{X}\\ P^{2}\mathbf{X}\\ P^{3}\mathbf{X} \end{bmatrix} = \mathbf{0}$

6. 展开上面的公式：

$\left ( \mathbf{x}P^{3} - P^{1}\right )\mathbf{X} = \mathbf{0}$

$\left ( \mathbf{y}P^{3} - P^{2}\right )\mathbf{X} = \mathbf{0}$

$\left ( \mathbf{x}P^{2} - \mathbf{y} P^{1}\right )\mathbf{X} = \mathbf{0}$

7. 可以发现上面的式3和式1式2是线性相关的，只取式1式2可以表示成

$\begin{bmatrix} \mathbf{x1}P1^{3} - P1^{1}\\ \mathbf{y1}P1^{3} - P1^{2}\\ \mathbf{x2}P2^{3} - P2^{1}\\ \mathbf{y2}P2^{3} - P2^{2} \end{bmatrix} \mathbf{X} = \mathbf{0}$

8. 此时相当于求解最小二乘问题，后面将讲解一下如何使用奇异值分解求解AX=0的最小二乘问题。

---------------------------------------------------------------------------------------------------------

对于AX=b的最小二乘求解：

$\mathbf{X}= \left ( A^{T}\mathbf{A} \right )^{-1}A^{T}\mathbf{b}$

但是当b=0时，像上面的求解空间点的世界坐标，X总是零向量，此时可以使用SVD分解进行求解，下面论证下，SVD求解最小二乘的可行性：证明 $A_{m\times n}X_{n\times 1}= \mathbf{0}\left ( m> n \right )$ 的最小二乘解是A经过SVD分解后的V的第一列向量。

1. A经过SVD分解后，可以得：

$A_{m\times n}= U_{m\times m}D_{m\times n}\left ( V_{n\times n} \right )^{T}$ （式1）

其中：

$D_{m\times n}=\begin{bmatrix} d_{1} & & & & \\ & .& & & \\ & & .& & \\ & & &. & \\ & & & & d_{n}\\ & &0_{\left ( m-n \right )\times n} & & \end{bmatrix}$ （式2）

而且：

$d_{n}> d_{n-1}>...>d_{1}>0$

2. 令：

$D_{0}= \begin{bmatrix} d_{1} & & & & \\ & . & & & \\ & & . & & \\ & & & . & \\ & & & & d_{n} \end{bmatrix}$ （式3）

则：

$D_{m\times n}^{T}D_{m\times n}=D_{0}^{2}$

3. 若设：

$b= AX$

则：

$b^{T}\mathbf{b}=\left ( AX \right )^{T}\left ( AX \right )$ （式4）

将式1代入式4得：

$b^{T}\mathbf{b}=X^{T}VD^{T}U^{T}UDV^{T}X$

$=X^{T}VD^{T}DV^{T}X$ （式5）

$=X^{T}VD_{0}^2{}V^{T}X$

因为 $V_{n\times n}$ 是正交矩阵，所以V的列向量是 $R^{n}$ 的一组标准正交基，故 $X_{n\times 1}$ 可以由 $V_{n\times n}$ 表示：

$X_{n\times 1}=V_{n\times n}L_{n\times 1}$ （式6）

其中 $L_{n\times 1}=\begin{bmatrix} l_{1} &. & . & . &l_{n} \end{bmatrix}^{T}$ ， $l$ 不全为零

将式6代入式5得：

$b^{T}b=L^{T}V^{T}VD_{0}^{2}V^{T}VL$

$=L^{T}D_{0}^{2}L$

$=\left ( D_{0}L \right )^{T}\left ( D_{0}L \right )$

$=l_{1}^{2}d_{1}^{2}+ ... + l_{n}^{2}d_{n}^{2}$

因为 $d_{n}> d_{n-1}>...>d_{1}>0$ 所以当 $l_{2}= ... = l_{n} = 0$ 时 $b^{T}b$ 有最小值

此时X等于V的第一列向量

wzj5530

关注

1
点赞
踩
12

收藏

觉得还不错? 一键收藏
0
评论
SLAM中的三角化方案

Linear triangulation methods 又称为线性三角形法，其作用是通过一组空间点在两个视图中的平面坐标，求解这组空间点的世界坐标。已知条件为：空间点的世界坐标X，X在两个视图中的平面坐标，，以及两个视图的投影矩阵，。求解过程：1. 根据相机模型可以得出：， 2. 对上面公式同时进行叉乘计算：
复制链接

扫一扫

专栏目录