第七节、双目视觉之空间坐标计算

 在上一节我们已经介绍了如何对相机进行标定。然后获取相机的内部参数，外部参数。

内参包括焦距、主点、倾斜系数、畸变系数：

$$M=\begin{bmatrix} f_x & γ & u_0 \\ 0 & f_y & v_0  \\ 0  & 0 & 1\end{bmatrix}$$

其中$\gamma$为坐标轴倾斜参数，理想情况下为0。

外参包括旋转矩阵$R$、平移向量$T$，它们共同描述了如何把点从世界坐标系转换到摄像机坐标系，旋转矩阵描述了世界坐标系的坐标轴相对于摄像机坐标轴的方向，平移向量描述了在摄像机坐标系下空间原点的位置。

人类可以看到3维立体的世界，是因为人的两只眼睛，从不同的方向看世界，两只眼睛中的图像的视差，让我们可以看到三维立体的世界。类似的，要想让计算机“看到”三维世界，就需要使用至少两个摄像头构成双目立体视觉系统。

这里我们想要通过获取三维空间物体的坐标有两种方法，下面会介绍。

一、自己动手实现(只进行图片矫正、不进行立体校正)

1、标定双目后，首先要根据其畸变系数来矫正原图，这里用来去除图片的畸变：

        cv::Mat mapx = cv::Mat(imageSize, CV_32FC1);
        cv::Mat mapy = cv::Mat(imageSize, CV_32FC1);
        cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
        std::cout << "显示矫正图像" << endl;
        for (int i = 0; i != imageCount; i++)
        {
            std::cout << "Frame #" << i + 1 << "..." << endl;
            //计算图片畸变矫正的映射矩阵mapx、mapy(不进行立体校正，立体校正需要利用双摄)
            initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, imageSize, CV_32FC1, mapx, mapy);
            //读取一张图片
            Mat imageSource = imread(filenames[i]);
            Mat newimage = imageSource.clone();
            //另一种不需要转换矩阵的方式
            //undistort(imageSource,newimage,cameraMatrix,distCoeffs);
            //进行校正
            remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
            imshow("原始图像", imageSource);
            imshow("矫正后图像", newimage);
            waitKey();
        }

2、坐标计算

上一节我们介绍了从世界坐标系到像素坐标系的转换：

其中${\begin{bmatrix} u & v & 1 \end{bmatrix}}^T$为矫正后的图像中的点在像素坐标系中的坐标，${\begin{bmatrix} x_w & y_w &  z_w & 1 \end{bmatrix}}^T$为点在世界坐标系中的坐标。

现在我们来研究一下从像素坐标系到世界坐标系的转换：

光轴会聚模型(两个摄像机的像平面不是平行的)：

对于左右相机分别有：

将上面两式整理可以得到：

采用最小二乘法求解$X$,$Y$,$Z$，在opencv中可以用solve(A,B,XYZ,DECOMP_SVD)求解：

代码如下(来源于网上)：

//opencv2.4.9 vs2012
#include <opencv2\opencv.hpp>
#include <fstream>
#include <iostream>
 
using namespace std;
using namespace cv;
 
Point2f xyz2uv(Point3f worldPoint,float intrinsic[3][3],float translation[1][3],float rotation[3][3]);
Point3f uv2xyz(Point2f uvLeft,Point2f uvRight);
 
//图片对数量
int PicNum = 14;
 
//左相机内参数矩阵
float leftIntrinsic[3][3] = {
   4037.82450,             0,        947.65449,
                                      0,    3969.79038,        455.48718,
                                      0,             0,                1};
//左相机畸变系数
float leftDistortion[1][5] = {
   0.18962, -4.05566, -0.00510, 0.02895, 0};
//左相机旋转矩阵
float leftRotation[3][3] = {
   0.912333,        -0.211508,         0.350590, 
                            0.023249,        -0.828105,        -0.560091, 
                            0.408789,         0.519140,        -0.750590};
//左相机平移向量
float leftTranslation[1][3] = {-127.199992, 28.190639, 1471.356768};
 
//右相机内参数矩阵
float rightIntrinsic[3][3] = {
   3765.83307,             0,        339.31958,
                                        0,    3808.08469,        660.05543,
                                        0,             0,                1};
//右相机畸变系数
float rightDistortion[1][5] = {-0.24195, 5.97763, -0.02057, -0.01429, 0};
//右相机旋转矩阵
float rightRotation[3][3] = {-0.134947,         0.989568,        -0.050442, 
                              0.752355,         0.069205,        -0.655113, 
                             -0.644788,        -0.126356,        -0.753845};
//右相机平移向量
float rightTranslation[1][3] = {
   50.877397, -99.796492, 1507.312197};
 
 
int main()
{
    //已知空间坐标求像素坐标
    Point3f point(700,220,530);
    cout<<"左相机中坐标："<<endl;
    cout<<xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation)<<endl;
    cout<<"右相机中坐标："<<endl;
    cout<<xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation)<<endl;
 
    //已知左右相机像素坐标求空间坐标
    Point2f l = xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation);
    Point2f r = xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation);
    Point3f worldPoint;
    worldPoint = uv2xyz(l,r);
    cout<<"空间坐标为:"<<endl<<uv2xyz(l,r)<<endl;
 
    system("pause");
 
    return 0;
}
 
 
//************************************
//