版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/u011178262/article/details/86729887
求解空间点坐标
Triangulate in ORB-SLAM2
已知，两个视图下匹配点的 图像坐标
p1 和
p2，两个相机的相对位姿
T ，相机内参矩阵
K，求 对应的三维点坐标
P，其齐次坐标为
P~。
两个视图的 投影矩阵 分别为
P1=K⋅[I3×303×1],P1∈R3×4P2=K⋅[R3×3t3×1],P2∈R3×4
T=T21=[Rt]∈R3×4
由于是齐次坐标的表示形式，使用叉乘消去齐次因子
p1~×(P1P~)=0p2~×(P2P~)=0
把
P1 和
P2 按行展开（上标代表行索引）代入，对于第一视图有
⎣⎡01−v1−10u1v1−u10⎦⎤⋅⎣⎡P11⋅P~P12⋅P~P13⋅P~⎦⎤=0
即
u1(P13⋅P~)−(P11⋅P~)=0v1(P13⋅P~)−(P12⋅P~)=0u1(P12⋅P~)−v1(P11⋅P~)=0
可见第三个式子可以由上两个式子线性表示，所以只需要取前连个式子即可
[u1P13−P11v1P13−P12]⋅P~=0
同样的，对于第二视图
[u2P23−P21v2P23−P22]⋅P~=0
组合起来
A4×4⋅P~=⎣⎢⎢⎡u1P13−P11v1P13−P12u2P23−P21v2P23−P22⎦⎥⎥⎤⋅P~=0
求解
P 相当于解一个 线性最小二乘问题。
SVD分解
A
SVD(A)=UΣVT
方程的解为
A 的 最小奇异值 对应的 奇异矢量，即 齐次坐标
P~=(X,Y,Z,W)=V3
最终，
P（第一视图坐标系下三维坐标）为
P=(WX,WY,WZ)
orbslam2_cg中示例代码：
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
已知，两个视图下匹配点的 归一化平面(z=1)齐次坐标
p1 和
p2，两个相机的相对位姿
T，求 对应的三维点坐标
P（第一视图坐标系下三维坐标），其齐次坐标为
P~。
方程
p1×(I3×4⋅P~)=0p2×(T21⋅P~)=0
T=T21=[Rt]∈R3×4
矩阵形式
A6×4⋅P~=[p1×I3×4p2×T21]⋅P~=0
求解
P 相当于解一个 线性最小二乘问题。
SVD分解
A
SVD(A)=UΣVT
方程的解为
A 的 最小奇异值 对应的 奇异矢量，即 齐次坐标
P~=(X,Y,Z,W)=V3
最终，
P（第一视图坐标系下三维坐标）为
P=(WX,WY,WZ)
ptam_cg中示例代码Triangulate：
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);
}
ptam_cg中示例代码TriangulateNew：
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);
}
求解空间点深度
已知，两个视图下匹配点的 归一化平面(z=1)齐次坐标
p1 和
p2，两个相机的相对位姿
T，求解空间点深度
Z1 和
Z2
T=T21=[Rt]∈R3×4
Z2⋅p2=T21⋅(Z1⋅p1)=Z1⋅Rp1+t
矩阵形式
[p2−Rp1]⋅[Z2Z1]=t
Triangulate in SVO
上式即
Ax=b 的形式，解该方程可以用 正规方程
ATAx=ATb
解得
x=(ATA)−1ATb
svo_cg中示例代码：
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
由于解向量是二维的，对上式采用 克莱默法则 求解：
[p2Rp1][p2−Rp1][Z2Z1]=[p2p2p2⋅Rp1−p2⋅Rp1−Rp1⋅Rp1][Z2Z1]=t
REMODE中示例代码：
// 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