SLAM--三角测量SVD分解法、最小二乘法及R t矩阵的判断

机器人学渣

已于 2022-05-04 23:25:48 修改

阅读量4k

点赞数 12

分类专栏： SLAM 文章标签： slam 计算机视觉

于 2021-07-19 23:34:25 首次发布

本文链接：https://blog.csdn.net/qq_42995327/article/details/118917141

版权

SLAM 专栏收录该内容

30 篇文章 15 订阅

订阅专栏

一、三角测量

方法一：SVD分解法的推导

我们假设在相机坐标1和2存在这样的重投影关系：
$\begin{bmatrix} u_1\\v_1\\1\end{bmatrix}=\frac 1 {s_1}KT_{1w}P_w$
$\begin{bmatrix} u_2\\v_2\\1\end{bmatrix}=\frac 1 {s_2}KT_{2w}P_w$
如果只考虑相机位姿1和2的相对关系，则又可以写成：
$\left \{ \begin{aligned} \begin{bmatrix} u_1\\v_1\\1\end{bmatrix}&=\frac 1 {s_1}K\cdot\space[\space I\space|\space 0 \space]\space \cdot P_w\quad,\space P_1 = P_w =\begin{bmatrix} X\\Y\\Z\\1\end{bmatrix} \\~\\ \begin{bmatrix} u_2\\v_2\\1\end{bmatrix} &=\frac 1 {s_2}K\cdot\space[\space R_{21}\space|\space t \space]\space \cdot P_1 \end{aligned} \right.\tag{2}$
这里，一般我们是知道相机内参 $K$ 的，所以为了忽略相机内参，我们令映射矩阵(3X4)：
$pr_1 = K\cdot\space[\space I\space|\space 0 \space]\\~\\ pr_2 = K\cdot\space[\space R_{21}\space|\space t \space]$
写成行向量的形式
$pr_1 = \begin{bmatrix} pr_{1,1}\\pr_{1,2}\\pr_{1,3}\end{bmatrix}, pr_2 = \begin{bmatrix} pr_{2,1}\\pr_{2,2}\\pr_{2,3}\end{bmatrix}$
投影映射方程变为：
$\begin{bmatrix} s_1u_1\\s_1v_1\\s_1\end{bmatrix}=\begin{bmatrix} pr_{1,1}\\pr_{1,2}\\pr_{1,3}\end{bmatrix}P_1\\~\\ \begin{bmatrix} s_2u_2\\s_2v_2\\s_2\end{bmatrix}=\begin{bmatrix} pr_{2,1}\\pr_{2,2}\\pr_{2,3}\end{bmatrix}P_1$

我们观察两个方程，可以发现能用第三行消去深度参数，所以有：
$\begin{bmatrix} pr_{1,3}\cdot P_1\cdot u_1\\pr_{1,3}\cdot P_1\cdot v_1\end{bmatrix}=\begin{bmatrix} pr_{1,1}\cdot P_1\\pr_{1,2}\cdot P_1\end{bmatrix}\\~\\ \begin{bmatrix} pr_{2,3}\cdot P_1\cdot u_2\\pr_{2,3}\cdot P_1\cdot v_2\end{bmatrix}=\begin{bmatrix} pr_{2,1}\cdot P_1\\pr_{2,2}\cdot P_1\end{bmatrix}\\~\\$
我们把要求的P1提出来，就可以得到线性方程：
$\begin{bmatrix} pr_{1,3}\cdot u_1-pr_{1,1}\\pr_{1,3}\cdot v_1-pr_{1,2}\end{bmatrix}P_1=0\\~\\ \begin{bmatrix} pr_{2,3}\cdot u_2-pr_{2,1}\\pr_{2,3}\cdot v_2-pr_{2,2}\end{bmatrix}P_1=0\\~\\$
即：
$\begin{bmatrix} pr_{1,3}\cdot u_1-pr_{1,1}\\pr_{1,3}\cdot v_1-pr_{1,2}\\pr_{2,3}\cdot u_2-pr_{2,1}\\pr_{2,3}\cdot v_2-pr_{2,2}\end{bmatrix} P_1=0$
我们利用SVD分解的方法来解这个方程，求出P1，但我们发现，这其实是一个超定方程，所以利用SVD分解来得到一个最小二乘解，（可以回顾下矩阵论的知识，超定方程怎么求最小二乘解）；

我们令:
$\begin{bmatrix} pr_{1,3}\cdot u_1-pr_{1,1}\\pr_{1,3}\cdot v_1-pr_{1,2}\\pr_{2,3}\cdot u_2-pr_{2,1}\\pr_{2,3}\cdot v_2-pr_{2,2}\end{bmatrix}$
对A进行奇异值分解：
$A_{4\times4} = UWV^T$
即
$U\cdot \begin{bmatrix} \sigma_1 \\&\sigma_2\\& &\sigma_3\\ &&&\sigma_4\end{bmatrix}[V1\quad V2\quad V3 \quad V4] ^T，\sigma_1 \geq \sigma_2 \geq \sigma_3 \geq \sigma_4 \geq 0$
根据 $A x = 0$ 最小二乘法求解方法，其最小二乘解即为 $A^TA$ 最小特征值的特征向量。
参考：
证明AX=0的最小二乘解是ATA最小特征值对应的特征向量

所以我们取SVD分解得到的V矩阵最后一列作为 $P_1$ 的解，但我们还需要归一化以后才能使用。

方法二：最小二乘法求解

我们知道的投影关系：(P1和P2分别是在相机1和相机2坐标系下的点)
$\begin{bmatrix} u_1\\v_1\\1\end{bmatrix}=\frac 1 {s_1}KP_1$
$\begin{bmatrix} u_2\\v_2\\1\end{bmatrix}=\frac 1 {s_2}KP_2$
所以可以推出：
$K^{-1}\begin{bmatrix} u_1\\v_1\\1\end{bmatrix}=\frac 1 {s_1}P_1 = e_1\\~\\ K^{-1} \begin{bmatrix} u_2\\v_2\\1\end{bmatrix}=\frac 1 {s_2}P_2=e_2$
其中相机坐标系下坐标 $P_1$ 和 $P_2$ 存在关系：
$P_2 = R_{21}\cdot P_1+t_{21}$
所以联合上面的式子我们可以得到：
$K^{-1} \begin{bmatrix} u_2\\v_2\\1\end{bmatrix}=\frac 1 {s_2}(s_1\cdot R_{21}\cdot K^{-1}\begin{bmatrix} u_1\\v_1\\1\end{bmatrix}+t_{21} )$
简单表示，即：
$e_2=\frac 1 {s_2}(s_1\cdot R_{21}\cdot e_1+t_{21} )\\~\\ \Longrightarrow s_2\cdot e_2=(s_1\cdot R_{21}\cdot e_1+t_{21} )\\~\\ \Longrightarrow \begin{bmatrix} -R_{21}\cdot e_1&e_2 \end{bmatrix} \begin{bmatrix} s_1 \\ s_2\end{bmatrix} = t_{21}$
这样我们就得到了线性方程的形式: $A x = b$
这里：
$A=\begin{bmatrix} -R_{21}\cdot e_1&e_2 \end{bmatrix} ，x=\begin{bmatrix} s_1 \\ s_2\end{bmatrix}$
我们根据矩阵论的知识可以知道，一个无解方程组存在唯一的最小二乘解，一般我们会对A矩阵进行BC满秩分解：
$r，A_{m\times n} = B_{m\times r}C_{r\times n}\\~\\ 若r = m（行满秩），A = EA\\~\\ 若r = n (列满秩)，A=AE$
则A的广义逆矩阵为：
$A^{+} = C^T(CC^T)^{-1}(B^TB)^{-1}B^T\\~\\$
特殊情况：
$行满秩：A^+ = A^T(AA^T)^{-1}\\~\\列满秩：A^+=(A^TA)^{-1}A^T$

我们的最小二乘解就是： $x = A^+b$ （不进行推导了，可自行复习矩阵论的知识）

在这里我们的A是列满秩(3X2的矩阵)，对于无解方程组，其最小二乘解：
$\begin{bmatrix} s_1 \\ s_2\end{bmatrix}= (A^TA)^{-1}A^T\cdot t_{21}$
$s_1$ 和 $s_2$ 分别是在两个相机坐标系下的深度；

二、ORB_SLAM2 三角测量源码：

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();//取SVD分解得到的V矩阵最后一列作为$P_1$的解
    x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}

三、利用Eigen源码实现三角测量：

方法一：SVD分解法

为了更好的理解过程，利用Eigen实现了三角测量：

Eigen::Vector3d triangulatedByEigenSVD(Point2f pixel_1,Point2f pixel_2,Mat R,Mat t,Mat &K)
{
    Eigen::Matrix3d K_eigen;
    cv::cv2eigen(K,K_eigen);//eigen 类型的K内参 需要包含 #include <opencv2/core/eigen.hpp>

    Eigen::Matrix<double,3,4> T_1,T_21;
    T_1<<   1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0;

    T_21<<  R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0),
            R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0),
            R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0);

    Eigen::Matrix<double,3,4> ProjectMatrix_1,ProjectMatrix_2;

    ProjectMatrix_1 = K_eigen * T_1;
    ProjectMatrix_2 = K_eigen * T_21;

    Eigen::Matrix4d A;
    A.row(0) = ProjectMatrix_1.row(2)*pixel_1.x - ProjectMatrix_1.row(0);
    A.row(1) = ProjectMatrix_1.row(2)*pixel_1.y - ProjectMatrix_1.row(1);
    A.row(2) = ProjectMatrix_2.row(2)*pixel_2.x - ProjectMatrix_2.row(0);
    A.row(3) = ProjectMatrix_2.row(2)*pixel_2.y - ProjectMatrix_2.row(1);

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    Eigen::Matrix4d U = svd.matrixU();
    Eigen::Matrix4d V = svd.matrixV();
    Eigen::Vector4d S = svd.singularValues();

    Eigen::Vector4d p_w = V.col(3);
    p_w /= p_w(3,0);

    return Eigen::Vector3d(p_w(0),p_w(1),p_w(2));
}

方法二：最小二乘法求解（速度最快）

Eigen::Vector3d triangulatedByEigenLeastSquare(Point2f pixel_1, Point2f pixel_2, Mat R, Mat t, Mat &K)
{
    Eigen::Vector3d point_1,point_2;
    point_1<<pixel_1.x,pixel_1.y,1;//齐次坐标
    point_2<<pixel_2.x,pixel_2.y,1;

    Eigen::Matrix3d K_eigen,R_eigen;
    Eigen::Vector3d t_eigen;
    cv::cv2eigen(K,K_eigen);//eigen 类型的K内参
    cv::cv2eigen(R,R_eigen);//eigen 类型的R
    cv::cv2eigen(t,t_eigen);//eigen 类型的t

    Eigen::Matrix<double,3,2> A;
    A.block(0,0,3,1) = -R_eigen*K_eigen.inverse()*point_1;
    A.block(0,1,3,1) = K_eigen.inverse()*point_2;

    //行满秩 最小二乘解：x = A^T * (A*A^T)^-1 b
    //列满秩 最小二乘解：x = (A^T*A)^-1 * A^T b
    Eigen::Vector2d d =(A.transpose()*A).inverse()*A.transpose()*t_eigen;//d[0],d[1]就是相机坐标系1 和 2下特征点深度

    Eigen::Vector3d p1 = d[0]*K_eigen.inverse()*point_1;
    //Eigen::Vector3d p2 = d[1]*K_eigen.inverse()*point_2;//p_2也能求出
    return p1;
}

方法三：利用OpenCV自带函数

参考源码：视觉SLAM十四讲–高翔

//像素坐标系到相机坐标系
Point3d pixel2camera( const Mat &K,const Point2d &p,const double depth = 1.0 )
{
  return Point3d
    (
      depth*(p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      depth*(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1),
              depth
    );
}

void triangulation(vector<Point2f> pixel_1,vector<Point2f> pixel_2,Mat R,Mat t,vector< Point3d >& points)
{
    Mat projMatr1 = (Mat_<float> (3,4) <<
            1,0,0,0,
            0,1,0,0,
            0,0,1,0);
    Mat projMatr2 = (Mat_<float> (3,4) <<
            R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0),
            R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0),
            R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0)
        );

    vector<Point2f> projPoints1,projPoints2;
    for(size_t i =0;i<pixel_1.size();i++)
    {
        Point3d camera_1 = pixel2camera(K,pixel_1[i],1.0);
        Point3d camera_2 = pixel2camera(K,pixel_2[i],1.0);
        projPoints1.push_back(Point2f(camera_1.x,camera_1.y));
        projPoints2.push_back(Point2f(camera_2.x,camera_2.y));
    }

    Mat points4D;
    cv::triangulatePoints( projMatr1, projMatr2, projPoints1, projPoints2, points4D );

    // 转换成非齐次坐标
    for ( int i=0; i<points4D.cols; i++ )
    {
        Mat x = points4D.col(i);
        x /= x.at<float>(3,0); // 归一化
        Point3d p (
            x.at<float>(0,0),
            x.at<float>(1,0),
            x.at<float>(2,0)
        );
        points.push_back( p );
    }
}

四、对求解的R t筛选的方法：

我们利用求解的两个相机坐标系下坐标点深度的正负的比例来判断R t 是否是最佳的(pixel_1是匹配特征点在相机1的像素坐标，pixel_2是在相机2中的)：

bool checkRt(Eigen::Matrix3d R_eigen, Eigen::Vector3d t_eigen, vector<Point2f> pixel_1, vector<Point2f> pixel_2)
{
    Mat R,t;
    cv::eigen2cv(R_eigen,R);
    cv::eigen2cv(t_eigen,t);
    vector<Eigen::Vector3d> P_1,P_2;
    for(auto i=0;i<pixel_1.size();i++)
    {
        P_1.push_back(triangulatedByEigenSVD(pixel_1[i],pixel_2[i],R,t,K));
        //P2 = RP1 + t
        P_2.push_back(R_eigen * P_1[i] + t_eigen);
    }

    int good_num_p1 = 0,good_num_p2 = 0;
    for(auto ptr_1:P_1)
    {
        if (ptr_1(2,0) > 0)
        {
            good_num_p1++;
        }
    }

    for(auto ptr_2:P_2)
    {
        if (ptr_2(2,0) > 0)
        {
            good_num_p2++;
        }
    }

    //cout<<good_num_p1<<" "<<good_num_p2;

    //设定阈值
    double posibility = (good_num_p1/(double)P_1.size())*(good_num_p2/(double)P_2.size());
    if(posibility > 0.7)
        return true;
    else
        return false;
}

使用演示：
首先可以利用本质矩阵opencv 解出的essential_matrix，进行SVD分解求出四组 R ， t：

    Eigen::Matrix3d A;
    cv::cv2eigen(essential_matrix,A);
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
    auto U = svd.matrixU();
    auto V = svd.matrixV();
    auto S = svd.singularValues();
    cout<<"A = \n"<<A<<endl;
    cout<<"S = \n"<<S<<endl;
    cout<<"U = \n"<<U<<endl;
    cout<<"VT = \n"<<V.transpose()<<endl;
    auto A_souce = U*S.asDiagonal()*V.transpose();
    cout<<"A constructed by U*S*VT is \n"<<A_souce<<endl;
    Eigen::Matrix3d Z,W;
    Z<<0,1,0,
       -1,0,0,
       0,0,0;
    W<<0,-1,0,
       1,0,0,
       0,0,1;
    //四种情况，需要利用深度信息（正负）来判断和筛选。
    Eigen::Matrix3d t_eigen_hat_1 = U*Z*U.transpose();              // 也可利用 U.col(2).transpose()
    Eigen::Matrix3d t_eigen_hat_2 = U*Z.transpose()*U.transpose();  // 也可利用 -U.col(2).transpose()
    Eigen::Matrix3d R_eigen_1 = U*W*V.transpose();
    Eigen::Matrix3d R_eigen_2 = U*W.transpose()*V.transpose();

    //----------转换为3X1的向量------
    Eigen::Vector3d t_eigen_1,t_eigen_2;
        t_eigen_1<<-t_eigen_hat_1(1,2),t_eigen_hat_1(0,2),-t_eigen_hat_1(0,1);
        t_eigen_2<<-t_eigen_hat_2(1,2),t_eigen_hat_2(0,2),-t_eigen_hat_2(0,1);

再利用checkRt函数进行判断筛选：

    bool index_1 = checkRt(R_eigen_1,t_eigen_1,pixel_1,pixel_2);
    bool index_2 = checkRt(R_eigen_1,t_eigen_2,pixel_1,pixel_2);
    bool index_3 = checkRt(R_eigen_2,t_eigen_1,pixel_1,pixel_2);
    bool index_4 = checkRt(R_eigen_2,t_eigen_2,pixel_1,pixel_2);
    cout<<"The R1 t1 is score:"<<index_1<<endl;
    cout<<"The R1 t2 is score:"<<index_2<<endl;
    cout<<"The R2 t1 is score:"<<index_3<<endl;
    cout<<"The R2 t2 is score:"<<index_4<<endl;

    cout<<"The best Rotation and translation is :\n";
    if(index_1 == 1)
    {
        cout<<"R_eigen_1:\n"<<R_eigen_1<<endl;
        cout<<"t_eigen_1:\n"<<t_eigen_1.transpose()<<endl;
    }
    else if(index_2 == 1)
    {
        cout<<"R_eigen_1:\n"<<R_eigen_1<<endl;
        cout<<"t_eigen_2:\n"<<t_eigen_2.transpose()<<endl;
    }
    else if(index_3 == 1)
    {
        cout<<"R_eigen_2:\n"<<R_eigen_2<<endl;
        cout<<"t_eigen_1:\n"<<t_eigen_1.transpose()<<endl;
    }
    else if(index_4 == 1)
    {
        cout<<"R_eigen_2:\n"<<R_eigen_2<<endl;
        cout<<"t_eigen_2:\n"<<t_eigen_2.transpose()<<endl;
    }

运行结果：

The R1 t1 is score:1
The R1 t2 is score:0
The R2 t1 is score:0
The R2 t2 is score:0
The best Rotation and translation is :
R_eigen_1:
  0.996291 -0.0532112  0.0676278
 0.0503369   0.997784  0.0435187
-0.0697936 -0.0399531   0.996761
t_eigen_1:
  -0.9663 -0.182217   0.18183