立体视觉匹配

最新推荐文章于 2024-06-14 11:58:57 发布

骑士——永不止步

最新推荐文章于 2024-06-14 11:58:57 发布

阅读量1.5k

点赞数

分类专栏： SLAM 立体视觉编程语言 C++ 文章标签：立体视觉三维坐标 SLAM

本文链接：https://blog.csdn.net/weixin_41074793/article/details/88392745

版权

C++ 同时被 3 个专栏收录

22 篇文章 0 订阅

订阅专栏

SLAM

19 篇文章 7 订阅

订阅专栏

编程语言

17 篇文章 2 订阅

订阅专栏

文章目录

题目

已知以下参数：
左相机内参(依次为fx\cx\fy\cy):
double Ml[4] = { 1674.86, 658.302, 1674.76, 529.925 };

左相机畸变系数:(k1\k2\p1\p2\k3\k4\k5\k6)
double Dl[8] = { -0.0315542, - 0.0298582, 0.000139779, - 0.000262658, - 0.308588, 0.0312938 ,- 0.100438, - 0.376244 };

右相机内参(依次为fx\cx\fy\cy):
double Mr[4] = { 1671.91, 686.756, 1671.78, 516.332 };

左右相机之间旋转矩阵R：
double R[3][3] = { 0.888061, - 0.00125292, 0.459724,
0.00433001, 0.999975, - 0.0056391,
- 0.459705, 0.00699847, 0.888044 };

左右相机之间平移向量T：
double T[3] = { -204.265, - 0.252961, 50.1539 };

其中RT满足关系：R*Pl+T=Pr。Pl为左相机中某一三维点（xl,yl,zl），Pr为右相机相机坐标系中某一三维点（xr,yr,zr）。

问：现有左相机图像像素坐标系中一二维点uvL(900,615)（未进行畸变校正）,和右相机坐标系图像像素坐标系中一二维点uvR(939.325073,597.613770)（已进行畸变校正）。已知两点为正确匹配点对，根据以上信息，编程实现：
1、重建以右相机为世界坐标系下的真实三维点坐标(X,Y,Z)。
2、求满足关系：R’*Pr+T’=Pl的旋转矩阵R’和平移向量T’。
3、求重建出来的三维坐标(X,Y,Z)重投影回左右相机后分别的图像坐标uvL’和uvR’，以及误差值。
4、求二维点uvR与二维点uvL在右相机中的极线L的距离。

在这里插入图片描述
关于图中的参数说明：
Ol、Or：左右相机光心；
pl、pr:像素坐标；
QlQr：直线Olp1与直线Orpr公垂线；
P：公垂线QlQr的中点；
$pl^{'}\ pr^{'}$ :重投影像素坐标；

解析

第一题

重点解析第1题

总体思路

先将左边的像素点去畸变
2.将所有点的坐标转换到以右camera为世界坐标系下的值
然后通过公垂线投影方法，解算出三维坐标P（也可以通过最小二乘方法解算）；

数学推导

在这里插入图片描述

第二题

$R^T$
$T'=-R^T*T$

第三题

由第一题求解得知的P坐标（是在右camera下的坐标值），通过内参矩阵和变换矩阵，可以求出投影的像素值。
右边的像素值：
$p_r' =\frac{1}{P_r.z()}\cdot K_r \cdot P_r$
左边的像素值：
$p_l' = K_l \cdot \begin{bmatrix} R^T & -R^T\cdot T \\ 0 &1\end{bmatrix}\cdot \begin{bmatrix} P_r\\ 1 \end{bmatrix}$

注意：
右边的像素值在求解的过程中，注意齐次与非齐次坐标的转换。
记 $\begin{bmatrix} R^T & -R^T\cdot T \\ 0 &1\end{bmatrix}\cdot \begin{bmatrix} P_r\\ 1 \end{bmatrix}$ 得到的结果是 ${\_}p_l =[x,y,z,1]^T$ ;
所以：
$p_l' = K_l \cdot \begin{bmatrix} x/z \\ y/z \\1\end{bmatrix}$
有了已知的像素坐标和重投影的像素坐标，很容易求解重投影误差，使用两点的距离公式即可求解。

第四题

求二维点uvR与二维点uvL在右相机中的极线L的距离。其中这里的都是指已经去畸变的像素坐标。
题目要求的是在右相机中对应的极线，并要算出右边的像素点到极线的距离。因为存在误差，右像素点并不在左像素点对应的极线上。

第一步：求解Esssential Matrix ： $E = h a t (T^{'}) * R^{'}$
- 这里为什么不是T、R，而是逆变换？
  因为这个是从求解Essential Matrix中推导出来的。
  因为已知的是在右camera下的坐标， $x_l = R' \cdot x_r +T'$
第二步：求解Fundamental Matrix： $=k_l^{-T}\cdot E\cdot K_r^{-1}$
第三步：调用opencv中computeCorrespondEpilines的函数，求出极线方程，输出的结果是N*3维的。其中传递参数的时候，第2个参数需要注意，1还是2。表示点来自于图片1还是图片2 。本题是2。1还是2取决于F矩阵。
对极约束公式为： $x_l^T\cdot E \cdot x_r=0$ ,所以： $p_l^Tk_l^{-T}\cdot E \cdot k_l^{-1}p_r=0$
- 也可以自己求解极线方程，已知空间的两个平面，求平面的交线）
第四步：使用点到直线的距离的公式，即可求出距离。

源代码

/*本代码需要提前安装好OpenCV、Soghus、Eigen库
*/
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <opencv2/core/eigen.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include "sophus/so3.h"
#include "sophus/se3.h"

using namespace std;
using namespace Eigen;
using namespace cv;

void pixel2camera(const Mat &k , const Point2d &point_uv, Vector3d &point_cam);
void pixel2camera(const Mat &k , const Vector2d &point_uv, Vector3d &point_cam);
void camera2pixel(const Mat &k , const Point2d &point_cam, Vector2d &point_uv);
void GetFMatrix( Eigen::Matrix3d &R, Eigen::Vector3d &T,
                 Eigen::Matrix3d &k_left, Eigen::Matrix3d &k_right,
                 Eigen::Matrix3d &Fundamental_matrix);
bool my_Invert(double A[3][3], double B[3][3], int n);

int main(){
    Matrix3d R;
    Vector3d T;
    Vector3d PL,PR;                               //p点在左右相机下的三维坐标
    Vector2d uvl;                                 //左边去畸变的像素坐标
    Point2d uvr= Point2d(939.325073,597.613770);   //右边去畸变的像素坐标
    Point2d uvl_undistorted = Point2d(900,615);  //左camera的带畸变的像素坐标
    Vector2d uvl_,uvr_;                              //重投影坐标

    R<<0.888061, -0.00125292, 0.459724,
       0.00433001, 0.999975, -0.0056391,
       -0.459705, 0.00699847, 0.888044 ;

    T<<-204.265,-0.252961,50.1539;

    Mat k_left, k_right;         //左相机内参
    k_left=(Mat_<double>(3,3)<<1674.86, 0, 658.302,
                            0, 1674.76, 529.925,
                             0, 0, 1);
    k_right = (Mat_<double>(3,3)<<1671.91, 0, 686.756,
            0, 1617.78, 516.332,
            0, 0, 1);

    Matrix3d k_left_,k_right_;
    cv2eigen(k_left,k_left_);
    cv2eigen(k_right,k_right_);

    vector<Point2d> uvl_array;              //为调用去畸变函数准备
    vector<Point2d> uvl_undistorted_array;  //为调用去畸变函数准备
    uvl_undistorted_array.push_back(uvl_undistorted);
    //左camera的畸变参数
    vector<double> Distort_Coefficients_left;
    double k1= -0.0315542;double k2= -0.0298582;double p1= 0.000139779;double p2= -0.000262658;
    double k3= -0.308588;double k4= 0.0312938;double k5= -0.100438;double k6= -0.376244;
    Distort_Coefficients_left.push_back(k1);Distort_Coefficients_left.push_back(k2);
    Distort_Coefficients_left.push_back(p1);Distort_Coefficients_left.push_back(p2);
    Distort_Coefficients_left.push_back(k3);Distort_Coefficients_left.push_back(k4);
    Distort_Coefficients_left.push_back(k5);Distort_Coefficients_left.push_back(k6);

    undistortPoints(uvl_undistorted_array,uvl_array,k_left,Distort_Coefficients_left);   //矫正左边的点
     //需要将变换后的点先变化为齐次坐标系，然后还需要乘以相机参数矩阵，才是最终的变化的坐标。
    Point2d _uvl_;                  //去畸变的归一化平面坐标
    _uvl_=uvl_array.front();
    camera2pixel(k_left,_uvl_ ,uvl);
    printf("左边去畸变的像素坐标uvl \n%.6f %.6lf \n",uvl.x(),uvl.y());

    /***************公垂线中点坐标解算三维点P坐标*************/
    Vector3d Or = Vector3d(0,0,0);
    Vector3d Ol = T;

    Vector3d Pr = Vector3d(0,0,0);   //右camera坐标下的uvr三维坐标
    pixel2camera(k_right,uvr,Pr);

    Vector3d Pl_ = Vector3d(0,0,0);    //左像素点的归一化平面坐标
    Vector3d Pl = Vector3d(0,0,0);     //左像素点在右camera下的三维坐标
    pixel2camera(k_left,uvl,Pl_);
    Pl = R*Pl_+T;                      //得到在右camera坐标下的uvl三维坐标

    Vector3d Pl_Ol = Pl-Ol;
    Vector3d Pr_Or = Pr-Or;
    Vector3d ll = Pl_Ol/sqrt( pow(Pl_Ol.x(),2) + pow(Pl_Ol.y(),2) + pow(Pl_Ol.z(),2) );
    Vector3d lr = Pr_Or/sqrt( pow(Pr_Or.x(),2) + pow(Pr_Or.y(),2) + pow(Pr_Or.z(),2) );   //单位方向向量

    Vector3d Pr_Pl = Pr-Pl;
    double l1l2=ll.dot(lr);
    double a1 = ( Pr_Pl.dot(ll) - ((Pr_Pl.dot(lr)) * l1l2 ))/( 1 - pow(l1l2,2) );
    double a2 = ( Pr_Pl.dot(ll) * l1l2 - Pr_Pl.dot(lr) )/( 1- pow(l1l2,2) );

    Vector3d Ql = Pl+a1*ll;
    Vector3d Qr = Pr+a2*lr;
    Vector3d Ql_Qr = Ql-Qr;

    PR=(Ql+Qr)/2;

    cout<<"公垂线长度||Ql_Qr|| = \n"<<sqrt(pow(Ql_Qr.x(),2)+pow(Ql_Qr.y(),2)+pow(Ql_Qr.z(),2))<<endl;
    cout<<"以右camera为世界坐标系的三维坐标 PR = "<<endl;
    printf("%.6f %.6f %.6f\n",PR.x(),PR.y(),PR.z());

    Matrix3d R_=R.transpose();
    Vector3d T_= -R.transpose()*T;
    cout<<"满足关系：R’*Pr+T’=Pl的旋转矩阵R=R^T' = \n"<<R_<<endl;  //右camera到左camera的转换
    cout<<"满足关系：R’*Pr+T’=Pl的平移向量T=-R^T*T' = \n"<<T_<<endl;

    //计算重投影像素坐标
    Sophus::SE3 SE3_Rt(R_, T_);  //注意旋转矩阵的方向
    Vector4d p_=SE3_Rt.matrix() * Vector4d(PR.x(),PR.y(),PR.z(),1); //齐次坐标化为非齐次坐标，注意维度
    Vector3d uvl_1 =  k_left_ * Vector3d( p_.x()/p_.z(),p_.y()/p_.z(),1);  //注意：先除z再与K相乘
    uvl_=Vector2d(uvl_1.x(),uvl_1.y());
    printf("左camera重投影的像素坐标 uvl' \n%.6f %.6lf \n",uvl_.x(),uvl_.y());

    uvr_.x()=k_right_(0,0)*PR.x()/PR.z()+k_right_(0,2);
    uvr_.y()=k_right_(1,1)*PR.y()/PR.z()+k_right_(1,2);
    printf("右边camera重投影的像素坐标 uvr' \n%.6f %.6lf \n",uvr_.x(),uvr_.y());

    //计算重投影坐标误差,两点距离
    double error_left =sqrt(pow(uvl.x()-uvl_.x(),2)+pow(uvl.y()-uvl_.y(),2) );
    double error_right=sqrt(pow(uvr.x-uvr_.x(),2)+pow(uvr.y-uvr_.y(),2) );

    printf("左边的重投影误差为：%.6f\n",error_left);
    printf("右边的重投影误差为：%.6f\n",error_right);



    Matrix3d Fundamental_matrix;
    GetFMatrix( R, T,k_left_, k_right_,Fundamental_matrix);

//    Fundamental_matrix<<-3.60256e-08 ,-1.76293e-05,0.00873334,
//                              -1.7912e-05, 4.88138e-07, -0.109915,
//    0.00955052, 0.133643 ,-9.40406;
    Fundamental_matrix<<-3.60256e-08 ,-1.82178e-05, 0.00903719,
                              -1.7912e-05, 5.04432e-07 ,-0.109923,
    0.00955052 ,0.138104, -11.7074;

    Mat fundamental;
    eigen2cv(Fundamental_matrix,fundamental);


   //计算极线L的距离
   vector<Point2d> points;
   points.push_back( Point2d(uvl.x(),uvl.y()) );
   Mat cor_lin;                        //correspondent_lines
   computeCorrespondEpilines(points,2,fundamental,cor_lin);   //极线计算函数
   // 函数解释：输入立体图像中的其中一幅图片，计算出在另一幅图片中的极线
    //第2个参数的解释：whichImage：1或者2,点是哪一幅图片中的。
    // 左边是2,右边是1,因为p1=kp，p2=k(Rp+t)。转换到这里：pr=k_left*PR,pl=k_right(R_*PR+T)
   cout<<"correspondent_lines= \n"<<cor_lin<<endl;
   double length = fabs(cor_lin.at<double>(0,0)*uvr.x+cor_lin.at<double>(0,1)*uvr.y +cor_lin.at<double>(0,2));
   printf("uvr到极线的距离 = \n %.6lf\n",length);
   //Mat默认是以行向量的形式存储

   return 0;
}

void pixel2camera(const Mat &k , const Point2d &point_uv, Vector3d &point_cam){
    point_cam.x()=(point_uv.x-k.at<double>(0,2))/k.at<double>(0,0);
    point_cam.y()=(point_uv.y-k.at<double>(1,2))/k.at<double>(1,1);
    point_cam.z()=1;          //写错了，写成了point_cam.y()=1
}
void pixel2camera(const Mat &k , const Vector2d &point_uv, Vector3d &point_cam){
    point_cam.x()=(point_uv.x()-k.at<double>(0,2))/k.at<double>(0,0);
    point_cam.y()=(point_uv.y()-k.at<double>(1,2))/k.at<double>(1,1);
    point_cam.z()=1;
}

void camera2pixel(const Mat &k , const Point2d &point_cam, Vector2d &point_uv){

    point_uv.x() = k.at<double>(0,0) * point_cam.x+ k.at<double>(0,2);
    point_uv.y() = k.at<double>(1,1) * point_cam.y+ k.at<double>(1,2);
}

void GetFMatrix( Eigen::Matrix3d &R, Eigen::Vector3d &T,
                 Eigen::Matrix3d &k_left, Eigen::Matrix3d &k_right,
                 Eigen::Matrix3d &Fundamental_matrix){
    Eigen::Vector3d T_;
    Eigen::Matrix3d R_;
    T_ = -R.transpose()*T;
    R_ = R.transpose();
    Matrix3d T_hat = Sophus::SO3::hat(T_);
    //K要注意，因为两边的K不一样.R、t也要注意，是逆变换。
    cout<<"T_hat"<<T_hat<<endl;
    Matrix3d e_matrix = T_hat * R_;
    cout<<"essential_matrix \n"<<e_matrix<<endl;
    Fundamental_matrix = ((k_left.inverse()).transpose()) * e_matrix * k_right.inverse();
    cout<<"fundamental_matrix = \n"<< Fundamental_matrix<<endl;
}

结果显示

左边去畸变的像素坐标uvl 
900.372577 615.121998 

公垂线长度||Ql_Qr|| = 
0.689768

以右camera为世界坐标系的三维坐标 PR = 
64.284953 21.034812 425.536019

满足关系：R’*Pr+T’=Pl的旋转矩阵R=R^T' = 
   0.888061  0.00433001   -0.459705
-0.00125292    0.999975  0.00699847
   0.459724  -0.0056391    0.888044
   
满足关系：R’*Pr+T’=Pl的平移向量T=-R^T*T' = 
  204.457
-0.353974
  49.3652
  
左camera重投影的像素坐标 uvl' 
900.405225 616.388165 

右边camera重投影的像素坐标 uvr' 
939.328407 596.301020 

左边的重投影误差为：1.266588

右边的重投影误差为：1.312754

fundamental_matrix = 
-3.60256e-08 -1.82178e-05   0.00903719
 -1.7912e-05  5.04432e-07    -0.109923
  0.00955052     0.138104     -11.7074
  
correspondent_lines= 
[-0.01229268817243207, 0.9999244420542461, -583.3964755063806]

uvr到极线的距离 =  2.625310

双目视觉测量中三维坐标的求取方法研究的部分内容

在这里插入图片描述

参考文献

[1] 罗世民, 李茂西. 双目视觉测量中三维坐标的求取方法研究[D]. , 2006.
[2] Gao X, Zhang T, Liu Y, et al. Visual SLAM fourteen lectures: From theory to practice［M］.Beijing: Electronic Industry Press, 2017.高翔, 张涛, 刘毅, 等. 视觉SLAM十四讲:从理论到实践［M］. 北京: 电子工业出版社, 2017.
[3] Bradski G, Kaehler A, 于仕琪, 等. 学习 OpenCV (中文版)[J]. 2009.