cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, useExtrinsicGuess, flags);1. Parameter description:
- objectPoints: A vector<cv::Point3f>, containing the coordinates of three-dimensional points in the world coordinate system, at least 4 points are required.
- imagePoints: A vector<cv::Point2f>, containing the coordinates of the corresponding two-dimensional points on the image, corresponding to the points in objectPoints one-to-one.
- cameraMatrix: The internal parameter matrix of the camera, type is cv::Mat, generally a 3x3 floating point matrix.
- distCoeffs: The distortion coefficient of the camera, type cv::Mat, generally a 4x1 or 5x1 floating point matrix.
- rvec: The output rotation vector, type cv::Mat, is a floating point matrix of size 3x1.
- tvec: The output translation vector, type cv::Mat, is a floating point matrix of size 3x1.
- useExtrinsicGuess: A Boolean value indicating whether to use optional initial guesses for rotation and translation vectors. Default is false.
- flags: An option flag used to control function behavior, default is 0.
The function returns:
- Returns true on success and false on failure.
2. Instructions for use:
objectPoints, imagePoints, cameraMatrix, distCoeffs four parameters as input parameters
rvec, tvec as output parameters
objectPoints are the three-dimensional coordinates of the world coordinate system
imagePoints are the two-dimensional point coordinates on the image
For example, use a calibrated monocular camera to capture a rectangular object (the camera intrinsic parameters & distortion coefficient are known),
objectPoints: Use a ruler to measure the distance between the upper left corner, upper right corner, lower left corner, and lower right corner of the object. Use any point as the 0 point to establish the world coordinate system. Set the z value to 0. The coordinates of all points are obtained as objectPoints, stored in vector.
imagePoints: Find the upper left corner, upper right corner, lower left corner, and lower right corner of the object in the image. The pixel coordinates of all the points are imagePoints and are saved in the vector.
Usage example:
#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
int main() {
std::vector<cv::Point3f> objectPoints; // 世界坐标系中的三维点
std::vector<cv::Point2f> imagePoints; // 图像上的二维点
// 添加 objectPoints 和 imagePoints 的数据
// 创建相机内参数矩阵
cv::Mat cameraMatrix = (cv::Mat_<double>(3,3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
// 创建相机畸变系数矩阵
cv::Mat distCoeffs = (cv::Mat_<double>(1,5) << k1, k2, p1, p2, k3;
//完善内参参数＆畸变系数参数
cv::Mat rvec; // 输出的旋转向量
cv::Mat tvec; // 输出的平移向量
bool success = cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
if (success) {
// 获取旋转向量和平移向量的结果
cv::Mat rotationMatrix;
cv::Rodrigues(rvec, rotationMatrix);
std::cout << "Rotation Vector:" << std::endl << rvec << std::endl;
std::cout << "Translation Vector:" << std::endl << tvec << std::endl;
std::cout << "Rotation Matrix:" << std::endl << rotationMatrix << std::endl;
}
return 0;
}3. Things to note:
When using solvePnP, please note thatthe point coordinates in the objectPoints and imagePoints containers must correspond one to one. For example, when there are only four points, All are stored in the container in the order of upper left corner, upper right corner, lower left corner, and lower right corner; if the order is not the same, the final output value will be wrong.
When the author uses solvePnP, the photographed objects are four positioning circles. The pixel coordinates of the circles are identified through the SimpleBlobDetector of opencv. After identification, the pixel coordinates of the circles are disordered and cannot correspond to objectPoints. Therefore, the following algorithm is developed.
is used to perform bubble sorting on four two-dimensional coordinates to obtain the points corresponding to the upper left corner, upper right corner, lower left corner, and lower right corner respectively
(If there is no such requirement, you can ignore this paragraph)
int main() {
std::vector<cv::Point2f> imagePoints; // 存放四个点的 vector
// 假设已经将四个点的坐标存入 imagePoints 中
// 寻找左上角、右上角、右下角和左下角对应的点
cv::Point2f topLeft, topRight, bottomRight, bottomLeft;
float minX = FLT_MAX, minY = FLT_MAX;
float maxX = FLT_MIN, maxY = FLT_MIN;
for (const auto& point : imagePoints) {
if (point.x <= minX && point.y <= minY) {
topLeft = point;
minX = point.x;
minY = point.y;
}
if (point.x >= maxX && point.y <= minY) {
topRight = point;
maxX = point.x;
minY = point.y;
}
if (point.x >= maxX && point.y >= maxY) {
bottomRight = point;
maxX = point.x;
maxY = point.y;
}
if (point.x <= minX && point.y >= maxY) {
bottomLeft = point;
minX = point.x;
maxY = point.y;
}
}
// 输出左上角、右上角、右下角和左下角对应的点的坐标
std::cout << "左上角坐标: (" << topLeft.x << ", " << topLeft.y << ")" << std::endl;
std::cout << "右上角坐标: (" << topRight.x << ", " << topRight.y << ")" << std::endl;
std::cout << "右下角坐标: (" << bottomRight.x << ", " << bottomRight.y << ")" << std::endl;
std::cout << "左下角坐标: (" << bottomLeft.x << ", " << bottomLeft.y << ")" << std::endl;
// 新建一个vector存放四个点坐标，按照objectPoints的存放顺序进行存放
std::vector<cv::Point2f> imagePoints2;
imagePoints2.push_back(topLeft);
imagePoints2.push_back(topRight);
imagePoints2.push_back(bottomRight);
imagePoints2.push_back(bottomLeft);
return 0;
}4. Supplement
After obtaining the rotation vector rvec and the translation vector tvec through solvePnP, you can calculate the actual distance from the camera to the center of the object being measured
#include <cmath>
#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
int main() {
std::vector<cv::Point3f> objectPoints; // 世界坐标系中的三维点
std::vector<cv::Point2f> imagePoints; // 图像上的二维点
// 添加 objectPoints 和 imagePoints 的数据
// 创建相机内参数矩阵
cv::Mat cameraMatrix = (cv::Mat_<double>(3,3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
// 创建相机畸变系数矩阵
cv::Mat distCoeffs = (cv::Mat_<double>(1,5) << k1, k2, p1, p2, k3;
//完善内参参数＆畸变系数参数
cv::Mat rvec; // 输出的旋转向量
cv::Mat tvec; // 输出的平移向量
bool success = cv::solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec);
if (success) {
//计算相机距离被测物的实际距离
float distance = sqrt(tvec.at<double>(0,0) * tvec.at<double>(0,0) + tvec.at<double>(1,0) * tvec.at<double>(1,0) + tvec.at<double>(2,0) * tvec.at<double>(2,0)) / 10;
std::cout << "distance = "<< distance << std::endl;
}
return 0;
}