单目测距与双目测距一样需要完成的第一步是相机的标定
推荐用matlab进行标定，标定的方法可以参看这个博客http://blog.csdn.net/dreamharding/article/details/53700166
和https://blog.csdn.net/heroacool/article/details/51023921
在添加工具箱时注意选择第二个add withsubfolders（和子文件夹一起添加），否则容易导致用calib命令时打不开工具箱
标定相机需要注意的第二个问题是，由于使用的是外接usb摄像头其传输的像素大小由于usb带宽限制为640*480，所以标定使用的图片大小也需要缩放至640*480否则会导致标定出来的内参比实际使用时由于带宽影响导致图像缩小的内参大数倍
标定完相机得到内参矩阵和畸变向量后就可以开始进行opencv的单目测距
单目测距使用的是sovelpnp函数，其第一个参数是输入的三维点坐标矩阵（单位是cm），注意此三维坐标不能为随意点（ps:网上有些的三维点写的比较随意），三维点需要与第二个参数imgPoints的点一一对应（imgPoints是输入的图像二维点向量）
第三个参数是由上一步得到的相机内参矩阵（9*9）
第四个参数是相机的外参（1*5）
第五个参数是输出的相机旋转向量
第六个参数输出的是相机的平移向量
通过旋转向量和平移向量就可以得到相机坐标系相对于世界坐标系的旋转参数与平移情况
关于世界坐标系与相机坐标系转换的原理公式问题可以参看
http://blog.csdn.net/chenmohousuiyue/article/details/78157509?locationnum=9&fps=1
下面来发表一下我的理解
通过小孔成像的原理，根据相似三角形定理x*Z=f*X,其中的x是像素宽度，f是对应轴上的焦距，X是物体的实际长度，Z是物体到相机的距离
然后考虑到主点pinhole plane中的点的坐标u（相对于（0，0）左上角的偏移量），就可以得到x方向的像素坐标，y方向同理可得
然后通过线性代数的知识把上述的式子写成矩阵相乘
我这里进行了逆向的推导证明式子的成立
可以发现其中的第一个矩阵是相机的内参矩阵。
上述式子得到的是相机坐标系与图像坐标系之间的关系，
由于相机坐标系与世界坐标系存在xyz轴的平移与yaw,pinch,row的旋转，及世界坐标系转换到相机坐标系需要乘上R(选择矩阵)T（平移矩阵）
那么可以得到像素坐标系与世界坐标系的关系就是在原有的像素坐标系到相机坐标系的式子的基础上乘以RT矩阵
即有：x =M*[R|t]*X，其中的M是相机的内参矩阵，R,T是世界坐标转换为相机坐标的旋转平移矩阵
到这一步后可以发现在等式x =M*[R|t]*X中我们已知x,X,M就可以得到R,T，所以这里也可以反应出在相机标定中缩放图像为640*480的重要性，以及把像素点和世界坐标点一一对应的重要性
这里求R,T矩阵的过程就是使用opencv的sovelpnp输入像素坐标（及公式的x），世界坐标(及公式的X)，输入相机的内参矩阵和畸变矩阵（及公式的M）可以求得rvec和tvec及公式的（R,T矩阵）
在通过sovelpnp得到相机与世界坐标系转换关系的R,T矩阵后就是需要通过R,T矩阵求解出相机坐标系相对于世界坐标系的欧拉角关系和距离关系
旋转矩阵的公式推导可以参看这个图假使p点在原坐标系中的点为（x，y），在旋转角度o后的坐标系中的坐标为（x',y'）
可以得到数学公式为x'=xcoso-ysino,y'=xsino+ysino;
单个轴的旋转矩阵为
旋转矩阵R[3][3]
R[3][3]=R(row)*R(pitch)*R(yaw)得到的，可以得到总的
然后求解角度的公式为
其中rm就是那个3*3rvtc的矩阵，由sovelpnp求得,然后距离转换公式我就不知道怎么推倒就直接给我网上找的代码了
最后的结果是测距精度为1cm左右
代码：
#include "opencv2/opencv.hpp"
#include "iostream"
using namespace cv;
using namespace std;
int h_min = 75;
int s_min = 75;
int v_min = 75;
int h_max = 177;
int s_max = 195;
int v_max = 177;
Scalar hsv_min(h_min, s_min, v_min);
Scalar hsv_max(h_max, s_max, v_max);
void getTarget2dPoinstion(const cv::RotatedRect & rect, vector<Point2f> & target2d, const cv::Point2f & offset) {
Point2f vertices[4];
rect.points(vertices);//把矩形的四个点复制给四维点向量
Point2f lu, ld, ru, rd;
sort(vertices, vertices + 4, [](const Point2f & p1, const Point2f & p2) { return p1.x < p2.x; });//从4个点的第一个到最后一个进行排序
if (vertices[0].y < vertices[1].y) {
lu = vertices[0];
ld = vertices[1];
}
else {
lu = vertices[1];
ld = vertices[0];
}
if (vertices[2].y < vertices[3].y) {
ru = vertices[2];
rd = vertices[3];
}
else {
ru = vertices[3];
rd = vertices[2];
}
target2d.clear();
target2d.push_back(lu + offset);
target2d.push_back(ru + offset);
target2d.push_back(rd + offset);
target2d.push_back(ld + offset);
}
vector<cv::Point3f> Generate3DPoints()
{
std::vector<cv::Point3f> points;
float x, y, z;
x = 0; y = 0; z = 0;
points.push_back(cv::Point3f(x, y, z));
x = 12; y = 0; z = 0;
points.push_back(cv::Point3f(x, y, z));
x = 12; y = 3; z = 0;
points.push_back(cv::Point3f(x, y, z));
x = 0; y = 3; z = 0;
points.push_back(cv::Point3f(x, y, z));
//for (unsigned int i = 0; i < points.size(); ++i)
//{
// cout << "三维点：" << points[i] << endl;
//}
return points;
}
//将空间点绕Z轴旋转
//输入参数 x y为空间点原始x y坐标
//thetaz为空间点绕Z轴旋转多少度，角度制范围在-180到180
//outx outy为旋转后的结果坐标
static void CodeRotateByZ(double x, double y, double thetaz, double& outx, double& outy)
{
double x1 = x;//将变量拷贝一次，保证&x == &outx这种情况下也能计算正确
double y1 = y;
double rz = thetaz * 3.14 / 180;
outx = cos(rz) * x1 - sin(rz) * y1;
outy = sin(rz) * x1 + cos(rz) * y1;
}
static void CodeRotateByY(double x, double z, double thetay, double& outx, double& outz)
{
double x1 = x;
double z1 = z;
double ry = thetay * 3.14 / 180;
outx = cos(ry) * x1 + sin(ry) * z1;
outz = cos(ry) * z1 - sin(ry) * x1;
}
//将空间点绕X轴旋转
//输入参数 y z为空间点原始y z坐标
//thetax为空间点绕X轴旋转多少度，角度制，范围在-180到180
//outy outz为旋转后的结果坐标
static void CodeRotateByX(double y, double z, double thetax, double& outy, double& outz)
{
double y1 = y;//将变量拷贝一次，保证&y == &y这种情况下也能计算正确
double z1 = z;
double rx = thetax * 3.14 / 180;
outy = cos(rx) * y1 - sin(rx) * z1;
outz = cos(rx) * z1 + sin(rx) * y1;
}
void main()
{
VideoCapture cap(0);
Mat objPM;//三维点矩阵
vector<Point3f> objectPoints = Generate3DPoints();//三维坐标点
vector<Point2f> projectedPoints;//三维点投影到二维点的向量用于重画
Mat(objectPoints).convertTo(objPM, CV_32F);//把三维点向量变成三维点矩阵
double cameraD[3][3] = { { 412.92673 ,0 ,318.52471, },
{ 0, 412.27311 , 258.65883, },
{ 0 ,0 , 1.0000 } };//通过matlab标定的单相机内参
double distC[5] = { 0.24775, - 0.50118, - 0.00513 , 0.00706 , 0.00000 };//通过matlab获取的相机畸变参数
Mat cameraMatrix(3, 3, cv::DataType<double>::type, cameraD);
Mat distCoeffs(5, 1, cv::DataType<double>::type, distC);
Mat rvec(3, 1, cv::DataType<double>::type);
Mat tvec(3, 1, cv::DataType<double>::type);
if (cap.isOpened())
{
while (1)
{
Mat frame;
cap >> frame;
Mat tempImage = frame.clone();
Mat thre_M;
cvtColor(frame, frame, CV_BGR2HSV);
Mat dst;
inRange(frame, hsv_min, hsv_max, thre_M);
Mat element = getStructuringElement(MORPH_RECT, Size(3, 3));
morphologyEx(thre_M, thre_M, MORPH_ERODE, element);//腐蚀
morphologyEx(thre_M, thre_M, MORPH_DILATE, element);//膨胀
morphologyEx(thre_M, thre_M, MORPH_DILATE, element);//膨胀
imshow("thre_M", thre_M);
vector<vector<Point>>contours;//contours是一个二维向量，内层[j]是单个轮廓的线条点,外层是以轮廓为单位元素[j]，表示所有轮廓信息
findContours(thre_M, contours, RETR_EXTERNAL, CHAIN_APPROX_NONE);
vector<RotatedRect>box(contours.size());//定义旋转矩形类向量rect，他的大小是轮廓的个数
vector<Point2f> imagePoints;
Point2f rect[4];
for (int i = 0; i<contours.size(); i++)//contours.size()表示轮廓个数
{
box[i] = minAreaRect(Mat(contours[i]));//contours[i]表示的是单个轮廓的首地址，是第一维向量,第i个轮廓的所有点转换成Mat类型
box[i].points(rect);//box[i]的成员函数points返回的就是point2f类的四个角点
//box[i].angle=//box[i]表示第i个最小外接矩形
if (box[i].size.width*box[i].size.height > 3000)
{
getTarget2dPoinstion(box[i], imagePoints, Point2f(0, 0));
if (imagePoints.size() == 4 && objPM.size>0)
{
for (int i = 0; i < 4; i++)
{
line(tempImage, rect[i], rect[(i + 1) % 4], Scalar(0, 0, 255), 2);
}
solvePnP(objPM, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
double rm[3][3];
cv::Mat rotMat(3, 3, CV_64FC1, rm);//使得rm数组与rotmat矩阵共享数据
Rodrigues(rvec, rotMat);//罗技,把旋转向量变成旋转矩阵
float theta_z = atan2(rm[1][0], rm[0][0])*57.2958;
float theta_y = atan2(-rm[2][0], sqrt(rm[2][0] * rm[2][0] + rm[2][2] * rm[2][2]))*57.2958;
float theta_x = atan2(rm[2][1], rm[2][2])*57.2958;
//提出平移矩阵，表示从相机坐标系原点，跟着向量(x,y,z)走，就到了世界坐标系原点
double tx = tvec.ptr<double>(0)[0];
double ty = tvec.ptr<double>(0)[1];
double tz = tvec.ptr<double>(0)[2];
CodeRotateByZ(tx, ty, -1 * theta_z, tx, ty);
CodeRotateByY(tx, tz, -1 * theta_y, tx, tz);
CodeRotateByX(ty, tz, -1 * theta_x, ty, tz);
projectedPoints.clear();
projectPoints(objPM, rvec, tvec, cameraMatrix, distCoeffs, projectedPoints);//projectPoints将3D坐标投影到图像平面上
for (unsigned int i = 0; i < projectedPoints.size(); ++i)
{
if (projectedPoints[i].x>0 && projectedPoints[i].x<640 && projectedPoints[i].y>0 && projectedPoints[i].y<480)
circle(tempImage, projectedPoints[i], 3, Scalar(255, 0, 0), -1, 8);
}
char theta_z_name[20];
char theta_y_name[20];
char theta_x_name[20];
char x_name[20];
char y_name[20];
char z_name[20];
sprintf_s(z_name, "z%d", int(tz*-1));
sprintf_s(y_name, "y%d", int(ty*-1));
sprintf_s(x_name, "x%d", int(tx*-1));
sprintf_s(theta_z_name, "theta_z_name%d", int(theta_z));
sprintf_s(theta_y_name, "theta_z_name%d", int(theta_y));
sprintf_s(theta_x_name, "theta_z_name%d", int(theta_x));
/*putText(tempImage, theta_z_name, Point(100, 100), CV_FONT_HERSHEY_COMPLEX, 1, Scalar(255, 0, 0));
putText(tempImage, theta_y_name, Point(150, 150), CV_FONT_HERSHEY_COMPLEX, 1, Scalar(255, 0, 0));
putText(tempImage, theta_x_name, Point(200, 200), CV_FONT_HERSHEY_COMPLEX, 1, Scalar(255, 0, 0));*/
putText(tempImage, z_name, Point(50, 100), CV_FONT_HERSHEY_COMPLEX, 1, Scalar(255, 0, 0));
putText(tempImage, y_name, Point(100, 150), CV_FONT_HERSHEY_COMPLEX, 1, Scalar(255, 0, 0));
putText(tempImage, x_name, Point(150, 200), CV_FONT_HERSHEY_COMPLEX, 1, Scalar(255, 0, 0));
}
}
}
imshow("tempImage", tempImage);
waitKey(10);
}
}
}
扫一扫
1016
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
