双目摄像头的标定和测距（一）

运行环境	Ubuntu 18.04 LTS
ROS version	Melodic
OpenCV1 version	3.2.0
gcc version	7.5.0

摄像头测试

若要使用以下代码需要更改代码中双目摄像头的描述符。
并且需要用到一个 camera.sh 脚本，该脚本是商家提供的。

// Test.cpp
#include 
#include 

using namespace cv;
using namespace std;

int main()
{
    VideoCapture Camera(4);   //需要改成你系统中双目摄像头的标识符
    if (!Camera.isOpened())
    {
        cout << "Could not open the Camera " << endl;
        return -1;
    }
    Mat Fream;
    Camera >> Fream;
    imshow("Double",Fream);
    Mat DoubleImage;
    system("/home/oision/camera.sh");  //此处改成你的脚本存放绝对路径
    //imshow("【双目视图】",Fream);
    while (true)
    {
        Camera >> Fream;
        if (Fream.empty()) break;
        resize(Fream, DoubleImage, Size(640, 240), (0, 0), (0, 0), INTER_AREA);
        imshow("Double", DoubleImage);
        Mat LeftImage = DoubleImage(Rect(0, 0, 320, 240));
        Mat RightImage = DoubleImage(Rect(320, 0, 320, 240));

        imshow("Left", LeftImage);
        imshow("Right", RightImage);
        char key = waitKey(30);
        char c = cvWaitKey(30);
        if (c == 27)//Esc键退出
        {
            break;
        }
    }
    return 0;
}

编译及运行命令：

g++ Test.cpp -o test `pkg-config --cflags --libs opencv`
./test

运行效果：

摄像头标定

这部分分为拍照和标定获取参数两个部分。这部分程序运行结束后，会生成一个 intrinsics.yml² 文件，具体内容（部分）如下图所示：

图像获取

采用以下代码对标定版图片进行不同角度、不同距离的信息采集。按下 ESC 键将图像保存至设定的路径。

// Capturer.cpp
#include 
#include 

using namespace cv;
using namespace std;
		
int main()
{
    VideoCapture Camera(4);  //需要改成你系统中双目摄像头的标识符
    if (!Camera.isOpened())
    {
        cout << "Could not open the Camera" << endl;
        return -1;
    }
    Mat Fream;
    Camera >> Fream;
    imshow("Double",Fream);
    system("~/camera.sh");  //此处改成你的脚本存放绝对路径

    //setMouseCallback("Double", on_Mouse);
    Mat DoubleImage;
    char buf[50] = { 0 };

    while (true) {
        Camera >> Fream;
        if (Fream.empty()) break;
        resize(Fream, DoubleImage, Size(640, 240), (0, 0), (0, 0), INTER_AREA);
        imshow("Double", DoubleImage);

        Mat LeftImage = DoubleImage(Rect(0, 0, 320, 240));
        Mat RightImage = DoubleImage(Rect(320, 0, 320, 240));
        imshow("Left", LeftImage);
        imshow("Right", RightImage);
        char key = waitKey(30);

	static int i = 0;
	if (key == 27) {
		// imwrite("/home/oision/camera_label/test.png", LeftImage);
		// 设置保存路径
		sprintf(buf, "/home/oision/camera_label/left_%d.png", i);  //存储地址要写绝对路径
		cout << buf << endl;
		imwrite(buf, LeftImage);

		sprintf(buf, "/home/oision/camera_label/right_%d.png", i);
		cout << buf << endl;
		imwrite(buf, RightImage);

		sprintf(buf, "/home/oision/camera_label/doub_%d.png", i);
		cout << buf << endl;
		imwrite(buf, DoubleImage);
		i++;
		}
    }
    return 0;
}

摄像头标定

首先编写 stereo_calibration.xml 文件，内容如下（请根据具体情况修改图片路径）：

<opencv_storage>
<images>
"/home/oision/camera_label/left_0.png" 
"/home/oision/camera_label/right_0.png" 
"/home/oision/camera_label/left_1.png"
"/home/oision/camera_label/right_1.png" 
"/home/oision/camera_label/left_2.png" 
"/home/oision/camera_label/right_2.png" 
"/home/oision/camera_label/left_3.png" 
"/home/oision/camera_label/right_3.png" 
"/home/oision/camera_label/left_4.png" 
"/home/oision/camera_label/right_4.png" 
"/home/oision/camera_label/left_5.png" 
"/home/oision/camera_label/right_5.png" 
"/home/oision/camera_label/left_6.png" 
"/home/oision/camera_label/right_6.png" 
"/home/oision/camera_label/left_7.png" 
"/home/oision/camera_label/right_7.png" 
"/home/oision/camera_label/left_8.png" 
"/home/oision/camera_label/right_8.png" 
images>
opencv_storage>

标定程序运行开始后会从 stereo_calibration.xml 文件获取采集到的图片路径，依次读取图片进行标定。
标定代码：

// Calibration.cpp
#if 1
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

// 此处参数需要根据棋盘格个数修改    
// 例如 黑白棋盘格 宽（w）为10个棋盘格 那么 w 为 10 -1 = 9

#define  w  9     // 棋盘格宽的黑白交叉点个数  
#define  h  6     // 棋盘格高的黑白交叉点个数   
const  float chessboardSquareSize = 24.6f;  // 每个棋盘格方块的边长 单位 为 mm

using namespace std;
using namespace cv;

// 从 xml 文件中读取图片存储路径 
static bool readStringList(const string& filename, vector<string>& list)
{
	list.resize(0);
	FileStorage fs(filename, FileStorage::READ);
	if (!fs.isOpened())
		return false;
	FileNode n = fs.getFirstTopLevelNode();
	if (n.type() != FileNode::SEQ)
		return false;
	FileNodeIterator it = n.begin(), it_end = n.end();
	for (; it != it_end; ++it)
		list.push_back((string)*it);
	return true;
}

// 记录棋盘格角点个数
static void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners)
{
	corners.resize(0);
	for (int i = 0; i < boardSize.height; i++)        //height和width位置不能颠倒
		for (int j = 0; j < boardSize.width; j++)
		{
			corners.push_back(Point3f(j*squareSize, i*squareSize, 0));
		}
}


bool calibrate(Mat& intrMat, Mat& distCoeffs, vector<vector<Point2f>>& imagePoints,
	vector<vector<Point3f>>& ObjectPoints, Size& imageSize, const int cameraId,
	vector<string> imageList)
{

	double rms = 0;  //重投影误差

	Size boardSize;
	boardSize.width = w;
	boardSize.height = h;  

	vector<Point2f> pointBuf;
	float squareSize = chessboardSquareSize;

	vector<Mat> rvecs, tvecs; // 定义两个摄像头的旋转矩阵 和平移向量

	bool ok = false;

	int nImages = (int)imageList.size() / 2;
	cout <<"图片张数"<< nImages;
	namedWindow("View", 1);

	int nums = 0;  // 有效棋盘格图片张数

	for (int i = 0; i< nImages; i++)
	{
		Mat view, viewGray;
		cout<<"Now: "<<imageList[i * 2 + cameraId]<<endl;
		view = imread(imageList[i * 2 + cameraId], 1); // 读取图片
		imageSize = view.size();
		cvtColor(view, viewGray, COLOR_BGR2GRAY); // 转化成灰度图

		bool found = findChessboardCorners(view, boardSize, pointBuf,
			CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);//寻找棋盘格角点
		if (found)
		{
			nums++;
			cornerSubPix(viewGray, pointBuf, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
			drawChessboardCorners(view, boardSize, Mat(pointBuf), found);
			bitwise_not(view, view);
			imagePoints.push_back(pointBuf);
			cout << '.';
		}
		else{
			cout<<"Wrong"<<endl;
		}
		imshow("View", view);
		waitKey(100);
	}

	cout << "有效棋盘格张数" << nums << endl;

	// calculate chessboardCorners
	calcChessboardCorners(boardSize, squareSize, ObjectPoints[0]);
	ObjectPoints.resize(imagePoints.size(), ObjectPoints[0]);

	rms = calibrateCamera(ObjectPoints, imagePoints, imageSize, intrMat, distCoeffs,
		rvecs, tvecs);
	ok = checkRange(intrMat) && checkRange(distCoeffs);

	if (ok)
	{
		cout << "done with RMS error=" << rms << endl;
		return true;
	}
	else
		return false;
}

int main()
{
	// initialize some parameters
	bool okcalib = false;
	Mat intrMatFirst, intrMatSec, distCoeffsFirst, distCoffesSec;
	Mat R, T, E, F, RFirst, RSec, PFirst, PSec, Q;
	vector<vector<Point2f>> imagePointsFirst, imagePointsSec;
	vector<vector<Point3f>> ObjectPoints(1);
	Rect validRoi[2];
	Size imageSize;
	int cameraIdFirst = 0, cameraIdSec = 1;
	double rms = 0;

	// get pictures and calibrate
	vector<string> imageList;
	string filename = "stereo_calibration.xml";
	bool okread = readStringList(filename, imageList);
	if (!okread || imageList.empty())
	{
		cout << "can not open " << filename << " or the string list is empty" << endl;
		return false;
	}
	if (imageList.size() % 2 != 0)
	{
		cout << "Error: the image list contains odd (non-even) number of elements\n";
		return false;
	}

	FileStorage fs("intrinsics.yml", FileStorage::WRITE);
	// calibrate

	cout << "calibrate left camera..." << endl;
	okcalib = calibrate(intrMatFirst, distCoeffsFirst, imagePointsFirst, ObjectPoints,
		imageSize, cameraIdFirst, imageList);

	if (!okcalib)
	{
		cout << "fail to calibrate left camera" << endl;
		return -1;
	}
	else
	{
		cout << "calibrate the right camera..." << endl;
	}


	okcalib = calibrate(intrMatSec, distCoffesSec, imagePointsSec, ObjectPoints,
		imageSize, cameraIdSec, imageList);

	fs << "M1" << intrMatFirst << "D1" << distCoeffsFirst <<
		"M2" << intrMatSec << "D2" << distCoffesSec;

	if (!okcalib)
	{
		cout << "fail to calibrate the right camera" << endl;
		return -1;
	}
	destroyAllWindows();

	// estimate position and orientation
	cout << "estimate position and orientation of the second camera" << endl
		<< "relative to the first camera..." << endl;
	cout << "intrMatFirst:";
	cout << intrMatFirst << endl;
	cout << "distCoeffsFirst:";
	cout << distCoeffsFirst << endl;
	cout << "intrMatSec:";
	cout << intrMatSec << endl;
	cout << "distCoffesSec:";
	cout << distCoffesSec << endl;

	rms = stereoCalibrate(ObjectPoints, imagePointsFirst, imagePointsSec,
		intrMatFirst, distCoeffsFirst, intrMatSec, distCoffesSec,
		imageSize, R, T, E, F, CALIB_USE_INTRINSIC_GUESS,//CV_CALIB_FIX_INTRINSIC,
		TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 1e-6));          //计算重投影误差
	cout << "done with RMS error=" << rms << endl;

	// stereo rectify
	cout << "stereo rectify..." << endl;
	stereoRectify(intrMatFirst, distCoeffsFirst, intrMatSec, distCoffesSec, imageSize, R, T, RFirst,
		RSec, PFirst, PSec, Q, CALIB_ZERO_DISPARITY, -1, imageSize, &validRoi[0], &validRoi[1]);
	cout << "Q" << Q << endl;
	cout << "P1" << PFirst << endl;
	cout << "P2" << PSec << endl;
	// read pictures for 3d-reconstruction

	if (fs.isOpened())
	{
		cout << "in";
		fs << "R" << R << "T" << T << "R1" << RFirst << "R2" << RSec << "P1" << PFirst << "P2" << PSec << "Q" << Q;
		fs.release();
	}

	namedWindow("canvas", 1);
	cout << "read the picture for 3d-reconstruction..."<<endl;;
	Mat canvas(imageSize.height, imageSize.width * 2, CV_8UC3), viewLeft, viewRight;
	Mat canLeft = canvas(Rect(0, 0, imageSize.width, imageSize.height));
	Mat canRight = canvas(Rect(imageSize.width, 0, imageSize.width, imageSize.height));

	viewLeft = imread(imageList[6], 1);  // cameraIdFirst
	viewRight = imread(imageList[7], 1); // cameraIdSec
	cout<<"Choose: "<<imageList[6]<<"  "<<imageList[7]<<endl;
	viewLeft.copyTo(canLeft);
	viewRight.copyTo(canRight);
	cout << "done" << endl;
	imshow("canvas", canvas);
	waitKey(1500); // 必须要加waitKey ，否则可能存在无法显示图像问题
	

	//stereoRectify
	Mat rmapFirst[2], rmapSec[2], rviewFirst, rviewSec;
	initUndistortRectifyMap(intrMatFirst, distCoeffsFirst, RFirst, PFirst,
		imageSize, CV_16SC2, rmapFirst[0], rmapFirst[1]);//CV_16SC2
	initUndistortRectifyMap(intrMatSec, distCoffesSec, RSec, PSec,//CV_16SC2
		imageSize, CV_16SC2, rmapSec[0], rmapSec[1]);
	remap(viewLeft, rviewFirst, rmapFirst[0], rmapFirst[1], INTER_LINEAR);
	imshow("remap", rviewFirst);
	waitKey(2000);

	remap(viewRight, rviewSec, rmapSec[0], rmapSec[1], INTER_LINEAR);
	rviewFirst.copyTo(canLeft);
	rviewSec.copyTo(canRight);

	//rectangle(canLeft, validRoi[0], Scalar(255, 0, 0), 3, 8);
	//rectangle(canRight, validRoi[1], Scalar(255, 0, 0), 3, 8);

	Mat before_rectify = imread("/home/oision/camera_label/doub_0.png");
	// 地址改为保存的双目图像
	for (int j = 0; j <= canvas.rows; j += 16)  //画绿线
		line(canvas, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8);
	for (int j = 0; j <= canvas.rows; j += 16)  //画绿线
		line(before_rectify, Point(0, j), Point(canvas.cols, j), Scalar(0, 255, 0), 1, 8);
	cout << "stereo rectify done" << endl;

	imshow("Before", before_rectify); //显示画绿线的校正后图像
	imshow("After", canvas); //显示画绿线的校正前图像

	waitKey(400000);
	//官方解释   http://masikkk.com/article/OpenCV-imshow-waitkey/ 
	/*   http://masikkk.com/article/OpenCV-imshow-waitkey/ 
	A common mistake for OpenCV newcomers is to call cv::imshow() in a loop through video frames,
	without following up each draw with cv::waitKey(30).In this case, nothing appears on screen,
	because highgui is never given time to process the draw requests from cv::imshow().
	*/
	return 0;
}
#endif

过程出现类似于下图的窗口及代表该图片有效：

代码执行结束会显示 remap 后的图像及得到我们需要的 intrinsics.yml 文件。

摄像头测距

待补充–

---

1. OpenCV 为安装 ROS 时默认安装的版本，具体安装教程可搜索其他相关文章。 ↩︎

2. 这个文件里保存了摄像头的参数：左右内参矩阵、左右畸变参数、平移向量、旋转向量等。 ↩︎