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

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

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

$$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<< "显示矫正图像" <

{

std::cout<< "Frame #" << i + 1 << "..." <

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 #include#include

using namespacestd;using namespacecv;

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};intmain()

{//已知空间坐标求像素坐标

Point3f point(700,220,530);

cout<

cout<

cout<

cout<

Point2f l =xyz2uv(point,leftIntrinsic,leftTranslation,leftRotation);

Point2f r=xyz2uv(point,rightIntrinsic,rightTranslation,rightRotation);

Point3f worldPoint;

worldPoint=uv2xyz(l,r);

cout<

system("pause");return 0;

}//************************************//Description: 根据左右相机中像素坐标求解空间坐标//Method: uv2xyz//FullName: uv2xyz//Access: public//Parameter: Point2f uvLeft//Parameter: Point2f uvRight//Returns: cv::Point3f//Author: 小白//Date: 2017/01/10//History://************************************

Point3f uv2xyz(Point2f uvLeft,Point2f uvRight)

{//[u1] [xw] [u2] [xw]//zc1 *|v1| = Pl*[yw] zc2*|v2| = P2*[yw]//[ 1] [zw] [ 1] [zw]//[1 ] [1 ]

Mat mLeftRotation = Mat(3,3,CV_32F,leftRotation);

Mat mLeftTranslation= Mat(3,1,CV_32F,leftTranslation);

Mat mLeftRT= Mat(3,4,CV_32F);//左相机RT矩阵

hconcat(mLeftRotation,mLeftTranslation,mLeftRT);

Mat mLeftIntrinsic= Mat(3,3,CV_32F,leftIntrinsic);

Mat mLeftP= mLeftIntrinsic *mLeftRT;//cout<

Mat mRightRotation= Mat(3,3,CV_32F,rightRotation);

Mat mRightTranslation= Mat(3,1,CV_32F,rightTranslation);

Mat mRightRT= Mat(3,4,CV_32F);//右相机RT矩阵

hconcat(mRightRotation,mRightTranslation,mRightRT);

Mat mRightIntrinsic= Mat(3,3,CV_32F,rightIntrinsic);

Mat mRightP= mRightIntrinsic *mRightRT;//cout<

Mat A = Mat(4,3,CV_32F);

A.at(0,0) = uvLeft.x * mLeftP.at(2,0) - mLeftP.at(0,0);

A.at(0,1) = uvLeft.x * mLeftP.at(2,1) - mLeftP.at(0,1);

A.at(0,2) = uvLeft.x * mLeftP.at(2,2) - mLeftP.at(0,2);