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

    文章目录

    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
  • 相关阅读:
    nodejs worker_threads的事件监听问题
    Jenkins汉化设置
    SpringBoot健身房教练课程预约管理系统java
    小满nestjs（第七章 RESTful 风格设计）
    C语言详解系列——数组详解，一维数组、二维数组
    Hive-DDL-常用操作命令（数据库操作、表操作、分区操作、show语法）
    【OpenCV 例程300篇】250. 梯度算子的传递函数
    c语言练习89：链表的使用
    ARM-A架构入门基础（三）MMU
    debian无法使用root用户登陆系统
  • 原文地址：https://blog.csdn.net/guanjing_dream/article/details/125520636