最近研究了一下单目测距,关于单目测距的原理有各位大神的讲解,这里只写一些自已使用上的记录,使用环境为windows10+opencv3.1+vs2015。
买了一个摄像头(笔记本的定焦摄像头也可以),不知道具体参数,想用它实现测距功能。
原理上就是根据三角性的相似性,假设摄像头焦距为f,摄像头距物体距离为dmm,物体在图像中的尺寸为p个像素(假设物体水平放置,为水平上的长度),物体实际长度为xmm, 根据三角形相似性,有
(式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像素,这样就有
(式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;
程序中的 为棋盘单元格实际物理尺寸,单位mm,为棋盘角点数,一般是棋盘实际(row-1)*(col-1)。
这样输出的M矩阵即是相机内参数矩阵,其形式为:
其中,为摄像头焦距,单位为图像像素;
- 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;
}
}
以上 部分为本人作为测试用的相关代码,并未完善,作为备忘,仅供参考。
由于刚接触这方面时间不长,难免有不知道的和理解不对的地方,如有错误,请批评指出,
文章出处登录后可见!