# 求解空间点坐标

## Triangulate in ORB-SLAM2

$\boldsymbol{P}_1 = \boldsymbol{K} \cdot [\boldsymbol{I}_{3 \times 3} \quad \mathbf{0}_{3 \times 1}], \quad \boldsymbol{P}_1 \in \mathbb{R}^{3 \times 4} \\ \boldsymbol{P}_2 = \boldsymbol{K} \cdot [ \boldsymbol{R}_{3 \times 3} \quad \boldsymbol{t}_{3 \times 1} ], \quad \boldsymbol{P}_2 \in \mathbb{R}^{3 \times 4}$

$\boldsymbol{T} = \boldsymbol{T}_{21} = [ \boldsymbol{R} \quad \boldsymbol{t} ] \in \mathbb{R}^{3 \times 4}$

$\tilde{\boldsymbol{p}_1} \times (\boldsymbol{P}_1 \tilde{\boldsymbol{P}}) = \mathbf{0} \\ \tilde{\boldsymbol{p}_2} \times (\boldsymbol{P}_2 \tilde{\boldsymbol{P}}) = \mathbf{0}$

$\boldsymbol{P}_1$$\boldsymbol{P}_2$ 按行展开（上标代表行索引）代入，对于第一视图有

$\begin{bmatrix} 0 & -1 & v_1 \\ 1 & 0 & -u_1 \\ -v_1 & u_1 & 0 \end{bmatrix} \cdot \begin{bmatrix} \boldsymbol{P}_1^1 \cdot \tilde{\boldsymbol{P}} \\ \boldsymbol{P}_1^2 \cdot \tilde{\boldsymbol{P}} \\ \boldsymbol{P}_1^3 \cdot \tilde{\boldsymbol{P}} \end{bmatrix} = \mathbf{0}$

$u_1 (\boldsymbol{P}_1^3 \cdot \tilde{\boldsymbol{P}}) - (\boldsymbol{P}_1^1 \cdot \tilde{\boldsymbol{P}}) = 0 \\ v_1 (\boldsymbol{P}_1^3 \cdot \tilde{\boldsymbol{P}}) - (\boldsymbol{P}_1^2 \cdot \tilde{\boldsymbol{P}}) = 0 \\ u_1 (\boldsymbol{P}_1^2 \cdot \tilde{\boldsymbol{P}}) - v_1 (\boldsymbol{P}_1^1 \cdot \tilde{\boldsymbol{P}}) = 0 \\$

$\begin{bmatrix} u_1 \boldsymbol{P}_1^3 - \boldsymbol{P}_1^1 \\ v_1 \boldsymbol{P}_1^3 - \boldsymbol{P}_1^2 \end{bmatrix} \cdot \tilde{\boldsymbol{P}} = \mathbf{0}$

$\begin{bmatrix} u_2 \boldsymbol{P}_2^3 - \boldsymbol{P}_2^1 \\ v_2 \boldsymbol{P}_2^3 - \boldsymbol{P}_2^2 \end{bmatrix} \cdot \tilde{\boldsymbol{P}} = \mathbf{0}$

$\boldsymbol{A_{4 \times 4}} \cdot \tilde{\boldsymbol{P}} = \begin{bmatrix} u_1 \boldsymbol{P}_1^3 - \boldsymbol{P}_1^1 \\ v_1 \boldsymbol{P}_1^3 - \boldsymbol{P}_1^2 \\ u_2 \boldsymbol{P}_2^3 - \boldsymbol{P}_2^1 \\ v_2 \boldsymbol{P}_2^3 - \boldsymbol{P}_2^2 \end{bmatrix} \cdot \tilde{\boldsymbol{P}} = \mathbf{0}$

SVD分解 $\boldsymbol{A}$

$\text{SVD}(\boldsymbol{A}) = \boldsymbol{U} \boldsymbol{\Sigma} \boldsymbol{V}^T$

$\tilde{\boldsymbol{P}} = (X,Y,Z,W) = \boldsymbol{V}_3$

$\boldsymbol{P} = (\frac{X}{W}, \frac{Y}{W}, \frac{Z}{W})$

void Initializer::Triangulate(
const cv::KeyPoint &kp1,
const cv::KeyPoint &kp2,
const cv::Mat &P1,
const cv::Mat &P2,
cv::Mat &x3D)
{
cv::Mat A(4,4,CV_32F);

A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);

cv::Mat u,w,vt;
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
x3D = vt.row(3).t();
x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}


## Triangulate in PTAM

$\boldsymbol{p}_1 \times (\boldsymbol{I}_{3 \times 4} \cdot \tilde{\boldsymbol{P}}) = \mathbf{0} \\ \boldsymbol{p}_2 \times (\boldsymbol{T}_{21} \cdot \tilde{\boldsymbol{P}}) = \mathbf{0}$

$\boldsymbol{T} = \boldsymbol{T}_{21} = [ \boldsymbol{R} \quad \boldsymbol{t} ] \in \mathbb{R}^{3 \times 4}$

$\boldsymbol{A_{6 \times 4}} \cdot \tilde{\boldsymbol{P}} = \begin{bmatrix} \boldsymbol{p}_1 \times \boldsymbol{I}_{3 \times 4} \\ \boldsymbol{p}_2 \times \boldsymbol{T}_{21} \end{bmatrix} \cdot \tilde{\boldsymbol{P}} = \mathbf{0}$

SVD分解 $\boldsymbol{A}$

$\text{SVD}(\boldsymbol{A}) = \boldsymbol{U} \boldsymbol{\Sigma} \boldsymbol{V}^T$

$\tilde{\boldsymbol{P}} = (X,Y,Z,W) = \boldsymbol{V}_3$

$\boldsymbol{P} = (\frac{X}{W}, \frac{Y}{W}, \frac{Z}{W})$

Vector<3> MapMaker::Triangulate(
SE3<> se3AfromB,
const Vector<2> &v2A,
const Vector<2> &v2B)
{
Matrix<3,4> PDash;
PDash.slice<0,0,3,3>() = se3AfromB.get_rotation().get_matrix();
PDash.slice<0,3,3,1>() = se3AfromB.get_translation().as_col();

Matrix<4> A;
A[0][0] = -1.0; A[0][1] =  0.0; A[0][2] = v2B[0]; A[0][3] = 0.0;
A[1][0] =  0.0; A[1][1] = -1.0; A[1][2] = v2B[1]; A[1][3] = 0.0;
A[2] = v2A[0] * PDash[2] - PDash[0];
A[3] = v2A[1] * PDash[2] - PDash[1];

SVD<4,4> svd(A);
Vector<4> v4Smallest = svd.get_VT()[3];
if(v4Smallest[3] == 0.0)
v4Smallest[3] = 0.00001;
return project(v4Smallest);
}

Vector<3> MapMaker::TriangulateNew(
SE3<> se3AfromB,
const Vector<2> &v2A,
const Vector<2> &v2B)
{
Vector<3> v3A = unproject(v2A);
Vector<3> v3B = unproject(v2B);

Matrix<3> m3A = TooN::Zeros;
m3A[0][1] = -v3A[2];
m3A[0][2] =  v3A[1];
m3A[1][2] = -v3A[0];
m3A[1][0] = -m3A[0][1];
m3A[2][0] = -m3A[0][2];
m3A[2][1] = -m3A[1][2];
Matrix<3> m3B = TooN::Zeros;
m3B[0][1] = -v3B[2];
m3B[0][2] =  v3B[1];
m3B[1][2] = -v3B[0];
m3B[1][0] = -m3B[0][1];
m3B[2][0] = -m3B[0][2];
m3B[2][1] = -m3B[1][2];

Matrix<3,4> m34AB;
m34AB.slice<0,0,3,3>() = se3AfromB.get_rotation().get_matrix();
m34AB.slice<0,3,3,1>() = se3AfromB.get_translation().as_col();

SE3<> se3I;
Matrix<3,4> m34I;
m34I.slice<0,0,3,3>() = se3I.get_rotation().get_matrix();
m34I.slice<0,3,3,1>() = se3I.get_translation().as_col();

Matrix<3,4> PDashA = m3A * m34AB;
Matrix<3,4> PDashB = m3B * m34I;

Matrix<6,4> A;
A.slice<0,0,3,4>() = PDashA;
A.slice<3,0,3,4>() = PDashB;

SVD<6,4> svd(A);
Vector<4> v4Smallest = svd.get_VT()[3];
if(v4Smallest[3] == 0.0)
v4Smallest[3] = 0.00001;

return project(v4Smallest);
}


# 求解空间点深度

$\boldsymbol{T} = \boldsymbol{T}_{21} = [ \boldsymbol{R} \quad \boldsymbol{t} ] \in \mathbb{R}^{3 \times 4}$

$Z_2 \cdot \boldsymbol{p}_2 = \boldsymbol{T}_{21} \cdot ( Z_1 \cdot \boldsymbol{p}_1 ) = Z_1 \cdot \boldsymbol{R} \boldsymbol{p}_1 + \boldsymbol{t}$

$\begin{bmatrix} \boldsymbol{p}_2 & -\boldsymbol{R} \boldsymbol{p}_1 \end{bmatrix} \cdot \begin{bmatrix} Z_2 \\ Z_1 \end{bmatrix} = \boldsymbol{t}$

## Triangulate in SVO

$A^T A x = A^T b$

$x = (A^TA)^{-1} A^T b$

bool depthFromTriangulation(
const SE3& T_search_ref,
const Vector3d& f_ref,
const Vector3d& f_cur,
double& depth)
{
Matrix<double,3,2> A; A << T_search_ref.rotation_matrix() * f_ref, f_cur;
const Matrix2d AtA = A.transpose() * A;
if(AtA.determinant() < 0.000001)
return false;
const Vector2d depth2 =
- AtA.inverse()* A.transpose() * T_search_ref.translation();
depth = fabs(depth2[0]);
return true;
}


## Triangulate in REMODE

$\begin{bmatrix} \boldsymbol{p}_2 \\ \boldsymbol{R} \boldsymbol{p}_1 \end{bmatrix} \begin{bmatrix} \boldsymbol{p}_2 & -\boldsymbol{R} \boldsymbol{p}_1 \end{bmatrix} \begin{bmatrix} Z_2 \\ Z_1 \end{bmatrix} = \begin{bmatrix} \boldsymbol{p}_2 \boldsymbol{p}_2 & -\boldsymbol{p}_2 \cdot \boldsymbol{R} \boldsymbol{p}_1 \\ \boldsymbol{p}_2 \cdot \boldsymbol{R} \boldsymbol{p}_1 & -\boldsymbol{R} \boldsymbol{p}_1 \cdot \boldsymbol{R} \boldsymbol{p}_1 \end{bmatrix} \begin{bmatrix} Z_2 \\ Z_1 \end{bmatrix} = \boldsymbol{t}$

// Returns 3D point in reference frame
// Non-linear formulation (ref. to the book 'Autonomous Mobile Robots')
__device__ __forceinline__
float3 triangulatenNonLin(
const float3 &bearing_vector_ref,
const float3 &bearing_vector_curr,
const SE3<float> &T_ref_curr)
{
const float3 t = T_ref_curr.getTranslation();
float3 f2 = T_ref_curr.rotate(bearing_vector_curr);
const float2 b = make_float2(dot(t, bearing_vector_ref),
dot(t, f2));
float A[2*2];
A[0] = dot(bearing_vector_ref, bearing_vector_ref);
A[2] = dot(bearing_vector_ref, f2);
A[1] = -A[2];
A[3] = dot(-f2, f2);

const float2 lambdavec = make_float2(A[3] * b.x - A[1] * b.y,
-A[2] * b.x + A[0] * b.y) / (A[0] * A[3] - A[1] * A[2]);
const float3 xm = lambdavec.x * bearing_vector_ref;
const float3 xn = t + lambdavec.y * f2;
return (xm + xn)/2.0f;
}


# Reference

©️2019 CSDN 皮肤主题: 大白 设计师: CSDN官方博客