基于opencv进行双目相机的标定_opencv双目标定

前言

基于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
3
4
5
6
7
8
9
10
11
12
13

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65

需要修改的一些地方：

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

编译好之后运行命令

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

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");
1
2
3
4
5
6
7

双目标定代码文件

#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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269

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})
1
2
3
4
5
6
7
8
9
10

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. ]
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82