前言

基于opencv的标定是早期的尝试，但是标定精度一般，最后发现基于ROS自带的标定工具包得到的参数较为准确。

1 数据准备

为了保证相机左右目图像能够一一对应，选择录制bag文件然后将bag文件转成图片的形式，从转成的图像中挑选位置较好的图像。

1.1 bag文件转图像的命令

# 安装图像处理的package
sudo apt-get install mjepgtools
sudo apt-get install ffmpeg

# 新建文件夹用于储存提取后的图片
mkdir -p images/left images/right
# <IMAGETOPICINBAGFILE>为bag文件中储存图片的topic
# 左右目的图片分开，在提取图片时需要分别在left和right两个文件夹下打开终端运行以下两个命令
# https://www.cnblogs.com/arkenstone/p/6676203.html
rosrun image_view extract_images _sec_per_frame:=0.01 image:=/cam_stereo_left/csi_cam/image_raw
rosrun image_view extract_images _sec_per_frame:=0.01 image:=/cam_stereo_right/csi_cam/image_raw
# 在images目录下播放数据集
rosbag play calib_stereo.bag

1.2 给挑选出来的图像重命名

为了方便下一步生成图像路径的配置文件，提前给图像重命名，比较简单
参考博客：《ubuntu不需要代码直接使用自带的文件管理器对文件批量重命名》

1.3 挑选图像并生成图像路径文件imagepath.xml

xml文件格式

一个左目对应一个右目，可以自己写一个python脚本，也很快，这样也不用1.1的重命名了，也可以用下面的一段代码
代码文件

/*this creates a yaml or xml list of files from the command line args
 * 由图片文件夹生成 图片文件路径 xml文件
 *  示例用法 ./imagelist_creator wimagelist.yaml ../t/*JPG
 */
 
#include "opencv2/core/core.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <string>
#include <iostream>
 
using std::string;
using std::cout;
using std::endl;
 
using namespace cv;
 
static void help(char** av)//argv参数
{
  cout << "\nThis creates a yaml or xml list of files from the command line args\n"
      "用法 usage:\n./" << av[0] << " imagelist.yaml *.png\n"  //命令行  用法
      << "Try using different extensions.(e.g. yaml yml xml xml.gz etc...)\n"
      << "This will serialize this list of images or whatever with opencv's FileStorage framework" << endl;
}
 
int main(int argc, char** argv)
{
  cv::CommandLineParser parser(argc, argv, "{help h||}{@output||}");//解析参数  得到帮助参数 和 输出文件名
  if (parser.has("help"))//有帮助参数
  {
    help(argv);//显示帮助信息
    return 0;
  }
  string outputname = parser.get<string>("@output");
 
  if (outputname.empty())
  {
    help(argv);
    return 1;
  }
 
  Mat m = imread(outputname); //check if the output is an image - prevent overwrites!
  if(!m.empty()){
    std::cerr << "fail! Please specify an output file, don't want to overwrite you images!" << endl;
    help(argv);
    return 1;
  }
 
  FileStorage fs(outputname, FileStorage::WRITE);//opencv 读取文件类
  fs << "images" << "[";//文件流重定向
  int FrameCount = 1;
  while(FrameCount < 46)
  {
      char filenamel[100];
      char filenamer[100];
      sprintf(filenamel, "../images/left/%02d.jpg", FrameCount);
      sprintf(filenamer, "../images/right/%02d.jpg", FrameCount);
      fs << string(filenamel);
      fs << string(filenamer);
      FrameCount++;

  }
  fs << "]";
  return 0;
}

需要修改的一些地方：

# 改一下图像路径
sprintf(filenamel, "../images/left/%02d.jpg", FrameCount);
# 改一下图片数量
while(FrameCount < 46)

编译好之后运行命令

./create_xml ../imagepath.xml ../images/left/*jpg ../images/right/*jpg

2 双目标定

需要修改的几个地方：

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

# 矫正示例图片路径
Mat before_rectify = imread("/home/lusx/Demos/camera_calibration/data/images0620/left/09.jpg");

双目标定代码文件

#if 1
#include <iostream>
#include <stdio.h>
#include <time.h>
#include <iostream>
#include <stdio.h>
#include <string.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/core.hpp>
#include <stdlib.h>

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

#define  w  8     //棋盘格宽的黑白交叉点个数  
#define  h  6     //棋盘格高的黑白交叉点个数   
const  float chessboardSquareSize = 100.0f;  //每个棋盘格方块的边长 单位 为 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(int argc, char** argv)
{
	//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 = argv[1];
	bool okread = readStringList(filename.c_str(), 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_of_cali2.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; 
    cout << "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/lusx/Demos/camera_calibration/data/images0620/left/09.jpg");

	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);//必须要加waitKey ，否则可能存在无法显示图像问题


	return 0;
}
#endif

CMakeLists.txt

cmake_minimum_required(VERSION 3.2)

project(stereo_cali)

find_package(OpenCV 3.2 REQUIRED)

set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)

add_executable(stereo_calibration src/stereo_calibration.cpp)
target_link_libraries(stereo_calibration ${OpenCV_LIBS})

3 标定结果

%YAML:1.0
---
M1: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 6.7120059258745164e+02, 0., 6.4616896917541999e+02, 0.,
       6.7182565194969800e+02, 3.5348837759675257e+02, 0., 0., 1. ]
D1: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ -3.7333810947349610e-01, 1.6586194835872931e-01,
       3.5379352034843424e-04, 5.9527737207914491e-04,
       -3.8535447083818362e-02 ]
M2: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 6.3370135938776207e+02, 0., 5.9653984913805789e+02, 0.,
       6.3409704988811575e+02, 3.3945329672380274e+02, 0., 0., 1. ]
D2: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ -3.5144110493729563e-01, 1.5946409506013970e-01,
       3.1912357423557504e-04, -1.0822811432872103e-03,
       -3.9066365121157749e-02 ]
R: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 9.9954163867472001e-01, 1.0492054717228604e-02,
       -2.8397699612224728e-02, -1.0075423829223454e-02,
       9.9984008231756527e-01, 1.4774830823595410e-02,
       2.8548176651355221e-02, -1.4481939753190173e-02,
       9.9948750619048154e-01 ]
T: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [ -3.8858225802085644e+02, -4.8434327404429807e+00,
       -2.9152922764996365e+01 ]
R1: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 9.9867388066592999e-01, 2.1804972001696343e-02,
       4.6637144763132091e-02, -2.2158414538435135e-02,
       9.9972943797383940e-01, 7.0749921310147941e-03,
       -4.6470256517421656e-02, -8.0990150337119683e-03,
       9.9888684104591585e-01 ]
R2: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 9.9712051851405958e-01, 1.2428478310194405e-02,
       7.4807783586644541e-02, -1.1859955513830773e-02,
       9.9989735078872921e-01, -8.0392375814399373e-03,
       -7.4900020116573901e-02, 7.1288717602372006e-03,
       9.9716556607915563e-01 ]
P1: !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [ 3.2923671518196784e+02, 0., 5.2566915512084961e+02, 0., 0.,
       3.2923671518196784e+02, 3.5972601759433746e+02, 0., 0., 0., 1.,
       0. ]
P2: !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [ 3.2923671518196784e+02, 0., 5.2566915512084961e+02,
       -1.2830499807529009e+05, 0., 3.2923671518196784e+02,
       3.5972601759433746e+02, 0., 0., 0., 1., 0. ]
Q: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 1., 0., 0., -5.2566915512084961e+02, 0., 1., 0.,
       -3.5972601759433746e+02, 0., 0., 0., 3.2923671518196784e+02, 0.,
       0., 2.5660474659667575e-03, 0. ]