基于OPENCV的单目测距

基于OPENCV的单目测距

最近研究了一下单目测距,关于单目测距的原理有各位大神的讲解,这里只写一些自已使用上的记录,使用环境为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矩阵即是相机内参数矩阵,其形式为:

其中,

为摄像头焦距,单位为图像像素;

为摄像头成像中心点在图像坐标系中坐标,因为图像坐票系以左上角为原点,所以

理论值为图像像素值的一半, 若图像大小为640*480,则

约为320,240,可以用来作为参考。

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;

}

}

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

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

免责声明:本文仅代表文章作者的个人观点,与本站无关。其原创性、真实性以及文中陈述文字和内容未经本站证实,对本文以及其中全部或者部分内容文字的真实性、完整性和原创性本站不作任何保证或承诺,请读者仅作参考,并自行核实相关内容。

http://www.pinlue.com/style/images/nopic.gif

分享
评论
首页