去年三四月份实验室做了一个机器人与视觉识别系统的项目，主要就是利用双目摄像头进行物体空间坐标定位，然后利用机器人进行抓取物体。当时我才研一，还是个菜鸡，项目主要是几个学长负责做的，我也就是参与打打酱油，混混经验。现在过了一年多了，机器人一直在实验室放着，空着也是浪费，所以就想搞点事情。这里我们就先从利用双目摄像头进行空间定位说起，因为这是整个项目的核心部分。

双目视觉是建立在几何数学的基础上，数学推导是枯燥乏味的。因此这里不去过多的介绍数学原理，只是简要的叙述一下双目视觉的流程。

双目视觉主要包括相机标定、图片畸变矫正、摄像机校正、图片匹配、3D恢复五个部分。

面我们从相机标定开始说起。相机标定的目的有两个。

第一，要还原摄像头成像的物体在真实世界的位置就需要知道世界中的物体到计算机图像平面是如何变换的，相机标定的目的之一就是为了搞清楚这种变换关系，求解内外参数矩阵。
第二，摄像机的透视投影有个很大的问题——畸变。摄像头标定的另一个目的就是求解畸变系数，然后用于图像矫正。
一、三大坐标系
谈到相机标定，我们不得不说起摄相机坐标系、世界坐标系、图像坐标系。


图是三个坐标的示意简图，通过它大家可以对三个坐标有一个直观的认识。

世界坐标系(𝑋𝑤,𝑌𝑤,𝑍𝑤)(Xw,Yw,Zw)（等同(𝑥𝑤,𝑦𝑤,𝑧𝑤)(xw,yw,zw)）：目标物体位置的参考系。除了无穷远，世界坐标可以根据运算方便与否自由放置，单位为长度单位如𝑚𝑚mm。在双目视觉中世界坐标系主要有三个用途：
标定时确定标定物的位置；
作为双目视觉的系统参考系，给出两个摄像机相对世界坐标系的关系，从而求出相机之间的相对关系；
作为重建得到三维坐标的容器，存放重建后的物体的三维坐标。世界坐标系是将看见中物体纳入运算的第一站。
摄像机坐标系(𝑋𝑐,𝑌𝑐,𝑍𝑐)(Xc,Yc,Zc)（等同(𝑥𝑐,𝑦𝑐,𝑧𝑐)(xc,yc,zc)）：摄像机站在自己角度上衡量的物体的坐标系。摄像机坐标系的原点在摄像机的光心上，𝑧z轴与摄像机光轴平行。它是与拍摄物体发生联系的桥头堡，世界坐标系下的物体需先经历刚体变化转到摄像机坐标系，然后在和图像坐标系发生关系。它是图像坐标与世界坐标之间发生关系的纽带，沟通了世界上最远的距离。单位为长度单位如𝑚𝑚mm。
图像坐标系(𝑥,𝑦)(x,y)：以CCD 图像平面的中心为坐标原点，为了描述成像过程中物体从相机坐标系到图像坐标系的投影透射关系而引入，方便进一步得到像素坐标系下的坐标。图像坐标系是用物理单位（例如毫米）表示像素在图像中的位置。
像素坐标系(𝑢,𝑣)(u,v)：以 CCD 图像平面的左上角顶点为原点，为了描述物体成像后的像点在数字图像上（相片）的坐标而引入，是我们真正从相机内读取到的信息所在的坐标系。像素坐标系就是以像素为单位的图像坐标系。
备注：有很多人把图像坐标系和像素坐标系合在一起，称作三大坐标系，也有人分开，称为四大坐标系。

1.1 图像坐标系到像素坐标系
讲到这里，你可能会问有了图像坐标系为什么还要建一个像素坐标系？

我们以图像左上角为原点建立以像素为单位的直接坐标系𝑢u-𝑣v。像素的横坐标𝑢u与纵坐标𝑣v分别是在其图像数组中所在的列数与所在行数。

由于(𝑢,𝑣)(u,v)只代表像素的列数与行数，而像素在图像中的位置并没有用物理单位表示出来，所以，我们还要建立以物理单位（如毫米）表示的图像坐标系𝑥x-𝑦y。

将相机光轴与图像平面的交点（一般位于图像平面的中心处，也称为图像的主点（principal point）定义为该坐标系的原点𝑂1O1，且𝑥x轴与𝑢u轴平行，𝑦y轴与𝑣v轴平行，假设(𝑢0,𝑣0)(u0,v0)代表𝑂1O1在𝑢u-𝑣v坐标系下的坐标，𝑑𝑥dx与𝑑𝑦dy分别表示每个像素在横轴𝑥x和纵轴𝑦y上的物理尺寸，则图像中的每个像素在𝑢u-𝑣v坐标系中的坐标和在𝑥x-𝑦y坐标系中的坐标之间都存在如下的关系：
𝑢= 𝑥 / 𝑑𝑥+𝑢0
v=y / dy+v0
其中，我们假设物理坐标系中的单位为毫米，那么𝑑𝑥dx的的单位为:毫米/像素。那么𝑥/𝑑𝑥x/dx的单位就是像素了，即和𝑢u的单位一样都是像素。为了使用方便，可将上式用齐次坐标与矩阵形式表示为：
差公式
为了让你更直接的理解这一块内容，我们举个例子，由于被摄像机摄物体的图像经过镜头投影到CCD芯片上(像平面)：

我们设CCD的大小为8×6𝑚𝑚8×6mm,而拍摄到的图像大小为640×480640×480，则𝑑𝑥=180𝑚𝑚dx=180mm/像素，𝑑𝑦=180𝑚𝑚dy=180mm/像素,𝑢0=320u0=320,𝑣0=240v0=240。

上面的矩阵公式运用了齐次坐标，初学者可能会感到有些迷惑。大家会问：怎样将普通坐标转换为齐次坐标呢？齐次坐标能带来什么好处呢？

这里对齐次坐标做一个通俗的解释。此处只讲怎么将普通坐标改写为齐次坐标及为什么引入齐次坐标。这里只做一个通俗但不太严谨的表述。力求简单明了。针对齐次坐标的严谨的纯数学推导，可参见“周兴和版的《高等几何》—1.3拓广平面上的齐次坐标”。玉米曾详细读过《高等几何》这本书，但觉得离计算机视觉有点远，是讲纯数学的投影关系的，较为生涩难懂。

齐次坐标可以理解为在原有坐标后面加一个“小尾巴”。将普通坐标转换为齐次坐标，通常就是在增加一个维度，这个维度上的数值为1。如图像坐标系(𝑢,𝑣)(u,v)转换为(𝑢,𝑣,1)(u,v,1)一样。对于无穷远点，小尾巴为0。注意，给零向量增加小尾巴，数学上无意义。

那么，为什么计算机视觉在坐标运算时要加上这个“小尾巴”呢？

将投影平面扩展到无穷远点。如对消隐点(vanishing point)的描述；
使得计算更加规整；
如果用普通坐标来表达的话，会是下面的样子：
差公式
这样的运算形式会给后面的运算带来一定的麻烦，所以齐次坐标是一个更好的选择。

齐次坐标还有一个重要的性质，伸缩不变性。即：设齐次坐标𝑀M，则α𝑀=𝑀αM=M。

我们介绍过了像素坐标系之后，我们在此三大坐标系的问题上。我们想知道这三个坐标系有什么样的关系，我们先从下图说起：

图中显示，世界坐标系通过刚体变换到达摄像机坐标系，然后摄像机坐标系通过透视投影变换到达图像坐标系。可以看出，世界坐标与图像坐标的关系建立在刚体变换和透视投影变换的基础上。

1.2 世界坐标系到摄像机坐标系
首先，让我们来看一下刚体变换是如何将世界坐标系与图像坐标系联系起来的吧。这里，先对刚体变换做一个介绍：

刚体变换(regidbody motion)：三维空间中， 当物体不发生形变时，对一个几何物体作旋转， 平移的运动，称之为刚体变换。

因为世界坐标系和摄像机坐标都是右手坐标系，所以其不会发生形变。我们想把世界坐标系下的坐标转换到摄像机坐标下的坐标，如下图所示，可以通过刚体变换的方式。空间中一个坐标系，总可以通过刚体变换转换到另外一个个坐标系的。

下面看一下，二者之间刚体变换的数学表达：
差公式
对应的齐次表达式为：
差公式

其中，𝑅R是3×33×3的正交单位矩阵（即旋转矩阵），𝑡t为平移向量，𝑅R、𝑡t与摄像机无关，所以称这两个参数为摄像机的外参数(extrinsic parameter)，可以理解为两个坐标原点之间的距离，因其受𝑥x,𝑦y,𝑧z三个方向上的分量共同控制，所以其具有三个自由度。

我们假定在世界坐标系中物点所在平面过世界坐标系原点且与𝑍𝑤Zw轴垂（也即棋盘平面与𝑋𝑤Xw-𝑌𝑤Yw平面重合，目的在于方便后续计算），则𝑍𝑤=0Zw=0。

1.3 摄像机坐标系到图像坐标系
首先，让我们来看一下透视投影是如何将摄像机坐标系与图像坐标系联系起来的吧。这里，先对透视投影做一个介绍：

透视投影(perspective projection)： 用中心投影法将形体投射到投影面上，从而获得的一种较为接近视觉效果的单面投影图。有一点像皮影戏。它符合人们心理习惯，即离视点近的物体大，离视点远的物体小，不平行于成像平面的平行线会相交于消隐点(vanish point)

这里我们还是拿针孔成像来说明（除了成像亮度低外，成像效果和透视投影是一样的，但是光路更简单）

下图是针kong-摄像机的基本模型。平面ππ称为摄像机的像平面，点𝑂𝑐Oc称为摄像机中心（或光心），𝑓f成为摄像机的焦距，𝑂𝑐Oc为端点且垂直于像平面的射线成为光轴或主轴，主轴与像平面的交点𝑝p是摄像机的主点。

如图所示，图像坐标系为𝑜o-𝑥𝑦xy，摄像机坐标系为𝑂𝑐Oc-𝑥𝑐𝑦𝑐𝑧𝑐xcyczc。记空间点𝑋𝑐Xc摄像机坐标系中的齐次坐标为:
差公式
它的像点𝑚m在图像坐标系中的齐次坐标记为
差公式
根据三角形相似原理，可得：
差公式
我们使用矩阵表示为：
差公式
注意由于齐次坐标的伸缩不变性，𝑧𝑐[𝑥𝑦1]𝑇zc[xy1]T和[𝑥𝑦1]𝑇[xy1]T表示的是同一点
1.4 总结
我们已经介绍了各个坐标系之间的转换过程，但是我们想知道的是如何从世界坐标系转换到像素坐标系，因此我们需要把上面介绍到的联系起来：

将三者相乘，可以把这三个过程和在一起，写成一个矩阵：

差一段

二、图片矫正
我们在摄像机坐标系到图像坐标系变换时谈到透视投影。摄像机拍照时通过透镜把实物投影到像平面上，但是透镜由于制造精度以及组装工艺的偏差会引入畸变，导致原始图像的失真。因此我们需要考虑成像畸变的问题。

透镜的畸变主要分为径向畸变和切向畸变，还有薄透镜畸变等等，但都没有径向和切向畸变影响显著，所以我们在这里只考虑径向和切向畸变。

2.1 径向畸变
顾名思义，径向畸变就是沿着透镜半径方向分布的畸变，产生原因是光线在原理透镜中心的地方比靠近中心的地方更加弯曲，这种畸变在普通廉价的镜头中表现更加明显，径向畸变主要包括桶形畸变和枕形畸变两种。以下分别是枕形和桶形畸变示意图：

它们在真实照片中是这样的：

像平面中心的畸变为0，沿着镜头半径方向向边缘移动，畸变越来越严重。畸变的数学模型可以用主点（principle point）周围的泰勒级数展开式的前几项进行描述，通常使用前两项，即𝑘1k1和𝑘2k2，对于畸变很大的镜头，如鱼眼镜头，可以增加使用第三项𝑘3k3来进行描述，成像仪上某点根据其在径向方向上的分布位置，调节公式为：
差公式
式里(𝑥0,𝑦0)(x0,y0)是畸变点在像平面的原始位置，(𝑥,𝑦)(x,y)是畸变较正后新的位置，下图是距离光心不同距离上的点经过透镜径向畸变后点位的偏移示意图，可以看到，距离光心越远，径向位移越大，表示畸变也越大，在光心附近，几乎没有偏移。

差一段

关于OpenCV提供的用于相机标定的API函数可以查看博客双目视觉标定程序讲解，单目标定的代码如下：

/*************************************************************************************
*
*   Description：相机标定，张氏标定法  单目标定
*   Author     ：JNU
*   Data       ：2018.7.22
*
************************************************************************************/
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <fstream>
#include <vector>using namespace cv;
using namespace std;void main(char *args)
{//保存文件名称std::vector<std::string>  filenames;//需要更改的参数//左相机标定，指定左相机图片路径，以及标定结果保存文件string infilename = "sample/left/filename.txt";        //如果是右相机把left改为rightstring outfilename = "sample/left/caliberation_result.txt";//标定所用图片文件的路径,每一行保存一个标定图片的路径  ifstream 是从硬盘读到内存ifstream fin(infilename);//保存标定的结果  ofstream 是从内存写到硬盘ofstream fout(outfilename);/*1.读取毎一幅图像，从中提取出角点，然后对角点进行亚像素精确化、获取每个角点在像素坐标系中的坐标像素坐标系的原点位于图像的左上角*/std::cout << "开始提取角点......" << std::endl;;//图像数量int imageCount = 0;//图像尺寸cv::Size imageSize;//标定板上每行每列的角点数cv::Size boardSize = cv::Size(9, 6);//缓存每幅图像上检测到的角点std::vector<Point2f>  imagePointsBuf;//保存检测到的所有角点std::vector<std::vector<Point2f>> imagePointsSeq;char filename[100];if (fin.is_open()){//读取完毕？while (!fin.eof()){//一次读取一行fin.getline(filename, sizeof(filename) / sizeof(char));//保存文件名filenames.push_back(filename);//读取图片Mat imageInput = cv::imread(filename);//读入第一张图片时获取图宽高信息if (imageCount == 0){imageSize.width = imageInput.cols;imageSize.height = imageInput.rows;std::cout << "imageSize.width = " << imageSize.width << std::endl;std::cout << "imageSize.height = " << imageSize.height << std::endl;}std::cout << "imageCount = " << imageCount << std::endl;imageCount++;//提取每一张图片的角点if (cv::findChessboardCorners(imageInput, boardSize, imagePointsBuf) == 0){//找不到角点std::cout << "Can not find chessboard corners!" << std::endl;exit(1);}else{Mat viewGray;//转换为灰度图片cv::cvtColor(imageInput, viewGray, cv::COLOR_BGR2GRAY);//亚像素精确化   对粗提取的角点进行精确化cv::find4QuadCornerSubpix(viewGray, imagePointsBuf, cv::Size(5, 5));//保存亚像素点imagePointsSeq.push_back(imagePointsBuf);//在图像上显示角点位置cv::drawChessboardCorners(viewGray, boardSize, imagePointsBuf, true);//显示图片//cv::imshow("Camera Calibration", viewGray);cv::imwrite("test.jpg", viewGray);//等待0.5s//waitKey(500);}}        //计算每张图片上的角点数 54int cornerNum = boardSize.width * boardSize.height;//角点总数int total = imagePointsSeq.size()*cornerNum;std::cout << "total = " << total << std::endl;for (int i = 0; i < total; i++){int num = i / cornerNum;int p = i%cornerNum;//cornerNum是每幅图片的角点个数，此判断语句是为了输出，便于调试if (p == 0){                                        std::cout << "\n第 " << num+1 << "张图片的数据 -->: " << std::endl;}//输出所有的角点std::cout<<p+1<<":("<< imagePointsSeq[num][p].x;std::cout << imagePointsSeq[num][p].y<<")\t";if ((p+1) % 3 == 0){std::cout << std::endl;}}std::cout << "角点提取完成!" << std::endl;/*2.摄像机标定 世界坐标系原点位于标定板左上角(第一个方格的左上角)*/std::cout << "开始标定" << std::endl;//棋盘三维信息，设置棋盘在世界坐标系的坐标//实际测量得到标定板上每个棋盘格的大小cv::Size squareSize = cv::Size(26, 26);//毎幅图片角点数量std::vector<int> pointCounts;//保存标定板上角点的三维坐标std::vector<std::vector<cv::Point3f>> objectPoints;//摄像机内参数矩阵 M=[fx γ u0,0 fy v0,0 0 1]cv::Mat cameraMatrix = cv::Mat(3, 3, CV_64F, Scalar::all(0));//摄像机的5个畸变系数k1,k2,p1,p2,k3cv::Mat distCoeffs = cv::Mat(1, 5, CV_64F, Scalar::all(0));//每幅图片的旋转向量std::vector<cv::Mat> tvecsMat;//每幅图片的平移向量std::vector<cv::Mat> rvecsMat;//初始化标定板上角点的三维坐标int i, j, t;for (t = 0; t < imageCount; t++){std::vector<cv::Point3f> tempPointSet;//行数for (i = 0; i < boardSize.height; i++){//列数for (j = 0; j < boardSize.width; j++){cv::Point3f realPoint;//假设标定板放在世界坐标系中z=0的平面上。realPoint.x = i*squareSize.width;realPoint.y = j*squareSize.height;realPoint.z = 0;tempPointSet.push_back(realPoint);}}objectPoints.push_back(tempPointSet);}//初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板for (i = 0; i < imageCount; i++){pointCounts.push_back(boardSize.width*boardSize.height);}//开始标定cv::calibrateCamera(objectPoints, imagePointsSeq, imageSize, cameraMatrix, distCoeffs, rvecsMat, tvecsMat);std::cout << "标定完成" << std::endl;//对标定结果进行评价std::cout << "开始评价标定结果......" << std::endl;//所有图像的平均误差的总和double totalErr = 0.0;//每幅图像的平均误差double err = 0.0;//保存重新计算得到的投影点std::vector<cv::Point2f> imagePoints2;std::cout << "每幅图像的标定误差:" << std::endl;fout << "每幅图像的标定误差:" << std::endl;for (i = 0; i < imageCount; i++){std::vector<cv::Point3f> tempPointSet = objectPoints[i];//通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点imagePoints2（在像素坐标系下的点坐标）cv::projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, imagePoints2);//计算新的投影点和旧的投影点之间的误差std::vector<cv::Point2f> tempImagePoint = imagePointsSeq[i];cv::Mat tempImagePointMat = cv::Mat(1, tempImagePoint.size(), CV_32FC2);cv::Mat imagePoints2Mat = cv::Mat(1, imagePoints2.size(), CV_32FC2);for (int j = 0; j < tempImagePoint.size(); j++){imagePoints2Mat.at<cv::Vec2f>(0, j) = cv::Vec2f(imagePoints2[j].x, imagePoints2[j].y);tempImagePointMat.at<cv::Vec2f>(0, j) = cv::Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);}//Calculates an absolute difference norm or a relative difference norm.err = cv::norm(imagePoints2Mat, tempImagePointMat, NORM_L2);totalErr += err /= pointCounts[i];std::cout << "  第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;fout<<  "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;}//每张图像的平均总误差std::cout << "  总体平均误差:" << totalErr / imageCount << "像素" << std::endl;fout << "总体平均误差:" << totalErr / imageCount << "像素" << std::endl;std::cout << "评价完成!" << std::endl;//保存标定结果std::cout << "开始保存标定结果....." << std::endl;//保存每张图像的旋转矩阵cv::Mat rotationMatrix = cv::Mat(3, 3, CV_32FC1, Scalar::all(0));fout << "相机内参数矩阵:" << std::endl;fout << cameraMatrix << std::endl << std::endl;fout << "畸变系数:" << std::endl;fout << distCoeffs << std::endl << std::endl;for (int i = 0; i < imageCount; i++){fout << "第" << i + 1 << "幅图像的旋转向量:" << std::endl;fout << tvecsMat[i] << std::endl;//将旋转向量转换为相对应的旋转矩阵cv::Rodrigues(tvecsMat[i], rotationMatrix);fout << "第" << i + 1 << "幅图像的旋转矩阵:" << std::endl;fout << rotationMatrix << std::endl;fout << "第" << i + 1 << "幅图像的平移向量:" << std::endl;fout << rvecsMat[i] << std::endl;}std::cout << "保存完成" << std::endl;/************************************************************************显示定标结果*************************************************************************/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();}//释放资源fin.close();fout.close();system("pause");        }
}

参考例2

#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <fstream>using namespace cv;
using namespace std;void main() 
{ifstream fin("calibdata.txt"); /* 标定所用图像文件的路径 */ofstream fout("caliberation_result.txt");  /* 保存标定结果的文件 */	//读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化	cout<<"开始提取角点………………";int image_count=0;  /* 图像数量 */Size image_size;  /* 图像的尺寸 */Size board_size = Size(4,6);    /* 标定板上每行、列的角点数 */vector<Point2f> image_points_buf;  /* 缓存每幅图像上检测到的角点 */vector<vector<Point2f>> image_points_seq; /* 保存检测到的所有角点 */string filename;int count= -1 ;//用于存储角点个数。while (getline(fin,filename)){image_count++;		// 用于观察检验输出cout<<"image_count = "<<image_count<<endl;		/* 输出检验*/cout<<"-->count = "<<count;		Mat imageInput=imread(filename);if (image_count == 1)  //读入第一张图片时获取图像宽高信息{image_size.width = imageInput.cols;image_size.height =imageInput.rows;			cout<<"image_size.width = "<<image_size.width<<endl;cout<<"image_size.height = "<<image_size.height<<endl;}/* 提取角点 */if (0 == findChessboardCorners(imageInput,board_size,image_points_buf)){			cout<<"can not find chessboard corners!\n"; //找不到角点exit(1);} else {Mat view_gray;cvtColor(imageInput,view_gray,CV_RGB2GRAY);/* 亚像素精确化 */find4QuadCornerSubpix(view_gray,image_points_buf,Size(5,5)); //对粗提取的角点进行精确化image_points_seq.push_back(image_points_buf);  //保存亚像素角点/* 在图像上显示角点位置 */drawChessboardCorners(view_gray,board_size,image_points_buf,true); //用于在图片中标记角点imshow("Camera Calibration",view_gray);//显示图片waitKey(500);//暂停0.5S		}}int total = image_points_seq.size();cout<<"total = "<<total<<endl;int CornerNum=board_size.width*board_size.height;  //每张图片上总的角点数for (int ii=0 ; ii<total ;ii++){if (0 == ii%CornerNum)// 24 是每幅图片的角点个数。此判断语句是为了输出 图片号，便于控制台观看 {	int i = -1;i = ii/CornerNum;int j=i+1;cout<<"--> 第 "<<j <<"图片的数据 --> : "<<endl;}if (0 == ii%3)	// 此判断语句，格式化输出，便于控制台查看{cout<<endl;}else{cout.width(10);}//输出所有的角点cout<<" -->"<<image_points_seq[ii][0].x;cout<<" -->"<<image_points_seq[ii][0].y;}	cout<<"角点提取完成！\n";//以下是摄像机标定cout<<"开始标定………………";/*棋盘三维信息*/Size square_size = Size(10,10);  /* 实际测量得到的标定板上每个棋盘格的大小 */vector<vector<Point3f>> object_points; /* 保存标定板上角点的三维坐标 *//*内外参数*/Mat cameraMatrix=Mat(3,3,CV_32FC1,Scalar::all(0)); /* 摄像机内参数矩阵 */vector<int> point_counts;  // 每幅图像中角点的数量Mat distCoeffs=Mat(1,5,CV_32FC1,Scalar::all(0)); /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */vector<Mat> tvecsMat;  /* 每幅图像的旋转向量 */vector<Mat> rvecsMat; /* 每幅图像的平移向量 *//* 初始化标定板上角点的三维坐标 */int i,j,t;for (t=0;t<image_count;t++) {vector<Point3f> tempPointSet;for (i=0;i<board_size.height;i++) {for (j=0;j<board_size.width;j++) {Point3f realPoint;/* 假设标定板放在世界坐标系中z=0的平面上 */realPoint.x = i*square_size.width;realPoint.y = j*square_size.height;realPoint.z = 0;tempPointSet.push_back(realPoint);}}object_points.push_back(tempPointSet);}/* 初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板 */for (i=0;i<image_count;i++){point_counts.push_back(board_size.width*board_size.height);}	/* 开始标定 */calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);cout<<"标定完成！\n";//对标定结果进行评价cout<<"开始评价标定结果………………\n";double total_err = 0.0; /* 所有图像的平均误差的总和 */double err = 0.0; /* 每幅图像的平均误差 */vector<Point2f> image_points2; /* 保存重新计算得到的投影点 */cout<<"\t每幅图像的标定误差：\n";fout<<"每幅图像的标定误差：\n";for (i=0;i<image_count;i++){vector<Point3f> tempPointSet=object_points[i];/* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */projectPoints(tempPointSet,rvecsMat[i],tvecsMat[i],cameraMatrix,distCoeffs,image_points2);/* 计算新的投影点和旧的投影点之间的误差*/vector<Point2f> tempImagePoint = image_points_seq[i];Mat tempImagePointMat = Mat(1,tempImagePoint.size(),CV_32FC2);Mat image_points2Mat = Mat(1,image_points2.size(), CV_32FC2);for (int j = 0 ; j < tempImagePoint.size(); j++){image_points2Mat.at<Vec2f>(0,j) = Vec2f(image_points2[j].x, image_points2[j].y);tempImagePointMat.at<Vec2f>(0,j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);}err = norm(image_points2Mat, tempImagePointMat, NORM_L2);total_err += err/=  point_counts[i];   std::cout<<"第"<<i+1<<"幅图像的平均误差："<<err<<"像素"<<endl;   fout<<"第"<<i+1<<"幅图像的平均误差："<<err<<"像素"<<endl;   }   std::cout<<"总体平均误差："<<total_err/image_count<<"像素"<<endl;   fout<<"总体平均误差："<<total_err/image_count<<"像素"<<endl<<endl;   std::cout<<"评价完成！"<<endl;  //保存定标结果  	std::cout<<"开始保存定标结果………………"<<endl;       Mat rotation_matrix = Mat(3,3,CV_32FC1, Scalar::all(0)); /* 保存每幅图像的旋转矩阵 */fout<<"相机内参数矩阵："<<endl;   fout<<cameraMatrix<<endl<<endl;   fout<<"畸变系数：\n";   fout<<distCoeffs<<endl<<endl<<endl;   for (int i=0; i<image_count; i++) { fout<<"第"<<i+1<<"幅图像的旋转向量："<<endl;   fout<<tvecsMat[i]<<endl;   /* 将旋转向量转换为相对应的旋转矩阵 */   Rodrigues(tvecsMat[i],rotation_matrix);   fout<<"第"<<i+1<<"幅图像的旋转矩阵："<<endl;   fout<<rotation_matrix<<endl;   fout<<"第"<<i+1<<"幅图像的平移向量："<<endl;   fout<<rvecsMat[i]<<endl<<endl;   }   std::cout<<"完成保存"<<endl; fout<<endl;/************************************************************************  显示定标结果  *************************************************************************/Mat mapx = Mat(image_size,CV_32FC1);Mat mapy = Mat(image_size,CV_32FC1);Mat R = Mat::eye(3,3,CV_32F);std::cout<<"保存矫正图像"<<endl;string imageFileName;std::stringstream StrStm;for (int i = 0 ; i != image_count ; i++){std::cout<<"Frame #"<<i+1<<"..."<<endl;initUndistortRectifyMap(cameraMatrix,distCoeffs,R,cameraMatrix,image_size,CV_32FC1,mapx,mapy);		StrStm.clear();imageFileName.clear();string filePath="chess";StrStm<<i+1;StrStm>>imageFileName;filePath+=imageFileName;filePath+=".bmp";Mat imageSource = imread(filePath);Mat newimage = imageSource.clone();//另一种不需要转换矩阵的方式//undistort(imageSource,newimage,cameraMatrix,distCoeffs);remap(imageSource,newimage,mapx, mapy, INTER_LINEAR);imshow("原始图像",imageSource);imshow("矫正后图像",newimage);waitKey();StrStm.clear();filePath.clear();StrStm<<i+1;StrStm>>imageFileName;imageFileName += "_d.jpg";imwrite(imageFileName,newimage);}std::cout<<"保存结束"<<endl;	return ;
}

参考博客：

1. 双目视觉之相机标定
2. 双目视觉标定程序讲解