Overview
Monocular camera pnp ranging is to measure the distance between something in the picture and the camera through a 2D picture taken by a monocular camera.
Need to know the actual size of the object being measured
Before ranging , the camera needs to be calibrated . Whichever camera needs to be used for ranging is calibrated. Once you switch to another camera, you need to recalibrate the final camera.
Why do you need camera calibration?
Camera calibration is to obtain the coordinate transformation details from any point in the 3D world to the corresponding point on the picture captured by the camera . The above coordinate transformation can be described by matrix operations. Camera calibration is to obtain the camera intrinsic parameter matrix .
The camera internal parameter matrix is only related to the camera itself, so changing the camera requires obtaining two matrices of the new camera, and has nothing to do with changing the measured target object.
Camera imaging related principles
Camera intrinsic parameter matrix: including camera matrix and distortion coefficient.
Camera matrix: [fx, 0, cx; 0, fy, cy; 0,0,1]. Among them, the focal length (fx, fy) and the optical center (cx, cy).
Distortion coefficient: five parameters k1, k2, P1, P2, k3 of the distortion mathematical model.
Camera extrinsic parameters: The coordinates of the point in the 3D world are converted into the plane coordinates of the 2D image through rotation and translation transformations. The rotation matrix and translation matrix are called camera extrinsic parameters.
The coordinate transformation process of camera imaging:
3D world coordinate system --> Camera coordinate system: [The 3D coordinates of an object in the 3D world relative to the world origin ] are converted into [the 3D coordinates of the object with the camera lens center as the coordinate origin ]
Camera coordinate system --> Image coordinate system: [The 3-dimensional coordinates of the object when the camera lens center is the coordinate origin ] are converted into [ 2-dimensional coordinates imaged on the image sensor ]
Image coordinate system --> Pixel coordinate system: The image coordinate system is continuous information, which is sampled and converted into non- continuous pixels .
Camera calibration
Prepare
This article adopts Zhang Zhengyou’s calibration method , and the calibration board uses a checkerboard pattern . (The purpose of calibration is to obtain the internal parameters of the camera, it does not matter what method or calibration board is used)
For the checkerboard, you can use the images that come with the opencv installation directory. (I don’t know if there is a python version downloaded by pip)
D:\opencv\sources\samples\data （参考此路径）
Note: When printing the picture on paper, you can scale it, but the proportion of the picture should not be changed.
When calibrating, you need to find the corner points of the checkerboard.
According to actual measurements, if the checkerboard is covered with reflective material, it is very likely that the corner points will not be found (even if you look at the pictures taken by the camera with the naked eye, no reflection can be seen at all). Including displaying the checkerboard on a screen and applying transparent tape to the checkerboard paper .
Take pictures for calibration
Run the code
#include <opencv2/opencv.hpp>
#include <string>
#include <iostream>
using namespace cv;
using namespace std;
int main()
{
VideoCapture inputVideo(0);
if (!inputVideo.isOpened())
{
cout << "Could not open the input video " << endl;
return -1;
}
Mat frame;
Mat frameCopy;
string imgname;
vector<Point2f> image_points_buf;
bool isFound;
int f = 1;
while (1)
{
inputVideo >> frame;
frame.copyTo(frameCopy);
if (frame.empty())
break;
// 在未找到角点时，视频会很卡。可以将30到33行注释掉
isFound = findChessboardCorners(frameCopy, Size(7, 7), image_points_buf);
if(isFound)
drawChessboardCorners(frameCopy, Size(7, 7), image_points_buf, isFound);
imshow("Camera", frameCopy);
char key = waitKey(1);
if (key == 'q' || key == 'Q') // quit退出
break;
if (key == 'k' || key == 'K') // 拍摄当前帧
{
cout << "frame:" << f << endl;
imgname = "snapPhotos/"+to_string(f++) + ".jpg";
imwrite(imgname, frame);
}
}
cout << "Finished writing" << endl;
return 0;
}The above code will save the captured pictures to the snapPhotos folder in the current working directory.
Note: If the code is correct and no error is reported during the running process, but the captured pictures cannot be found after running, you can read my article: https://blog.csdn.net/qq_35858902/article/details/128933950
Create a new calibdata.txt file in the current working directory and fill in the following content (no spaces, no blank lines at the end, don’t forget to save):
snapPhotos/1.jpg
snapPhotos/2.jpg
snapPhotos/3.jpg
snapPhotos/4.jpg
snapPhotos/5.jpg
snapPhotos/6.jpg
snapPhotos/7.jpg
snapPhotos/8.jpg
snapPhotos/9.jpg
snapPhotos/10.jpgIf you did not save it to the snapPhotos folder when you took the picture above, you need to change it to the actual path where you saved it. The above is a relative path, you can also use an absolute path:
D:/project_root/snapPhotos/1.jpgStart calibration
Run the following code
This code is slightly modified from https://blog.csdn.net/u012319493/article/details/77622053
#include <iostream>
#include <sstream>
#include <time.h>
#include <stdio.h>
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
using namespace cv;
using namespace std;
void main()
{
ifstream fin("calibdata.txt"); /* 标定所用图像文件的路径 */
ofstream fout("caliberation_result.txt"); /* 保存标定结果的文件 */
// 读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化
int image_count = 0; /* 图像数量 */
Size image_size; /* 图像的尺寸 */
Size board_size = Size(7, 7); /* 标定板上每行、列的角点数 */
vector<Point2f> image_points_buf; /* 缓存每幅图像上检测到的角点 */
vector<vector<Point2f>> image_points_seq; /* 保存检测到的所有角点 */
string filename; // 图片名
vector<string> filenames;
while (getline(fin, filename))
{
++image_count;
Mat imageInput = imread(filename);
filenames.push_back(filename);
//imshow("file", imageInput);
//waitKey(0);
// 读入第一张图片时获取图片大小
if (image_count == 1)
{
image_size.width = imageInput.cols;
image_size.height = imageInput.rows;
}
/* 提取角点 */
if (findChessboardCorners(imageInput, board_size, image_points_buf) == false)
{
cout << "can not find chessboard corners!找不到角点\n"; // 找不到角点
exit(1);
}
else
{
Mat view_gray;
cvtColor(imageInput, view_gray, COLOR_BGR2GRAY); // 转灰度图
/* 亚像素精确化 */
// image_points_buf 初始的角点坐标向量，同时作为亚像素坐标位置的输出
// Size(5,5) 搜索窗口大小
// （-1，-1）表示没有死区
// TermCriteria 角点的迭代过程的终止条件, 可以为迭代次数和角点精度两者的组合
cornerSubPix(view_gray, image_points_buf, Size(5, 5), Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 30, 0.1));
image_points_seq.push_back(image_points_buf); // 保存亚像素角点
/* 在图像上显示角点位置 */
drawChessboardCorners(view_gray, board_size, image_points_buf, false); // 用于在图片中标记角点
imshow("Camera Calibration", view_gray); // 显示图片
waitKey(500); //暂停0.5S
}
}
int CornerNum = board_size.width * board_size.height; // 每张图片上总的角点数
//-------------以下是摄像机标定------------------
/*棋盘三维信息*/
Size square_size = Size(2.6, 2.6); /* 实际测量得到的标定板上每个棋盘格的两边长，单位自定，但要保持统一 */
vector<vector<Point3f>> object_points; /* 保存标定板上角点的三维坐标 */
/*内外参数*/
Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 摄像机内参数矩阵 */
vector<int> point_counts; // 每幅图像中角点的数量
Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0)); /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */
vector<Mat> tvecsMat; /* 每幅图像的旋转向量 */
vector<Mat> rvecsMat; /* 每幅图像的平移向量 */
/* 初始化标定板上角点的三维坐标 */
int i, j, t;
for (t = 0; t < image_count; t++)
{
vector<Point3f> tempPointSet;
for (i = 0; i < board_size.height; i++)
{
for (j = 0; j < board_size.width; j++)
{
Point3f realPoint;
/* 假设标定板放在世界坐标系中z=0的平面上 */
realPoint.x = i * square_size.width;
realPoint.y = j * square_size.height;
realPoint.z = 0;
tempPointSet.push_back(realPoint);
}
}
object_points.push_back(tempPointSet);
}
/* 初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板 */
for (i = 0; i < image_count; i++)
{
point_counts.push_back(board_size.width * board_size.height);
}
/* 开始标定 */
// object_points 世界坐标系中的角点的三维坐标
// image_points_seq 每一个内角点对应的图像坐标点
// image_size 图像的像素尺寸大小
// cameraMatrix 输出，内参矩阵
// distCoeffs 输出，畸变系数
// rvecsMat 输出，旋转向量
// tvecsMat 输出，位移向量
// 0 标定时所采用的算法
calibrateCamera(object_points, image_points_seq, image_size, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);
//------------------------标定完成------------------------------------
// -------------------对标定结果进行评价------------------------------
double total_err = 0.0; /* 所有图像的平均误差的总和 */
double err = 0.0; /* 每幅图像的平均误差 */
vector<Point2f> image_points2; /* 保存重新计算得到的投影点 */
fout << "每幅图像的标定误差：\n";
for (i = 0; i < image_count; i++)
{
vector<Point3f> tempPointSet = object_points[i];
/* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);
/* 计算新的投影点和旧的投影点之间的误差*/
vector<Point2f> tempImagePoint = image_points_seq[i];
Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);
for (int j = 0; j < tempImagePoint.size(); j++)
{
image_points2Mat.at<Vec2f>(0, j) = Vec2f(image_points2[j].x, image_points2[j].y);
tempImagePointMat.at<Vec2f>(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
}
err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
total_err += err /= point_counts[i];
fout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
}
fout << "总体平均误差：" << total_err / image_count << "像素" << endl << endl;
//-------------------------评价完成---------------------------------------------
//-----------------------保存定标结果-------------------------------------------
Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); /* 保存每幅图像的旋转矩阵 */
fout << "相机内参数矩阵：" << endl;
fout << cameraMatrix << endl << endl;
fout << "畸变系数：\n";
fout << distCoeffs << endl << endl << endl;
for (int i = 0; i < image_count; i++)
{
fout << "第" << i + 1 << "幅图像的旋转向量：" << endl;
fout << tvecsMat[i] << endl;
/* 将旋转向量转换为相对应的旋转矩阵 */
Rodrigues(tvecsMat[i], rotation_matrix);
fout << "第" << i + 1 << "幅图像的旋转矩阵：" << endl;
fout << rotation_matrix << endl;
fout << "第" << i + 1 << "幅图像的平移向量：" << endl;
fout << rvecsMat[i] << endl << endl;
}
fout << endl;
//--------------------标定结果保存结束-------------------------------
//----------------------显示定标结果--------------------------------
Mat mapx = Mat(image_size, CV_32FC1);
Mat mapy = Mat(image_size, CV_32FC1);
Mat R = Mat::eye(3, 3, CV_32F);
string imageFileName;
std::stringstream StrStm;
for (int i = 0; i != image_count; i++)
{
initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, image_size, CV_32FC1, mapx, mapy);
Mat imageSource = imread(filenames[i]);
Mat newimage = imageSource.clone();
remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
StrStm.clear();
imageFileName.clear();
StrStm << i + 1;
StrStm >> imageFileName;
imageFileName += "_d.jpg";
imageFileName = "calibrated/" + imageFileName;
imwrite(imageFileName, newimage);
}
fin.close();
fout.close();
return;
}-Board_size, (7, 7) in line 23 of the above code refers to the 7 horizontal and vertical corner points inside the checkerboard. If you are using the opencv built-in checkerboard provided in this article, you do not need to modify it.
- Read the comments on line 76 and modify it yourself.
After successful operation, a calibration_result.txt file will be generated in the current working directory, which contains the internal parameter matrix.
Ranging
Run the following code:
#include <opencv2/opencv.hpp>
#include <math.h>
using namespace std;
using namespace cv;
int main(int argc, char** argv)
{
Mat image = imread("test.jpg");
//被测物某个平面需要特征点，特征点可以是角点，但需要可测出实际坐标
//2d图像特征点像素坐标，可以用鼠标事件找出特征点坐标
vector<Point2d> image_points;
image_points.push_back(Point2d(298, 159));
image_points.push_back(Point2d(380, 159));
image_points.push_back(Point2d(380, 240));
image_points.push_back(Point2d(298, 240));
// 画出特征点
for (int i = 0; i < image_points.size(); i++)
{
circle(image, image_points[i], 3, Scalar(0, 0, 255), -1);
}
// 3D 特征点世界坐标，与像素坐标对应，单位是与标定时填的实际测量棋盘格大小的单位相同，如同是cm或同是mm
//世界坐标指所选的特征点在物理世界中的3d坐标，坐标原点自己选。
//下面以物块平面正中心为坐标原点，x正方向朝右，y正方向朝下
std::vector<Point3d> model_points;
model_points.push_back(Point3d(-1.4f, -1.45f, 0)); // 左上角(-1.4cm,-1.45cm)
model_points.push_back(Point3d(+1.4f, -1.45f, 0));
model_points.push_back(Point3d(+1.4f, +1.45f, 0));
model_points.push_back(Point3d(-1.4f, +1.45f, 0));
// 注意世界坐标和像素坐标要一一对应
// 相机内参矩阵和畸变系数均由相机标定结果得出
// 相机内参矩阵
Mat camera_matrix = (Mat_<double>(3, 3) << 659.9293277147924, 0, 145.8791713723572,
0, 635.3941888799933, 120.2096985290085,
0, 0, 1);
// 相机畸变系数
Mat dist_coeffs = (Mat_<double>(5, 1) << -0.5885200737681696, 0.6747491058456546, 0.006768694852797847,
0.02067272313155804, -0.3616453058722507);
cout << "Camera Matrix " << endl << camera_matrix << endl << endl;
// 旋转向量
Mat rotation_vector;
// 平移向量
Mat translation_vector;
// pnp求解
solvePnP(model_points, image_points, camera_matrix, dist_coeffs, \
rotation_vector, translation_vector, 0, SOLVEPNP_ITERATIVE);
// ITERATIVE方法，其他方法参照官方文档
cout << "Rotation Vector " << endl << rotation_vector << endl << endl;
cout << "Translation Vector" << endl << translation_vector << endl << endl;
Mat Rvec;
Mat_<float> Tvec;
rotation_vector.convertTo(Rvec, CV_32F); // 旋转向量转换格式
translation_vector.convertTo(Tvec, CV_32F); // 平移向量转换格式
Mat_<float> rotMat(3, 3);
Rodrigues(Rvec, rotMat);
// 旋转向量转成旋转矩阵
cout << "rotMat" << endl << rotMat << endl << endl;
Mat P_oc;
P_oc = -rotMat.inv() * Tvec;
// 求解相机的世界坐标，得出p_oc的第三个元素即相机到物体的距离即深度信息，单位是mm
cout << "P_oc" << endl << P_oc << endl;
imshow("Output", image);
waitKey(0);
}Lines 12, 27, 36, and 40, see the comments for modification.
After the operation is successful, the terminal will output information. The last number of P_oc is the distance, and the unit is the same as before.
The unit used in this article is cm, so the measurement is about 21.7568cm