三角化特征点(triangulation)方法及实现对比

文章目录

  • 三角化特征点(triangulation)方法及实现对比
    • 问题
    • 解决方法
      • 1 构造AX=0的形式,用SVD的方法进行求解
        • 求解原理
        • 代码实现
      • 2 Cramer's(克莱默)法则求解Ax=b问题
        • 求解原理
        • 实现代码
      • 3 单一化未知量求解
        • 求解原理
        • 实现代码
      • 4 A x = b Ax=b Ax=b求解, x = ( A T A ) − 1 A T b x=\left(A^TA\right)^{-1}A^Tb x=(ATA)1ATb
        • 求解原理
        • 代码实现
      • 5 DEMO算法的几何计算深度
        • 求解原理
        • 代码实现
      • 6 Tips
      • 7 完整代码
      • 9 其他
    • 参考链接

三角化特征点(triangulation)方法及实现对比

问题

已知两帧相机在世界坐标下的位姿 [ R , t ] [R,t] [R,t], 求两帧的共视点的3维坐标。

设两帧为参考帧(reference)和当前帧(current),共视点 p p p在两帧下的坐标分别为 x r x_r xr x c x_c xc,对应深度为 d r d_r dr d c d_c dc。图像归一化齐次坐标为 p r p_r pr p c p_c pc(非像素坐标)。
p r = [ u r v r 1 ] ,    p c = [ u c v c 1 ] p_r=\left[\begin{matrix}u_r\\v_r\\1\\\end{matrix}\right],\ \ p_c=\left[\begin{matrix}u_c\\v_c\\1\\\end{matrix}\right] pr=urvr1,  pc=ucvc1
满足:
x r = R r c x c + t r c x_r=R_{rc}x_c+t_{rc} xr=Rrcxc+trc d r p r = d c R r c p c + t r c d_rp_r=d_cR_{rc}p_c+t_{rc} drpr=dcRrcpc+trc

解决方法

1 构造AX=0的形式,用SVD的方法进行求解

求解原理

设第 k k k帧相机的变换矩阵为 T k = [ R k , t k ] ∈ R 3 × 4 T_k=[R_k,t_k]∈\mathbb{R}^{3×4} Tk=[Rk,tk]R3×4
则:
d k p = T k x w d_kp=T_k\bm{x}_w dkp=Tkxw
空间点的世界齐次坐标为 x w \bm{x}_w xw d k d_k dk为观测点 p p p对应的深度值, p p p为第 k k k帧相机该像素点的归一化齐次坐标。
得: (*)
d k [ u k v k 1 ] = T k [ x w y w z w 1 ] d_k\left[\begin{matrix}u_k\\v_k\\1\\\end{matrix}\right]=T_k\left[\begin{matrix}x_w\\y_w\\z_w\\1\\\end{matrix}\right] dkukvk1=Tkxwywzw1
由上述方程第三行可知: T 3 k T_3^k T3k表示 T k T_k Tk第三行。
d k = T 3 k x w d_k=T_3^k\bm{x}_w dk=T3kxw
将其带入(*)前两行,消去 d k d_k dk,得:
u k T 3 k x w = T 1 k x w u_kT_3^k\bm{x}_w=T_1^k\bm{x}_w ukT3kxw=T1kxw v k T 3 k x w = T 2 k x w v_kT_3^k\bm{x}_w=T_2^k\bm{x}_w vkT3kxw=T2kxw
每次观测得到以上两个方程,经过多次观测,得到以下方程,将未知量 x w \bm{x}_w xw移至一侧。
[ u 1 T 3 1 − T 1 1 v 1 T 3 1 − T 2 1 ⋮ u n T 3 n − T 1 n v n T 3 n − T 2 n ] x w = 0 → A x = 0 \left[\begin{matrix}u_1T_3^1-T_1^1\\v_1T_3^1-T_2^1\\\begin{matrix}\vdots\\u_nT_3^n-T_1^n\\v_nT_3^n-T_2^n\\\end{matrix}\\\end{matrix}\right]\bm{x}_w=0\rightarrow A\bm{x}=0 u1T31T11v1T31T21unT3nT1nvnT3nT2nxw=0Ax=0

求解问题变为:
在这里插入图片描述
拉格朗日乘子方程:
L ( x , λ ) = x T A T A x + λ ( 1 − x T x ) L(x,\lambda)=x^TA^TAx+\lambda(1-x^Tx) L(x,λ)=xTATAx+λ(1xTx)
我们分别对 x x x λ \lambda λ求偏导数:
∂ ( x , λ ) ∂ x = 2 A T A x − 2 λ x = 0 \frac{\partial(x,\lambda)}{\partial x}=2A^TAx-2\lambda x=0 x(x,λ)=2ATAx2λx=0 ∂ ( x , λ ) ∂ λ = 1 − x T x = 0 \frac{\partial(x,\lambda)}{\partial\lambda}=1-x^Tx=0 λ(x,λ)=1xTx=0
整理得:
A T A x = λ x ,    x T x = 1 A^TAx=\lambda x,\ \ x^Tx=1 ATAx=λx,  xTx=1
由上式可知: x x x A T A A^TA ATA的特征向量,且模长为1。
A A A进行SVD分解:
A = U Σ V T ,    A T A = V Σ T U T U Σ V T = V Σ T Σ V T A=U\mathrm{\Sigma}V^T,\ \ A^TA=V\mathrm{\Sigma}^TU^TU\mathrm{\Sigma}V^T=VΣ^TΣV^T A=UΣVT,  ATA=VΣTUTUΣVT=VΣTΣVT
可知: V V V A T A A^TA ATA特征向量
∣ ∣ A x ∣ ∣ 2 = x T A T A x = x T λ x = λ x T x = λ ||Ax||^2=x^TA^TAx=x^Tλx=λx^Tx=\lambda Ax2=xTATAx=xTλx=λxTx=λ
从而可知,当 x x x值为 A T A A^TA ATA的特征向量, ∣ ∣ A x ∣ ∣ 2 ||Ax||^2 Ax2的值为对应的特征值。
因此 ∣ ∣ A x ∣ ∣ 2 = λ m i n ||Ax||^2= λ_{min} Ax2=λmin,问题的解 x x x值为 λ m i n \lambda_{min} λmin对应的 A T A A^TA ATA的特征向量。

在使用Eigen等库调用SVD接口时,一般会将奇异值按照从大到小的顺序排列,因此问题的解 x x x V V V的最后一列。解的有效性条件,对应的 λ m i n \lambda_{min} λmin非常接近于0
1)直接对A进行SVD分解,奇异值最小对应的V的列为解值。
2)对A^TA进行SVD分解(相当于三角化),最小特征值对应的特征向量为解值。

代码实现

  auto loop_times = camera_pose.size() - start_frame_id;
  cout << "********* First solution *********" << endl;
  MatrixXd A((loop_times) * 2, 4);
  for (int j = start_frame_id; j < loop_times; ++j) {
    MatrixXd T_tmp(3, 4);
    T_tmp.block<3, 3>(0, 0) = camera_pose[j].Rwc.transpose();
    T_tmp.block<3, 1>(0, 3) = -camera_pose[j].Rwc.transpose() * camera_pose[j].twc;

    auto P_k1 = T_tmp.block<1, 4>(0, 0);
    auto P_k2 = T_tmp.block<1, 4>(1, 0);
    auto P_k3 = T_tmp.block<1, 4>(2, 0);
 
    A.block<1, 4>(2 * j, 0) = camera_pose[j].uv[0] * P_k3 - P_k1;
    A.block<1, 4>(2 * j + 1, 0) = camera_pose[j].uv[1] * P_k3 - P_k2;
  }

  //solution1: SVD1
  Matrix4d ATA = A.transpose() * A;
  //对ATA进行SVD
  JacobiSVD<Matrix4d> svd(ATA, ComputeFullU | ComputeFullV);
  auto res_U = svd.matrixU();
  auto res_V = svd.matrixV();
  cout << "U=" << res_U << endl;
  cout << "V=" << res_V << endl;
  auto tmp = res_U.rightCols(1);
  //第三项为1,归一化为标准坐标(x,y,z,1)
  cout << "First result = " << endl << tmp / tmp(3) << endl;

  //solution1: 对A进行SVD
  JacobiSVD<MatrixXd> svd2(A, ComputeFullU | ComputeFullV);
  auto res_U2 = svd2.matrixU();
  auto res_V2 = svd2.matrixV();
  //cout << "U=" << res_U2 << endl;
  cout << "V=" << res_V2 << endl;
  auto tmp2 = res_V2.rightCols(1);
  //第三项为1,归一化为标准坐标(x,y,z,1)
  cout << "Second result = " << endl << tmp2 / tmp2(3) << endl;

代码测试(地图观测次数对点的坐标估计的准确性的影响)
tri2.cpp加入高斯噪声

#include 
#include 
#include 
#include 
#include 

using namespace Eigen;
using namespace std;

double rx=0; double ry=0; double rz=0;
double tx=0; double ty=0; double tz=0;
default_random_engine poseRand(time(NULL));
//poseRand.seed(time(NULL));
normal_distribution<double> rxr(0.2, 1);
normal_distribution<double> ryr(0.1, 1);
normal_distribution<double> rzr(0.2, 1);
normal_distribution<double> txr(0.1, 1);
normal_distribution<double> tyr(0.2, 1);
normal_distribution<double> tzr(0.1, 1);

int main()
{
  default_random_engine pointRand;
  uniform_real_distribution<double> xy_rand(-4, 4.0);
  uniform_real_distribution<double> z_rand(8., 10.);
  double x = xy_rand(pointRand);
  double y = xy_rand(pointRand);
  double z = z_rand(pointRand);
 
  Vector3d pointW(x, y, z);
  cout << "Ground truth:" << pointW.transpose() << endl;

  int irNum = 7;
  int poseNum[irNum] = {2, 10, 20, 100, 200, 500, 1000};

  for (int ir=0; ir < irNum; ir++) {
    Eigen::MatrixXd mapPointA(2*poseNum[ir],4);
    Eigen::MatrixXd t(1, 4);
    t << -1*100, 0, pointW[0]/pointW[2]*100, 0;
    mapPointA.block<1, 4>(0, 0) = t;

    t << 0, -1*100, pointW[1]/pointW[2]*100, 0;
    mapPointA.block<1, 4>(1, 0) = t;

    for (int i = 1; i < poseNum[ir]; i++) {
      rx += rxr(poseRand);
      ry += ryr(poseRand);
      rz += rzr(poseRand);
      tx += txr(poseRand);
      ty += tyr(poseRand);
      tz += tzr(poseRand);

      double srx = sin(rx);
      double crx = cos(rx);
      double sry = sin(ry);
      double cry = cos(ry);
      double srz = sin(rz);
      double crz = cos(rz);

      normal_distribution<double> pr(0, 0.2);

      Vector3d point;
      //point[2] = crx*sry*pointW[0] - srx*pointW[1] + crx*cry*pointW[2] - tz;
      //point[0] = (crz*cry + srz*srx*sry)*pointW[0] + srz*crx*pointW[1] + (-crz*sry + srz*srx*cry)*pointW[2] - tx;
      //point[1] = (-srz*cry + crz*srx*sry)*pointW[0] + crz*crx*pointW[1] + (srz*sry + crz*srx*cry)*pointW[2] - ty;
      point[2] = crx*sry*pointW[0] - srx*pointW[1] + crx*cry*pointW[2] - tz + pr(poseRand);
      point[0] = (crz*cry + srz*srx*sry)*pointW[0] + srz*crx*pointW[1] + (-crz*sry + srz*srx*cry)*pointW[2] - tx + pr(poseRand);
      point[1] = (-srz*cry + crz*srx*sry)*pointW[0] + crz*crx*pointW[1] + (srz*sry + crz*srx*cry)*pointW[2] - ty + pr(poseRand);

      t << point[0]/point[2]*crx*sry - (crz*cry + srz*srx*sry),
          -point[0]/point[2]*srx - srz*crx,
           point[0]/point[2]*crx*cry - (-crz*sry + srz*srx*cry),
          -point[0]/point[2]*tz + tx; 
      mapPointA.block<1, 4>(2*i, 0) = t;
      
      t << point[1]/point[2]*crx*sry - (-srz*cry + crz*srx*sry),
          -point[1]/point[2]*srx - crz*crx,
           point[1]/point[2]*crx*cry - (srz*sry + crz*srx*cry),
          -point[1]/point[2]*tz + ty; 
      mapPointA.block<1, 4>(2*i+1, 0) = t;

    }

    Eigen::Matrix4d ATA = mapPointA.transpose()*mapPointA;
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(ATA,
                Eigen::ComputeFullU|Eigen::ComputeFullV);
    auto res_U = svd.matrixU();
    auto lambda = svd.singularValues();

    Vector3d pointE(x, y, z);
    //if (lambda[-1] < 0.5 && lambda[-1] != 0) {
      auto tmp = res_U.rightCols(1);
      pointE[0] = double(tmp(0) / tmp(3));
      pointE[1] = double(tmp(1) / tmp(3));
      pointE[2] = double(tmp(2) / tmp(3));
    //}

    cout << "point estimate: " << pointE.transpose() << endl;
    cout << "pose Num is: " << poseNum[ir] << endl;
    cout << "estimate error rmse: " << sqrt(pow(pointE[0]-pointW[0],2) + pow(pointE[1]-pointW[1],2) + pow(pointE[2]-pointW[2],2)) << endl;
    cout << "************iret: " << ir << " end*****************" << endl;
  }
}

编译tri2.cpp

g++ tri2.cpp `pkg-config eigen3 --libs --cflags`

运行:

./a.out

运行结果:
三角化特征点(triangulation)方法及实现对比_第1张图片

2 Cramer’s(克莱默)法则求解Ax=b问题

求解原理

根据公式推导如下:
d r e f ⋅ p r e f = d c u r ⋅ ( R r c ⋅ p c u r ) + t r c d_{ref}\cdot p_{ref}=d_{cur}\cdot\left(R_{rc}\cdot p_{cur}\right)+t_{rc} drefpref=dcur(Rrcpcur)+trc
将其化为非齐次形式 A x = b Ax=b Ax=b
对公式两侧同时乘以 p r e f T p_{ref}^T prefT,得到:
d r e f ⋅ p r e f T ⋅ p r e f − d c u r ⋅ p r e f T ⋅ ( R r c ⋅ p c u r ) = p r e f T ⋅ t r c d_{ref}\cdot p_{ref}^T\cdot p_{ref}-d_{cur}\cdot p_{ref}^T\cdot\left(R_{rc}\cdot p_{cur}\right)=p_{ref}^T\cdot t_{rc} drefprefTprefdcurprefT(Rrcpcur)=prefTtrc
公式两侧同时乘以 ( R r c p c u r ) T \left(R_{rc}p_{cur}\right)^T (Rrcpcur)T,得到:
d r e f ⋅ ( R r c p c u r ) T ⋅ p r e f − d c u r ⋅ ( R r c p c u r ) T ⋅ ( R r c ⋅ p c u r ) = ( R r c p c u r ) T ⋅ t r c d_{ref}\cdot\left(R_{rc}p_{cur}\right)^T\cdot p_{ref}-d_{cur}\cdot\left(R_{rc}p_{cur}\right)^T\cdot\left(R_{rc}\cdot p_{cur}\right)=\left(R_{rc}p_{cur}\right)^T\cdot t_{rc} dref(Rrcpcur)Tprefdcur(Rrcpcur)T(Rrcpcur)=(Rrcpcur)Ttrc
化为非齐次形式,如下:
[ p r e f T ⋅ p r e f − p r e f T ⋅ R r c ⋅ p c u r p c u r T R r c T ⋅ p r e f − p c u r T ⋅ p c u r ] ⋅ [ d r e f d c u r ] = [ p r e f T ⋅ t r c ( R r c p c u r ) T ⋅ t r c ] \left[\begin{matrix}p_{ref}^T\cdot p_{ref}&-p_{ref}^T\cdot R_{rc}\cdot p_{cur}\\p_{cur}^TR_{rc}^T\cdot p_{ref}&-p_{cur}^T\cdot p_{cur}\\\end{matrix}\right]\cdot\left[\begin{matrix}\begin{matrix}d_{ref}\\d_{cur}\\\end{matrix}\\\end{matrix}\right]=\left[\begin{matrix}\begin{matrix}p_{ref}^T\cdot t_{rc}\\\left(R_{rc}p_{cur}\right)^T\cdot t_{rc}\\\end{matrix}\\\end{matrix}\right] [prefTprefpcurTRrcTprefprefTRrcpcurpcurTpcur][drefdcur]=[prefTtrc(Rrcpcur)Ttrc]
对于上述两个公式组成的方程组,利用克莱默法则求解。克莱姆法则是一种求解线性方程组的方法,大多数线性代数教材都会提到。例如对于如下的线性方程组:
在这里插入图片描述
对于这样的方程,如果 ∣ A ∣ |A| A不为0,方程可以通过如下方式求解。
运用克莱姆法则,这个方程组的解可以如下:
在这里插入图片描述
其中 D , D x , D y D,D_x,D_y D,Dx,Dy,分别是如下三个行列式:
在这里插入图片描述

实现代码

  cout << "********* Second solution *********" << endl;
  for (int j = start_frame_id + 1; j < loop_times; ++j) {
    // dr * pr = dc * ( Rrc * pc ) + trc
    // => [ pr^T*pr,   -pr^T*R*pc ] [dr] = [pr^T*t]
    //    [ pc^T*R^T*pr, -pc^T*pc ] [dc] = [pc^T*R^T*t]
    Matrix3d Rrc = camera_pose[j-1].Rwc.transpose() * camera_pose[j].Rwc;
    Vector3d trc = camera_pose[j-1].Rwc.transpose() * (camera_pose[j].twc - camera_pose[j-1].twc);
    Vector3d pr(camera_pose[j-1].uv[0], camera_pose[j-1].uv[1], 1);
    Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);

    Vector3d f2 = Rrc * pc;
    double A[4];
    A[0] = pr.dot( pr );
    A[2] = pr.dot( f2 );
    A[1] = -A[2];
    A[3] = -f2.dot( f2 );

    Vector2d b = Vector2d (trc.dot(pr), trc.dot(f2));

    // 此处计算A的行列式
    double d = A[0]*A[3] - A[1]*A[2];
    Vector2d lambdavec =
        Vector2d ( A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ),
                    -A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;
    /*Vector3d */pr = lambdavec ( 0,0 ) * pr;
    /*Vector3d */pc = trc + lambdavec ( 1,0 ) * f2;

    pr = camera_pose[j-1].Rwc * pr + camera_pose[j-1].twc;
    pc = camera_pose[j-1].Rwc * pc + camera_pose[j-1].twc;
    cout << "ir:  " << j << " result1: " << pr << endl;
    cout << "ir:  " << j << " result2: " << pc << endl;

    Vector3d d_esti = ( pr+pc ) / 2.0;  // 三角化算得的深度向量
    //double depth_estimation = d_esti.norm();   // 深度值

    cout << "ir:  " << j << " final result: " << d_esti << endl;
  }

3 单一化未知量求解

求解原理

根据公式
d r p r = d c R r c p c + t r c d_rp_r=d_cR_{rc}p_c+t_{rc} drpr=dcRrcpc+trc
左右两侧都乘以 p r × p_r^\times pr×, p r × p_r^\times pr×为p_r的反对称矩阵,得:
d r p r × p r = d c p r × R r c p c + p r × t r c = 0 d_r{p_r^\times p}_r=d_c{p_r^\times R}_{rc}p_c+p_r^\times t_{rc}=0 drpr×pr=dcpr×Rrcpc+pr×trc=0
从而方程中只剩 d c d_c dc一个未知量,求解 d c d_c dc,从而得该点的三维坐标。

实现代码

  //solution3: drpr = dc*R*pc+t => dc*pr^*R*pc + pr^ * t = 0
  //dr*pr = dc*Rrc*pc + trc
  cout << "********* Third solution *********" << endl;
  for (int j = start_frame_id + 1; j < loop_times; ++j) {
    Matrix3d Rrc = camera_pose[j-1].Rwc.transpose() * camera_pose[j].Rwc;
    Vector3d trc = camera_pose[j-1].Rwc.transpose() * (camera_pose[j].twc - camera_pose[j-1].twc);
    Vector3d pr(camera_pose[j-1].uv[0], camera_pose[j-1].uv[1], 1);
    Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);

    Vector3d f1 = pr.cross(Rrc*pc);
    Vector3d f2 = -pr.cross(trc);

    Vector3d dc(f2[0]/f1[0], f2[1]/f1[1], f2[2]/f1[2]);
    cout << "dc: " << dc << endl;

    double dc_ave = (dc[0] + dc[1] + dc[2])/3;
    Vector3d xc = dc_ave * pc;

    xc = camera_pose[j].Rwc * xc + camera_pose[j].twc;
    cout << "ir:  " << j << " result: " << xc << endl;
}

4 A x = b Ax=b Ax=b求解, x = ( A T A ) − 1 A T b x=\left(A^TA\right)^{-1}A^Tb x=(ATA)1ATb

求解原理

根据公式
d r p r = d c R r c p c + t r c d_rp_r=d_cR_{rc}p_c+t_{rc} drpr=dcRrcpc+trc
写成矩阵形式:
[ p r − R r c p c ] [ d r d c ] = t r c → A d = b \left[\begin{matrix}p_r&-R_{rc}p_c\\\end{matrix}\right]\left[\begin{matrix}d_r\\d_c\\\end{matrix}\right]=t_{rc}\rightarrow Ad=b [prRrcpc][drdc]=trcAd=b
解:
d = ( A T A ) − 1 A T b d=\left(A^TA\right)^{-1}A^Tb d=(ATA)1ATb

代码实现

  cout << "********* Forth solution *********" << endl;
  for (int j = start_frame_id + 1; j < loop_times; ++j) {
    Matrix3d Rrc = camera_pose[j-1].Rwc.transpose() * camera_pose[j].Rwc;
    Vector3d trc = camera_pose[j-1].Rwc.transpose() * (camera_pose[j].twc - camera_pose[j-1].twc);
    Vector3d pr(camera_pose[j-1].uv[0], camera_pose[j-1].uv[1], 1);
    Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);

    MatrixXd A(3, 2);
    A.block<3, 1>(0, 0) = pr;
    A.block<3, 1>(0, 1) = -Rrc * pc;

    Vector2d d = (A.transpose() * A).inverse() * A.transpose() * trc;

    /* Vector3d */pr = d[0] * pr;
    /* Vector3d */pc = d[1] * pc;

    pr = camera_pose[j-1].Rwc * pr + camera_pose[j-1].twc;
    pc = camera_pose[j].Rwc * pc + camera_pose[j].twc;
    cout << "ir:  " << j << " result1: " << pr << endl;
    cout << "ir:  " << j << " result2: " << pc << endl;
}

5 DEMO算法的几何计算深度

求解原理

利用平行关系:
三角化特征点(triangulation)方法及实现对比_第2张图片
[u0,v0], [u1,v1]均为归一化平面坐标

作者将last相机位置沿O_Last->X方向移动tz到O_Last’,从而将O_Last移动到与O_Cur z方向平行的方向,此时, t x ′ = t x − t z ∗ u 0 , t y ′ = t y − t z ∗ v 0 tx'=tx-tz*u0,ty'=ty-tz*v0 tx=txtzu0ty=tytzv0

在O_Last‘作与O_Cur->X平行的辅助线,此时,[u0,v0], [u1,v1], X, O_Last, O_Cur, O_Last’都在同一平面上。
[u1,v1]->[u0,v0]平行于O_Last‘->O_Cur。
A B C O L ′ = O L ′ O C X D → A B C O L ′ c o s θ = O L ′ O C X D c o s θ \frac{AB}{CO_L^\prime}=\frac{O_L^\prime O_C}{XD}\rightarrow\frac{AB}{CO_L^\prime c o s\theta}=\frac{O_L^\prime O_C}{XDcos\theta} COLAB=XDOLOCCOLcosθAB=XDcosθOLOC
( u 1 − u 0 ) 2 + ( v 1 − v 0 ) 2 1 = ( t z u 0 − t x ) 2 + ( t z v 0 − t y ) 2 Z \frac{\sqrt{\left(u_1-u_0\right)^2+\left(v_1-v_0\right)^2}}{1}=\frac{\sqrt{\left({t_zu}_0-t_x\right)^2+\left({t_zv}_0-t_y\right)^2}}{Z} 1(u1u0)2+(v1v0)2 =Z(tzu0tx)2+(tzv0ty)2
Z = ( t z u 0 − t x ) 2 + ( t z v 0 − t y ) 2 ( u 1 − u 0 ) 2 + ( v 1 − v 0 ) 2 Z=\frac{\sqrt{\left({t_zu}_0-t_x\right)^2+\left({t_zv}_0-t_y\right)^2}}{\sqrt{\left(u_1-u_0\right)^2+\left(v_1-v_0\right)^2}} Z=(u1u0)2+(v1v0)2 (tzu0tx)2+(tzv0ty)2

代码中为什么除以cos(atan2(tz * v1 - ty, tz * u1 - tx) - atan2(v0 - v1, u0 - u1))未知。

代码实现

  cout << "********* Fifth solution *********" << endl;
  int ind_rec = start_frame_id;
  for (int j = start_frame_id; j < loop_times; ++j) {
    Vector3d dt = camera_pose[j].twc - camera_pose[ind_rec].twc;
    double dis = sqrt(pow(dt[0], 2) + pow(dt[1], 2) + pow(dt[2], 2));
    if (dis > 0.3) {
      Matrix3d Rrc = camera_pose[ind_rec].Rwc.transpose() * camera_pose[j].Rwc;
      Vector3d trc = camera_pose[ind_rec].Rwc.transpose()
                   * (camera_pose[j].twc - camera_pose[ind_rec].twc);
      Vector3d pr(camera_pose[ind_rec].uv[0], camera_pose[ind_rec].uv[1], 1);
      Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);
      //将pc的坐标系旋转到与rec坐标系方向一致
      //pcr << pcr[0]/pcr[2], pcr[1]/pcr[2], 1;
      Vector3d pcr = Rrc * pc;

      double u1 = camera_pose[ind_rec].uv[0];
      double v1 = camera_pose[ind_rec].uv[1];
      double u0 = pcr[0]/pcr[2];
      double v0 = pcr[1]/pcr[2];

      double tx = trc[0];
      double ty = trc[1];
      double tz = trc[2];

      double delta = sqrt((v0 - v1) * (v0 - v1) + (u0 - u1) * (u0 - u1)) 
                   * cos(atan2(tz * v1 - ty, tz * u1 - tx) - atan2(v0 - v1, u0 - u1));
      //double delta = sqrt((v0 - v1) * (v0 - v1) + (u0 - u1) * (u0 - u1));
      double dr = sqrt((tz * u0 - tx) * (tz * u0 - tx) + (tz * v0 - ty) * (tz * v0 - ty)) / delta;
      pr = dr * pr;

      pr = camera_pose[ind_rec].Rwc * pr + camera_pose[ind_rec].twc;
      cout << "ir:  " << j << " result: " << pr << endl;

      ind_rec = j;
    }
}

6 Tips

可三角化求解条件,两帧之间有足够的漂移运动。

点云深度的恢复,需要两帧相机之间有足够的运动漂移,小视差(近似纯旋转)的场景下,尺度不确定,深度解值无限。

设纯旋转: T = [ R 0 ] T=[\begin{matrix}R&0\\\end{matrix}] T=[R0]
图像归一化齐次坐标为 p r p_r pr p c p_c pc(非像素坐标)。
p r = [ u r v r 1 ] ,    p c = [ u c v c 1 ] p_r=\left[\begin{matrix}u_r\\v_r\\1\\\end{matrix}\right],\ \ p_c=\left[\begin{matrix}u_c\\v_c\\1\\\end{matrix}\right] pr=urvr1,  pc=ucvc1
满足:
x r = R r c x c x_r=R_{rc}x_c xr=Rrcxc d r p r = d c R r c p c d_rp_r=d_cR_{rc}p_c drpr=dcRrcpc
d r d_r dr为目标求解对象,从而得到地图点的3D坐标。
从上述公式可知,即使是有多帧观测,但是均是小漂移场景时,设其组成下述方程:
d r p r = d c R r c p c \bm{d}_r\bm{p}_r=\bm{d}_c\bm{R}_{rc}\bm{p}_c drpr=dcRrcpc d r p r = [ d r 1 p r 1 , d r 2 p r 2 , d r 3 p r 3 , ⋯ ] T \bm{d}_r\bm{p}_r=[d_{r1}p_{r1}, d_{r2}p_{r2}, d_{r3}p_{r3},⋯]^T drpr=[dr1pr1,dr2pr2,dr3pr3,]T右侧同理。

此时左右同时乘以任意常数 c c c,仍满足方程,因此解值无限, c d r c\bm{d}_r cdr

即使采用第一种方法来解算:
[ u 1 T 3 1 − T 1 1 v 1 T 3 1 − T 2 1 ⋮ u n T 3 n − T 1 n v n T 3 n − T 2 n ] x w = [ u 1 T 3 , 1 1 − T 1 , 1 1 u 1 T 3 , 2 1 − T 1 , 2 1 v 1 T 3 , 1 1 − T 2 , 1 1 v 1 T 3 , 2 1 − T 2 , 2 1 u 1 T 3 , 3 1 − T 1 , 3 1 u 1 T 3 , 4 1 − T 1 , 4 1 v 1 T 3 , 3 1 − T 2 , 3 1 v 1 T 3 , 4 1 − T 2 , 4 1 ⋮ ⋮ u n T 3 , 1 n − T 1 , 1 n u n T 3 , 2 n − T 1 , 2 n ⋮ ⋮ u n T 3 , 3 n − T 1 , 3 n u n T 3 , 4 n − T 1 , 4 n ] [ x y z f ] = 0 \left[\begin{matrix}u_1T_3^1-T_1^1\\v_1T_3^1-T_2^1\\\begin{matrix}\vdots\\u_nT_3^n-T_1^n\\v_nT_3^n-T_2^n\\\end{matrix}\\\end{matrix}\right]\bm{x}_w=\left[\begin{matrix}\begin{matrix}u_1T_{3,1}^1-T_{1,1}^1&u_1T_{3,2}^1-T_{1,2}^1\\v_1T_{3,1}^1-T_{2,1}^1&v_1T_{3,2}^1-T_{2,2}^1\\\end{matrix}&\begin{matrix}u_1T_{3,3}^1-T_{1,3}^1&u_1T_{3,4}^1-T_{1,4}^1\\v_1T_{3,3}^1-T_{2,3}^1&v_1T_{3,4}^1-T_{2,4}^1\\\end{matrix}\\\begin{matrix}\vdots&\vdots\\u_nT_{3,1}^n-T_{1,1}^n&u_nT_{3,2}^n-T_{1,2}^n\\\end{matrix}&\begin{matrix}\vdots&\vdots\\u_nT_{3,3}^n-T_{1,3}^n&u_nT_{3,4}^n-T_{1,4}^n\\\end{matrix}\\\end{matrix}\right]\left[\begin{matrix}x\\y\\\begin{matrix}z\\f\\\end{matrix}\\\end{matrix}\right]=0 u1T31T11v1T31T21unT3nT1nvnT3nT2nxw=u1T3,11T1,11v1T3,11T2,11u1T3,21T1,21v1T3,21T2,21unT3,1nT1,1nunT3,2nT1,2nu1T3,31T1,31v1T3,31T2,31u1T3,41T1,41v1T3,41T2,41unT3,3nT1,3nunT3,4nT1,4nxyzf=0

T = [ R 0 ] T=[\begin{matrix}R&0\\\end{matrix}] T=[R0]时,上述方程为: A A A阵的最后一列为0。
[ u 1 T 3 , 1 1 − T 1 , 1 1 u 1 T 3 , 2 1 − T 1 , 2 1 v 1 T 3 , 1 1 − T 2 , 1 1 v 1 T 3 , 2 1 − T 2 , 2 1 u 1 T 3 , 3 1 − T 1 , 3 1 0 v 1 T 3 , 3 1 − T 2 , 3 1 0 ⋮ ⋮ u n T 3 , 1 n − T 1 , 1 n u n T 3 , 2 n − T 1 , 2 n ⋮ ⋮ u n T 3 , 3 n − T 1 , 3 n 0 ] [ x y z f ] = 0 \left[\begin{matrix}\begin{matrix}u_1T_{3,1}^1-T_{1,1}^1&u_1T_{3,2}^1-T_{1,2}^1\\v_1T_{3,1}^1-T_{2,1}^1&v_1T_{3,2}^1-T_{2,2}^1\\\end{matrix}&\begin{matrix}u_1T_{3,3}^1-T_{1,3}^1&0\\v_1T_{3,3}^1-T_{2,3}^1&0\\\end{matrix}\\\begin{matrix}\vdots&\vdots\\u_nT_{3,1}^n-T_{1,1}^n&u_nT_{3,2}^n-T_{1,2}^n\\\end{matrix}&\begin{matrix}\vdots&\vdots\\u_nT_{3,3}^n-T_{1,3}^n&0\\\end{matrix}\\\end{matrix}\right]\left[\begin{matrix}x\\y\\\begin{matrix}z\\f\\\end{matrix}\\\end{matrix}\right]=0 u1T3,11T1,11v1T3,11T2,11u1T3,21T1,21v1T3,21T2,21unT3,1nT1,1nunT3,2nT1,2nu1T3,31T1,31v1T3,31T2,3100unT3,3nT1,3n0xyzf=0 s . t .    ∣ ∣ x ∣ ∣ 2 = 1 s.t.\ \ ||x||^2=1 s.t.  x2=1
此时上述方程的解无限,f为值不影响结果。从而可知尺度不确定。

7 完整代码

  1 #include <iostream>
  2 #include <vector>
  3 #include <random>
  4 #include <Eigen/Core>
  5 #include <Eigen/Geometry>
  6 #include <Eigen/Eigenvalues>
  7 
  8 using namespace Eigen;
  9 using namespace std;
 10 
 11 struct Pose {
 12   Pose(Matrix3d R, Vector3d t) : Rwc(R), qwc(R), twc(t) {};
 13 
 14   Matrix3d Rwc;
 15   Quaterniond qwc;
 16   Vector3d twc;
 17 
 18   Vector2d uv;
 19 };
 20 
 21 int main() {
 22   int poseNums = 10;
 23   double radius = 8;
 24   double fx = 1.;
 25   double fy = 1.;
 26 
 27   vector<Pose> camera_pose;
 28   for (int n = 0; n < poseNums; ++n) {
 29     double theta = n * 2 * M_PI / (poseNums * 4); // 1/4 ??
 30     Matrix3d R;
 31     R = AngleAxisd(theta, Vector3d::UnitZ());
 32     Vector3d t = Vector3d(radius * cos(theta) - radius,
 33        radius * sin(theta), 1 * sin(2 * theta));
 34     camera_pose.push_back(Pose(R, t));
 35   }
 36 
 37   default_random_engine generator;
 38   uniform_real_distribution<double> xy_rand(-4, 4.0);
 39   uniform_real_distribution<double> z_rand(8., 10.);
 40   double tx = xy_rand(generator);
 41   double ty = xy_rand(generator);
 42   double tz = z_rand(generator);
 43 
 44   Vector3d Pw(tx, ty, tz);
 45   cout << "Ground truth:" << Pw.transpose() << endl;
 46 
 47   int start_frame_id = 3;
 48   int end_frame_id = poseNums;
 49   //default_random_engine generator;
 50   for (int i = start_frame_id; i < end_frame_id; ++i) {
 51     Matrix3d Rcw = camera_pose[i].Rwc.transpose();
 52     Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);
 53     //normal_distribution randx(0, 0.01);
 56     //normal_distribution randy(0, 0.01);
 57     //normal_distribution randz(0, 0.01);
 58 
 54     double x = Pc.x();
 55     double y = Pc.y();
 56     double z = Pc.z();
 57 
 58     camera_pose[i].uv = Vector2d(x / z, y / z);
 59   }
 60 
 61   //solution1: SVD
 62   Vector3d P_est;
 63   P_est.setZero();
 64   /* your code begin */
 65   auto loop_times = camera_pose.size() - start_frame_id;
 66   MatrixXd A((loop_times) * 2, 4);
 67   for (int j = start_frame_id; j < loop_times; ++j) {
 68     MatrixXd T_tmp(3, 4);
 69     T_tmp.block<3, 3>(0, 0) = camera_pose[j].Rwc.transpose();
 70     T_tmp.block<3, 1>(0, 3) = -camera_pose[j].Rwc.transpose() * camera_pose[j].twc;
 71 
 72     auto P_k1 = T_tmp.block<1, 4>(0, 0);
 73     auto P_k2 = T_tmp.block<1, 4>(1, 0);
 74     auto P_k3 = T_tmp.block<1, 4>(2, 0);
 75 
 76     A.block<1, 4>(2 * j, 0) = camera_pose[j].uv[0] * P_k3 - P_k1;
 77     A.block<1, 4>(2 * j + 1, 0) = camera_pose[j].uv[1] * P_k3 - P_k2;
 78   }
 79 
 80   //solution1: SVD1
 81   cout << "********* First solution *********" << endl;
 82   //对ATA进行SVD
 83   Matrix4d ATA = A.transpose() * A;
 84   JacobiSVD<Matrix4d> svd(ATA, ComputeFullU | ComputeFullV);
 85   auto res_U = svd.matrixU();
 86   auto res_V = svd.matrixV();
 87   cout << "U=" << res_U << endl;
 88   cout << "V=" << res_V << endl;
 89   auto tmp = res_U.rightCols(1);
 90   //第三项为1,归一化为标准坐标(x,y,z,1)
 91   cout << "First result = " << endl << tmp / tmp(3) << endl;
 92 
 93   //solution1: SVD2
 94   //对A进行SVD
 95   JacobiSVD<MatrixXd> svd2(A, ComputeFullU | ComputeFullV);
 96   auto res_U2 = svd2.matrixU();
 97   auto res_V2 = svd2.matrixV();
 98   //cout << "U=" << res_U2 << endl;
 99   cout << "V=" << res_V2 << endl;
100   auto tmp2 = res_V2.rightCols(1);
101   //第三项为1,归一化为标准坐标(x,y,z,1)
102   cout << "Second result = " << endl << tmp2 / tmp2(3) << endl;
103 
104   //solution2: Cramer's
105   cout << "********* Second solution *********" << endl;
106   for (int j = start_frame_id + 1; j < loop_times; ++j) {
107     // dr * pr = dc * ( Rrc * pc ) + trc
108     // => [ pr^T*pr,   -pr^T*R*pc ] [dr] = [pr^T*t]
109     //    [ pc^T*R^T*pr, -pc^T*pc ] [dc] = [pc^T*R^T*t]
110     Matrix3d Rrc = camera_pose[j-1].Rwc.transpose() * camera_pose[j].Rwc;
111     Vector3d trc = camera_pose[j-1].Rwc.transpose() * (camera_pose[j].twc - camera_pose[j-1].twc)    ;
112     Vector3d pr(camera_pose[j-1].uv[0], camera_pose[j-1].uv[1], 1);
113     Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);
114 
115     Vector3d f2 = Rrc * pc;
116     double A[4];
117     A[0] = pr.dot( pr );
118     A[2] = pr.dot( f2 );
119     A[1] = -A[2];
120     A[3] = -f2.dot( f2 );
121 
122     Vector2d b = Vector2d (trc.dot(pr), trc.dot(f2));
123 
124     // 此处计算A的行列式
125     double d = A[0]*A[3] - A[1]*A[2];
126     Vector2d lambdavec =
127         Vector2d ( A[3] * b ( 0,0 ) - A[1] * b ( 1,0 ),
128                     -A[2] * b ( 0,0 ) + A[0] * b ( 1,0 )) /d;
129     /*Vector3d */pr = lambdavec ( 0,0 ) * pr;
130     /*Vector3d */pc = trc + lambdavec ( 1,0 ) * f2;
131 
132     pr = camera_pose[j-1].Rwc * pr + camera_pose[j-1].twc;
133     pc = camera_pose[j-1].Rwc * pc + camera_pose[j-1].twc;
134     cout << "ir:  " << j << " result1: " << pr << endl;
135     cout << "ir:  " << j << " result2: " << pc << endl;
136 
137     Vector3d d_esti = ( pr+pc ) / 2.0;  // 三角化算得的深度向量
138     //double depth_estimation = d_esti.norm();   // 深度值
139 
140     cout << "ir:  " << j << " final result: " << d_esti << endl;
141   }
142 
143   //solution3: drpr = dc*R*pc+t => dc*pr^*R*pc + pr^ * t = 0
144   //dr*pr = dc*Rrc*pc + trc
145   cout << "********* Third solution *********" << endl;
146   for (int j = start_frame_id + 1; j < loop_times; ++j) {
147     Matrix3d Rrc = camera_pose[j-1].Rwc.transpose() * camera_pose[j].Rwc;
148     Vector3d trc = camera_pose[j-1].Rwc.transpose() * (camera_pose[j].twc - camera_pose[j-1].twc)    ;
149     Vector3d pr(camera_pose[j-1].uv[0], camera_pose[j-1].uv[1], 1);
150     Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);
151 
152     Vector3d f1 = pr.cross(Rrc*pc);
153     Vector3d f2 = -pr.cross(trc);
154 
155     Vector3d dc(f2[0]/f1[0], f2[1]/f1[1], f2[2]/f1[2]);
156     cout << "dc: " << dc << endl;
157 
158     double dc_ave = (dc[0] + dc[1] + dc[2])/3;
159     Vector3d xc = dc_ave * pc;
160 
161     xc = camera_pose[j].Rwc * xc + camera_pose[j].twc;
162     cout << "ir:  " << j << " result: " << xc << endl;
163   }
164 
165   //solution4: (ATA)^-1*ATb
166   //dr*pr = dc*Rrc*pc + trc
167   // A = [pr -Rrc*pc] b = [trc], d = [dr, dc]
168   // Ad = b
169   cout << "********* Forth solution *********" << endl;
170   for (int j = start_frame_id + 1; j < loop_times; ++j) {
171     Matrix3d Rrc = camera_pose[j-1].Rwc.transpose() * camera_pose[j].Rwc;
172     Vector3d trc = camera_pose[j-1].Rwc.transpose() * (camera_pose[j].twc - camera_pose[j-1].twc)    ;
173     Vector3d pr(camera_pose[j-1].uv[0], camera_pose[j-1].uv[1], 1);
174     Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);
175 
176     MatrixXd A(3, 2);
177     A.block<3, 1>(0, 0) = pr;
178     A.block<3, 1>(0, 1) = -Rrc * pc;
179 
180     Vector2d d = (A.transpose() * A).inverse() * A.transpose() * trc;
181 
182     /* Vector3d */pr = d[0] * pr;
183     /* Vector3d */pc = d[1] * pc;
184 
185     pr = camera_pose[j-1].Rwc * pr + camera_pose[j-1].twc;
186     pc = camera_pose[j].Rwc * pc + camera_pose[j].twc;
187     cout << "ir:  " << j << " result1: " << pr << endl;
188     cout << "ir:  " << j << " result2: " << pc << endl;
189   }
190 
191   //solution5: demo 三角化几何计算深度
192   //dr*pr = dc*Rrc*pc + trc
193   //r:1 c:0
194   cout << "********* Fifth solution *********" << endl;
195   int ind_rec = start_frame_id;
196   for (int j = start_frame_id; j < loop_times; ++j) {
197     Vector3d dt = camera_pose[j].twc - camera_pose[ind_rec].twc;
198     double dis = sqrt(pow(dt[0], 2) + pow(dt[1], 2) + pow(dt[2], 2));
199     if (dis > 0.3) {
200       Matrix3d Rrc = camera_pose[ind_rec].Rwc.transpose() * camera_pose[j].Rwc;
201       Vector3d trc = camera_pose[ind_rec].Rwc.transpose()
202                    * (camera_pose[j].twc - camera_pose[ind_rec].twc);
203       Vector3d pr(camera_pose[ind_rec].uv[0], camera_pose[ind_rec].uv[1], 1);
204       Vector3d pc(camera_pose[j].uv[0], camera_pose[j].uv[1], 1);
205       //将pc的坐标系旋转到与rec坐标系方向一致
206       //pcr << pcr[0]/pcr[2], pcr[1]/pcr[2], 1;
207       Vector3d pcr = Rrc * pc;
208 
209       double u1 = camera_pose[ind_rec].uv[0];
210       double v1 = camera_pose[ind_rec].uv[1];
211       double u0 = pcr[0]/pcr[2];
212       double v0 = pcr[1]/pcr[2];
213 
214       double tx = trc[0];
215       double ty = trc[1];
216       double tz = trc[2];
217 
218       //double delta = sqrt((v0 - v1) * (v0 - v1) + (u0 - u1) * (u0 - u1)) 
219       //             * cos(atan2(tz * v1 - ty, tz * u1 - tx) - atan2(v0 - v1, u0 - u1));
220       double delta = sqrt((v0 - v1) * (v0 - v1) + (u0 - u1) * (u0 - u1));
221       double dr = sqrt((tz * u0 - tx) * (tz * u0 - tx) + (tz * v0 - ty) * (tz * v0 - ty)) / delta    ;
222       pr = dr * pr;
223 
224       pr = camera_pose[ind_rec].Rwc * pr + camera_pose[ind_rec].twc;
225       cout << "ir:  " << j << " result: " << pr << endl;
226 
227       ind_rec = j;
228     }
229   }
230 
231   return 0;
232 }

编译:

g++ triangulation.cpp `pkg-config eigen3 --libs --cflags` -o test

运行:

./test

结果:
三角化特征点(triangulation)方法及实现对比_第3张图片

9 其他

已知多次地图点的三维观测值,求其最佳坐标
地图点坐标优化:
一个地图点,被多个相机观测到或者在不同位姿下有多个3D观测值(深度相机/雷达),通过该计算该三维位姿的最佳三维估计。
问题:
m i n x W ∑ i = 0 N ∣ ∣ x W − T i x C ∣ ∣ 2 \underset{x_W} {min}{\sum_{i=0}^{N}||{x_W-T_ix_C}}||^2 xWmini=0NxWTixC2
通过求导可知,上述问题的极小值就为多次观测的平均值:
x W = ∑ i = 0 N T i x C x_W=\sum_{i=0}^{N}{T_ix_C} xW=i=0NTixC

tri2_ave.cpp

#include 
#include 
#include 
#include 
#include 

using namespace Eigen;
using namespace std;

double rx=0; double ry=0; double rz=0;
double tx=0; double ty=0; double tz=0;
default_random_engine poseRand(time(NULL));
//poseRand.seed(time(NULL));
normal_distribution<double> rxr(0.2, 1);
normal_distribution<double> ryr(0.1, 1);
normal_distribution<double> rzr(0.2, 1);
normal_distribution<double> txr(0.1, 1);
normal_distribution<double> tyr(0.2, 1);
normal_distribution<double> tzr(0.1, 1);

int main()
{
  default_random_engine pointRand;
  uniform_real_distribution<double> xy_rand(-4, 4.0);
  uniform_real_distribution<double> z_rand(8., 10.);
  double x = xy_rand(pointRand);
  double y = xy_rand(pointRand);
  double z = z_rand(pointRand);
 
  Vector3d pointW(x, y, z);
  Vector3d pointE(x, y, z);
  cout << "Ground truth:" << pointW.transpose() << endl;

  int irNum = 7;
  int poseNum[irNum] = {2, 10, 20, 100, 200, 500, 1000};

  double xnAve = 0; double ynAve = 0; double znAve = 0;

  for (int ir=0; ir < irNum; ir++) {

    // pw = RT * pc + t
    // pc = R * (pw - t)
    for (int i = 1; i < poseNum[ir]; i++) {
      rx += rxr(poseRand);
      ry += ryr(poseRand);
      rz += rzr(poseRand);
      tx += txr(poseRand);
      ty += tyr(poseRand);
      tz += tzr(poseRand);

      double srx = sin(rx);
      double crx = cos(rx);
      double sry = sin(ry);
      double cry = cos(ry);
      double srz = sin(rz);
      double crz = cos(rz);

      normal_distribution<double> pr(0, 0.2);

      Vector3d point;
      //point[2] = crx*sry*(pointW[0]-tx) - srx*(pointW[1]-ty) + crx*cry*(pointW[2]-tz);
      //point[0] = (crz*cry + srz*srx*sry)*pointW[0] + srz*crx*pointW[1] + (-crz*sry + srz*srx*cr    y)*pointW[2];
      //point[1] = (-srz*cry + crz*srx*sry)*pointW[0] + crz*crx*pointW[1] + (srz*sry + crz*srx*cr    y)*pointW[2];

      double xn = pr(poseRand);
      double yn = pr(poseRand);
      double zn = pr(poseRand);
      point[2] = crx*sry*(pointW[0]-tx) - srx*(pointW[1]-ty) + crx*cry*(pointW[2]-tz) + xn;
      point[0] = (crz*cry + srz*srx*sry)*(pointW[0]-tx) + srz*crx*(pointW[1]-ty) + (-crz*sry + srz*srx*cry)*(pointW[2]-tz) + yn;
      point[1] = (-srz*cry + crz*srx*sry)*(pointW[0]-tx) + crz*crx*(pointW[1]-ty) + (srz*sry + crz*srx*cry)*(pointW[2]-tz) + xn;

      xnAve += xn;
      ynAve += yn;
      znAve += zn;

      double x1 = crz * point[0] - srz * point[1];
      double y1 = srz * point[0] + crz * point[1];
      double z1 = point[2];
 
      double x2 = x1;
      double y2 = crx * y1 - srx * z1;
      double z2 = srx * y1 + crx * z1;
 
      pointE[0] += cry * x2 + sry * z2 + tx;
      pointE[1] += y2 + ty;
      pointE[2] += -sry * x2 + cry * z2 + tz;
    }

    xnAve /= poseNum[ir];
    ynAve /= poseNum[ir];
    znAve /= poseNum[ir];

    pointE[0] /= poseNum[ir];
    pointE[1] /= poseNum[ir];
    pointE[2] /= poseNum[ir];

    cout << "point estimate: " << pointE.transpose() << endl;
    cout << "pose Num is: " << poseNum[ir] << endl;

    cout << "Add noise rmse: " << sqrt(pow(xnAve, 2) + pow(ynAve, 2) + pow(znAve, 2)) << endl;;
    cout << "estimate error rmse: " << sqrt(pow(pointE[0]-pointW[0],2) + pow(pointE[1]-pointW[1],2) + pow(pointE[2]-pointW[2],2)) << endl;
    cout << "************iret: " << ir << " end*****************" << endl;
  }
}
//g++ tri2_ave.cpp `pkg-config eigen3 --libs --cflags`

参考链接

三角测距方法:https://blog.csdn.net/KYJL888/article/details/107222533/
克莱默法则-https://blog.csdn.net/zbq_tt5/article/details/90043320
参考SVO-https://blog.csdn.net/luoshi006/article/details/80792043

你可能感兴趣的:(SLAM,图像处理,计算机视觉)