基于OPENCV的单目测距

 

    最近研究了一下单目测距,关于单目测距的原理有各位大神的讲解,这里只写一些自已使用上的记录,使用环境为windows10+opencv3.1+vs2015。

    买了一个摄像头(笔记本的定焦摄像头也可以),不知道具体参数,想用它实现测距功能。

    原理上就是根据三角性的相似性,假设摄像头焦距为f,摄像头距物体距离为dmm,物体在图像中的尺寸为p个像素(假设物体水平放置,为水平上的长度),物体实际长度为xmm, 根据三角形相似性,有

                                                                                                          \frac{f}{d}=\frac{p}{x}    (式1)

    显然,这里的焦距是以图像像素为单位的。

    这里我们可以以棋盘纸为标准,用于计算相关参数,我所画的棋盘纸是在A4纸上打印的单元格为为20m*20mm,7行*10列个格,即boardSize为9*6。

    显然,现在焦距f是未知的,为了计算f,我们可以在距摄像头距离d的位置横放一张带有棋盘的A4纸。

     首先,我们估计一下摄像头的焦距,调整A4纸的位置,使的横放的A4纸的一边刚好填满图像的横坐标,测量此时A4纸距摄像头的距离d1 = 300mm,因为A4纸的尺寸是固定的为210mm*297mm,若图像尺寸为640*480,则也就是将297mm的宽度填满图像中的640像素,这样就有

                                                                                                     \frac{f}{d1}=\frac{640}{297}     (式2)

    于是可以计算出焦距f。这里的值只是个大概估计值,可以用来后来用棋盘纸校准的时候相对照估量。

    下面使用棋盘纸进行单目校准,关于棋盘纸的单目校准原理也有很多大神讲解,下面贴出来的仅为部分代码。里面缺少类成员函数声明以及相关头文件。

  •     1、使用opencv获取图像,并保存。
//打开摄像头
bool CCamera::OpenCamera(int index, VideoCapture &cap)
{

	if (cap.isOpened())
	{
		std::cout << "警告!摄像头"<<index<<"已经被打开!" << std::endl;
	}

	if (cap.open(index))
	{

		cout << "摄像头" << index << "打开成功!" << endl;
	}
	else
	{
		cout << "Error!摄像头" << index << "打开失败." << endl;
		return false;
	}

	return true;

}

 

//获取图像
void CCamera::GetCameraImage(VideoCapture cap, Mat &img)
{
	if (!cap.isOpened())
	{
		cout << "摄像头未打开!" << endl;
		return;
	}

	cap >> img;


}
//保存图像
int CCamera::SaveCameraImage(VideoCapture cap, int nGroup)
{

	Mat img;

	string imgName;
	int index = 1;

	cout << "--------操作指南--------" << endl;
	cout << "按'q'或'Q'退出图像采集\n";
	cout << "按's'或'S'保存当前采集图像\n";
	cout << "按'd'或'D'删除上一张采集图像\n";
	cout << "--------------------------" << endl;
	cout << "保存图像路径为:";
	system("CD");

	do
	{
		GetCameraImage(cap, img);
		imshow("image", img);
		char c = (char)waitKey(100);
		if (c == 's' || c == 'S')
		{
			char str[2];
			sprintf(str, "%03d", index);
			imgName = string("single") + str + string(".jpg");
			imwrite(imgName, img);
			index++;
			cout << "保存文件:" << imgName << endl;
		}
		else if (c == 'd' || c == 'D')
		{

			if (index >= 1)
			{
				char str[2];
				sprintf(str, "%03d", index - 1);
				imgName = string("single") + str + string(".jpg");
				remove(imgName.data());
				cout << "删除文件:" << imgName << endl;
				index--;
			}
			else
			{
				cout << "错误!未保存图像,不能删除" << endl;
			}


		}
		else if (c == 'q' || c == 'Q')
		{
			exit(-1);
		}
	} while (index <= nGroup);

	cout << "保存图像成功" << endl;

	return index;

}

    获取棋盘纸图像时用来校准时,应尽量保证棋盘纸面没有皱褶,并且棋盘纸在摄像头前有多个姿态,距离也应有一些变化。具体可参见opencv官方例程{opencv_dir}/soruces/samples/data/ 下的left01.jpg~left14.jpg的示例,棋盘纸格数不能太少,图像组数应有十几组,具体参数可以参考《learning opencv3》一书中P665中Camera Calibration中的讲述。

    主程序中可以如下写:

    VideoCapture cap;
    Mat img;

    if (camera.OpenCamera(0, cap))
    {
    	camera.SaveCameraImage(cap, 13);		
    }

    保存完成后我们可以在当前工程目录下找到所保存的图片, 这样我们就可以利用这些图片进行校准了。

  • 2 、进行校准,获取摄像机内参数矩阵。

    主程序中可以这样写:

    vector<string> singleFileName = { "single001.jpg", "single002.jpg", "single003.jpg", "single004.jpg", "single005.jpg", "single006.jpg", "single007.jpg", "single008.jpg",
		"single009.jpg", "single010.jpg", "single011.jpg", "single012.jpg", "single013.jpg", };

    camera.CameraCalibrate(singleFileName, boardSize, false);

 

//相机校准
void CCamera::CameraCalibrate(const vector<string> imgList, Size boardSize, bool displayCorners)
{
	assert(!imgList.empty());

	Size imageSize;

	vector<vector<Point2f> > imagePoints;
	vector<vector<Point3f> > objectPoints;

	const int maxScale = 2;
	const float squareSize = 20.f;

	int nimages = imgList.size();
	if (nimages < 3)
	{
		cout << "错误!图像组过少." << endl;

	}
	imagePoints.resize(nimages);
	vector<string> goodImageList;

	int i, j, k;


	//读入所有图像

	for (i = 0, j = 0; i < imgList.size(); i++)
	{

		string imgName = imgList[i];

		Mat img = imread(imgName, 0);
		if (img.empty())
		{
			cout << "错误!图像" << imgName << "不存在.跳过该组." << endl;
			break;
		}

		if (imageSize == Size())
		{
			imageSize = img.size();
		}
		else if (img.size() != imageSize)
		{
			cout << "错误!图像" << imgName << "大小不一致.跳过该组." << endl;
			break;
		}

		bool found = false;
		vector<Point2f>& corners = imagePoints[i];

		
		found = FindCornerPix(img, boardSize, corners, displayCorners);

		goodImageList.push_back(imgList[i]);
		j++;

	}

	cout << j << " pairs have been successfully detected.\n";
	nimages = j;
	if (nimages < 2)
	{
		cout << "Error: too little pairs to run the calibration\n";
		return;
	}

	imagePoints.resize(nimages);
	objectPoints.resize(nimages);

	for (i = 0; i < nimages; i++)                          //每个棋盘角点的世界坐标
	{
		for (j = 0; j < boardSize.height; j++)
			for (k = 0; k < boardSize.width; k++)
				objectPoints[i].push_back(Point3f(k*squareSize, j*squareSize, 0));
	}

	cout << "Running stereo calibration ...\n";

	Mat cameraMatrix, distCoeffs, R, T;
	//cameraMatrix = initCameraMatrix2D(objectPoints, imagePoints, imageSize, 0);//求机相机的内部参数

	double err = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, R, T, cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_PRINCIPAL_POINT);
	cout << "err:" << err << endl;

	showCalibrateData(cameraMatrix, "cameraMatrix");
	showCalibrateData(distCoeffs, "distCoeffs");
	showCalibrateData(R, "R");
	showCalibrateData(T, "T");

	stCameraParam.M1 = cameraMatrix.clone();
	stCameraParam.D1 = distCoeffs.clone();
	stCameraParam.R = R.clone();
	stCameraParam.T = T.clone();

	Mat map1, map2;
	initUndistortRectifyMap(cameraMatrix, distCoeffs, cv::Mat(), cameraMatrix, imageSize, CV_16SC2, map1, map2);

	stCameraParam.Map1 = map1.clone();
	stCameraParam.Map2 = map2.clone();

	cout << "校准完成.\n";
}
//获得图像的亚像素角点
bool CCamera::FindCornerPix(Mat img, Size boardSize, vector<Point2f>& corners, bool showImg /* = false */)
{
	if(img.channels() != 1)
	{
		//cout << "change color image to gray image.\n";
		cvtColor(img, img, COLOR_BGR2GRAY);
	}

	if (img.empty())
	{
		cout << "error! no image." << endl;
		return false;
	}
	if (boardSize == Size())
	{
		cout << "请输入棋盘角点数\n";
	}

	bool found = false;
	found = findChessboardCorners(img, boardSize, corners, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);
	

	if (showImg)
	{
		Mat cimg;
		cvtColor(img, cimg, COLOR_GRAY2BGR);
		drawChessboardCorners(cimg, boardSize, corners, found);
		imshow("chess board", cimg);
		waitKey(500);

	}
	

	if (!found)
		return false;
	cornerSubPix(img, corners, Size(11, 11), Size(-1, -1), TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 0.01));      //找到角点的亚像素点坐

	return true;



}
//显示参数
void CCamera::showCalibrateData(Mat data, string dataName)
{
	if (data.empty())
	{
		cout << "file" << dataName << " is empty.\n";
	}
	else
	{
		cout << dataName << " data is:\n";
		cout << data << endl;
	}

}

     

//相机参数结构体,因为还有双目测距的程序,里面的参数都在
typedef struct CAMERA_PARAM
{
	Mat M1;  //相机内部参数矩阵
	Mat D1;  //畸变向量
	Mat M2;  //相机内部参数矩阵
	Mat D2;  //畸变向量
	Mat R;  //R– 第一和第二相机坐标系之间的旋转矩阵。
	Mat T;  //T– 第一和第二相机坐标系之间的平移矩阵.
	Mat R1; //R1– 输出第一个相机的3x3矫正变换(旋转矩阵) .
	Mat R2; //R2– 输出第二个相机的3x3矫正变换(旋转矩阵) .
	Mat P1; //P1–在第一台相机的新的坐标系统(矫正过的)输出 3x4 的投影矩阵
	Mat P2; //P2–在第二台相机的新的坐标系统(矫正过的)输出 3x4 的投影矩阵
	Mat Q;  //4x4重投影矩阵
	Mat Map1;
	Mat Map2;

};



//成员变量
public:
	CAMERA_PARAM stCameraParam;

    程序中的squareSize 为棋盘单元格实际物理尺寸,单位mm,boardSize为棋盘角点数,一般是棋盘实际(row-1)*(col-1)。

    这样输出的M矩阵即是相机内参数矩阵,其形式为:

    基于OPENCV的单目测距

    其中,{f}_x为摄像头焦距,单位为图像像素;{c}_x,{c}_y

  •     3、使用校准的参数测距

    现在我们已经获得了相机的参数,可以用来测量一段已知长度或大小的物体距离摄像头的距离。

    这里依然使用棋盘纸,将棋盘纸放置在固定好摄像机前面,保证所有角点能落到摄像机图像里面,这样就会显示出距离参数。这里我是以水平方向的长度作为计量的,仅供参考。

    //此部分代码应紧接着上面的校准部分主程序代码使用
    float distance;
    while (1)
    {
    	camera.GetCameraImage(cap, img);
        camera.ShowRemapImage(img);
    	camera.CaluDistance(img, distance, Size(9, 6), Size(20, 20), true);
    }
//显示经过几何变换后的图像
void CCamera::ShowRemapImage(Mat img)
{
	assert(!img.empty());

	Mat img1;
	Mat map1, map2;
	map1 = stCameraParam.Map1;
	map2 = stCameraParam.Map2;
	//undistort(img, img1, cameraMatrix, distCoeffs);
	remap(img, img1, map1, map2, cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar());
	imshow("unsidistort", img1);
	waitKey(100);

}
//计算并显示距离
void CCamera::CaluDistance(Mat img, float & distance, const Size2d boardSize, const Size2f chessGridSize, bool displayCorners)
{
	assert(!img.empty());

	Size imageSize = img.size();

	bool found = false;
	vector<Point2f> corners;

	
	double f = stCameraParam.M1.ptr<double>(0)[0];



	found = FindCornerPix(img, boardSize, corners, displayCorners);

	if (found)
	{

#ifdef __DEBUG_MESSAGE__
		Mat img1;
		img1 = img.clone();
		for (int i = 0; i < corners.size(); i++)
		{
			cout << i << "\t" << corners[i] << endl;
			putText(img1, std::to_string(i), corners[i], FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 0, 0));
			imshow("img1", img1);
		}
#endif

		Mat dist;
		dist.create(6, 9, CV_32F);
		
		for (int i = 0; i < boardSize.height; i++)
		{
			float *data = dist.ptr<float>(i);
			for (int j = 0; j < boardSize.width - 1; j++)
			{
				float tdata = powf(corners[i * boardSize.width + j].x - corners[i*boardSize.width + j + 1].x, 2) + powf(corners[i * boardSize.width + j].y - corners[i*boardSize.width + j + 1].y, 2);
				data[j] = tdata;
				data[j] = 20./sqrtf(data[j])*f;
			}
		}
		system("CLS");
		cout << "distance:\n";
		cout << dist << endl;


	}

}

     以上 部分为本人作为测试用的相关代码,并未完善,作为备忘,仅供参考。

    由于刚接触这方面时间不长,难免有不知道的和理解不对的地方,如有错误,请批评指出,

 

 

 

文章出处登录后可见!

已经登录?立即刷新

共计人评分,平均

到目前为止还没有投票!成为第一位评论此文章。

(0)
青葱年少的头像青葱年少普通用户
上一篇 2022年5月19日 上午11:14
下一篇 2022年5月19日

相关推荐