cv::solvePnP作用及参数释义

本文介绍了OpenCV中cv::solvePnP函数,用于解决透视-点(PnP)问题,计算物体在相机坐标系中的姿态。还说明了其参数含义及返回值,可实现物体定位和跟踪。同时介绍了cv2.calibrateCamera()函数,用于相机标定,能求解相机内参和外参,返回相关参数用于图像畸变矫正。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

cv::solvePnP函数是OpenCV中的一个函数,用于解决透视-点(PnP)问题,即根据物体的3D模型和对应的2D图像点来估计物体在场景中的姿态。该函数的作用是计算物体在相机坐标系中的姿态,即旋转和平移向量。

cv::solvePnP函数的参数含义如下:
- objectPoints: 物体的3D模型点,即物体的空间坐标点。
- imagePoints: 对应的2D图像点,即物体在图像上的投影点。
- cameraMatrix: 相机的内参数矩阵,包括焦距、主点等参数。
- distCoeffs: 相机的畸变系数,用于校正图像畸变。
- rvec: 初始的旋转向量的估计值,可选参数。
- tvec: 初始的平移向量的估计值,可选参数。
- useExtrinsicGuess: 是否使用初始估计值,默认为false。
- flags: 解PnP问题时使用的算法标志,例如EPnP、DLT等。

cv::solvePnP函数会返回计算得到的物体在相机坐标系中的旋转向量和平移向量。通过这些向量,可以得到物体的姿态信息,从而实现物体在场景中的定位和跟踪。

注:以上信息由chatgpt生成

PnP的含义为 Project n Points,通过车辆坐标系或世界坐标系的坐标点和图像上的坐标点,来拟合出到相机系的旋转向量和平移向量,用于计算相机外参;

cv::Mat rotation_vector, translation_vector;  

//若在鱼眼图上进行求解
cv::solvePnP(worldPoints, imagePoints, cameraInstrinsic, cameraDistCoeffs, rotation_vector, translation_vector);

// 若在无畸变图像上进行拟合求解,则畸变参数可传递一个空矩阵,求解出旋转向量和平移向量,通过rotation_vector和translation_vector返回
cv::solvePnP(worldPoints, imagePoints, cameraInstrinsic, cv::Mat(), rotation_vector, translation_vector);

在python中的写法为:

# 在无畸变图中求解外参,若在畸变图上,则需传入畸变系数
_, rotation_vector, trans_vector = cv2.solvePnP(world_points, image_points, camera_instrinsic, None)

cv.solvePnP一般是在已知相机内参的情况下,用来求解相机外参;而cv.calibrateCamera()则可以通过棋盘格等同时求解出相机的内参和外参;

cv2.calibrateCamera()函数的作用是对相机进行标定,通过找到相机的内参和外参参数。这个函数会使用一组标定图像(如棋盘格图案)来确定相机的内部参数和畸变系数,以及相机的旋转和平移向量。

参数含义如下:
- objectPoints: 一个包含标定图案上的三维点坐标的列表,通常是在标定图案上预定义的。
- imagePoints: 一个包含标定图案在图像上对应的二维点坐标的列表。
- imageSize: 图像的尺寸,通常是一个元组,如(640, 480)。
- cameraMatrix: 输出的相机矩阵,包含相机的内部参数。
- distCoeffs: 输出的畸变系数,用来校正图像畸变。
- rvecs: 输出的旋转向量。
- tvecs: 输出的平移向量。
- flags: 用来指定标定相机时的一些选项,如是否考虑初始猜测值等。

这个函数会返回相机矩阵、畸变系数、旋转向量、平移向量等参数,这些参数可以用来对标定后的相机拍摄的图像进行畸变矫正。

【注:该释义由chatgpt生成】

#include <map> #include <vector> #include <Eigen/Eigen> #include <opencv2/opencv.hpp> #include <cv_bridge/cv_bridge.h> #include <iostream> #include <fstream> // 棋盘格检测器类,用于检测棋盘格并计算机器人/相机坐标系转换 class ChessboardDetector_cls { private: cv::Mat rvec; // 旋转向量(世界坐标系到相机坐标系的旋转) cv::Mat tvec; // 平移向量(世界坐标系原点到相机坐标系原点的平移) // 相机内参矩阵 [fx, 0, cx; 0, fy, cy; 0, 0, 1] cv::Mat cameraMatrix; // 畸变系数 [k1, k2, p1, p2, k3] cv::Mat distCoeffs; // 棋盘格方块物理尺寸(单位:米,需与实际应用场景一致) float squareSize; int boardWidth = 7; // 棋盘格宽度(单位:内角点) int boardHeight = 7; // 棋盘格高度(单位:内角点) bool cvVerPlus45 = false; // OpenCV版本判断标志 public: // 构造函数,初始化相机参数和棋盘格尺寸 ChessboardDetector_cls(float square_size = 12.5,int nDevType=1) // 默认1cm方块,1=r1d,0=r8c { if(nDevType == 0){ // ✅ 相机内参矩阵 cameraMatrix: // [[1.04969613e+03 0.00000000e+00 5.73570763e+02] // [0.00000000e+00 1.05086275e+03 4.05877726e+02] // [0.00000000e+00 0.00000000e+00 1.00000000e+00]] // ✅ 畸变系数 distCoeffs: // [[-1.85926395e-03 1.61417431e-03 1.14303737e-03 7.58650886e-05 // -1.17211371e-03]] this->cameraMatrix = (cv::Mat_<double>(3, 3) << 593.52455247, 0, 359.99576897, 0, 591.1469869, 231.48422218, 0, 0, 1); this->distCoeffs = (cv::Mat_<double>(1, 5) << 0.03468398, 0.44016135, -0.00044522, 0.01792056, -1.15909218); }else{ // ✅ 相机内参矩阵 cameraMatrix: // [[1.05382819e+03 0.00000000e+00 5.72604459e+02] // [0.00000000e+00 1.05425060e+03 4.02170466e+02] // [0.00000000e+00 0.00000000e+00 1.00000000e+00]] // ✅ 畸变系数 distCoeffs: // [[ 0.00272168 -0.04679861 0.00091088 -0.00053716 0.14500516]] this->cameraMatrix = (cv::Mat_<double>(3, 3) << 1.05382819e+03, 0.00000000e+00, 5.72604459e+02, 0.00000000e+00, 1.05425060e+03, 4.02170466e+02, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00); this->distCoeffs = (cv::Mat_<double>(1, 5) << 0.00272168, -0.04679861, 0.00091088, -0.00053716, 0.14500516); } this->squareSize = square_size; // 初始化棋盘格方块尺寸 int major_ver = 0, minor_ver = 0; std::sscanf(CV_VERSION, "%d.%d", &major_ver, &minor_ver); // 使用自适应阈值检测棋盘格角点 // 这里的版本判断是为了兼容OpenCV 4.5+和4.4-的不同函数调用 // 4.5+版本使用findChessboardCornersSB函数,4.4-版本使用findChessboardCorners函数 if (major_ver > 4 || (major_ver == 4 && minor_ver >= 5)){ // OpenCV 4.5+ cvVerPlus45 = true; } } // 棋盘格检测服务回调函数 // 输入:原始图像,输出:位姿向量[x,y,z](相机坐标系下的棋盘格原点坐标) bool ChessboardDetectionCallback(cv::Mat image, std::vector<float>& vecRt) { if(!ChessboardDetection(image)) { // 执行检测 return false; // 检测失败返回false } // 生成唯一文件名 auto now = std::chrono::system_clock::now(); auto timestamp = std::chrono::duration_cast<std::chrono::milliseconds>( now.time_since_epoch()).count(); std::string filename = "/home/nvidia/YaLongR8/src/bringup/detected_" + std::to_string(timestamp) + ".jpg"; // 保存带角点的图像 if(!cv::imwrite(filename, image)){ std::cerr << "Failed to save image: " << filename << std::endl; } else { std::cout << "Saved image with corners: " << filename << std::endl; } vecRt = PoseCalculation(); // 计算并返回位姿 return true; } // 计算标定板坐标系到相机坐标系的转换矩阵 cv::Point3f transformToCameraFrame(const cv::Point3f& obj_in_board) { cv::Mat R; cv::Rodrigues(rvec, R); // 将旋转向量转为旋转矩阵 cv::Mat pt_board = (cv::Mat_<double>(3,1) << obj_in_board.x, obj_in_board.y, obj_in_board.z); cv::Mat pt_camera = R * pt_board + tvec; return cv::Point3f( static_cast<float>(pt_camera.at<double>(0)), static_cast<float>(pt_camera.at<double>(1)), static_cast<float>(pt_camera.at<double>(2)) ); } private: // 核心检测函数 bool ChessboardDetection(cv::Mat image) { if(image.empty()) { std::cerr << "图像为空!" << std::endl; return false; } const cv::Size boardSize(boardWidth, boardHeight); // 棋盘格内部角点数量(7x7网格) std::vector<cv::Point2f> corners; // 存储检测到的角点坐标 // 转换为灰度图(棋盘格检测需要灰度图像) cv::Mat gray_image; cv::cvtColor(image, gray_image, cv::COLOR_BGR2GRAY); // 使用自适应阈值检测棋盘格角点 bool found = false; if (cvVerPlus45) { ROS_INFO("OpenCV version >= 4.5 detected, using findChessboardCornersSB."); // OpenCV 4.5+ found = cv::findChessboardCornersSB( gray_image, boardSize, corners, cv::CALIB_CB_EXHAUSTIVE | cv::CALIB_CB_ACCURACY ); } else { ROS_INFO("OpenCV version < 4.5 detected, using findChessboardCorners."); // OpenCV 4.4- or earlier fallback // found = cv::findChessboardCorners(gray_image, boardSize, corners); found = cv::findChessboardCorners( gray_image, boardSize, corners, cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE + cv::CALIB_CB_FAST_CHECK ); } if (!found) { // std::cerr << "未检测到棋盘格!" << std::endl; // cv::imshow("检测结果", image); // 显示原始图像辅助调试 // cv::waitKey(1000); // 显示1秒后自动关闭 // cv::destroyWindow("检测结果"); ROS_ERROR("Chessboard not found!"); return false; } // 亚像素级角点精确化 cv::cornerSubPix( gray_image, corners, cv::Size(11, 11), // 搜索窗口大小 cv::Size(-1, -1), // 死区大小(-1表示无死区) cv::TermCriteria( cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, // 最大迭代次数 0.1 // 精度阈值 ) ); // 绘制角点(绿色) cv::drawChessboardCorners(image, boardSize, cv::Mat(corners), found); // 生成三维物体点(假设棋盘格在Z=0平面) std::vector<cv::Point3f> objectPoints; // 棋盘格原点在左上第一个角点处 // for (int i = 0; i < boardSize.height; ++i) { // for (int j = 0; j < boardSize.width; ++j) { // objectPoints.emplace_back( // j * squareSize, // X坐标(沿宽度方向) // i * squareSize, // Y坐标(沿高度方向) // 0 // Z坐标固定为0 // ); // } // } // 棋盘格原点在中心位置 for (int i = 0; i < boardSize.height; ++i) { for (int j = 0; j < boardSize.width; ++j) { objectPoints.emplace_back( (j - (boardSize.width - 1) / 2.0) * squareSize, // X坐标(沿宽度方向) (i - (boardSize.height - 1) / 2.0) * squareSize, // Y坐标(沿高度方向) 0 // Z坐标固定为0 ); } } // 求解PnP问题(透视n点定位) // if (this->rvec.empty() || this->tvec.empty()){ // cv::solvePnP( // objectPoints, // 物体坐标系中的3D点 // corners, // 图像坐标系中的2D点 // cameraMatrix, // 相机内参矩阵 // distCoeffs, // 畸变系数 // rvec, // 输出旋转向量 // tvec, // 输出平移向量 // false, // 使用初始估计(连续帧时提高稳定性) // cv::SOLVEPNP_ITERATIVE // 使用迭代算法 // ); // } else { // cv::solvePnP( // objectPoints, // 物体坐标系中的3D点 // corners, // 图像坐标系中的2D点 // cameraMatrix, // 相机内参矩阵 // distCoeffs, // 畸变系数 // rvec, // 输出旋转向量 // tvec, // 输出平移向量 // true, // 使用初始估计(连续帧时提高稳定性) // cv::SOLVEPNP_ITERATIVE // 使用迭代算法 // ); // } std::vector<int> inliers; cv::solvePnPRansac( objectPoints, corners, cameraMatrix, distCoeffs, rvec, tvec, false, // 不使用初始估计 100, // 最大迭代次数 8.0, // 重投影误差阈值(像素) 0.99, // 置信度 inliers, // 返回的内点索引 cv::SOLVEPNP_ITERATIVE ); return true; } // 位姿计算函数 std::vector<float> PoseCalculation() { // 欧拉角(ZYX) cv::Mat R; cv::Rodrigues(rvec, R); float roll = atan2(R.at<double>(2,1), R.at<double>(2,2)); float pitch = atan2(-R.at<double>(2,0), sqrt(R.at<double>(2,1)*R.at<double>(2,1) + R.at<double>(2,2)*R.at<double>(2,2))); float yaw = atan2(R.at<double>(1,0), R.at<double>(0,0)); return { static_cast<float>(tvec.at<double>(0)), // X static_cast<float>(tvec.at<double>(1)), // Y static_cast<float>(tvec.at<double>(2)), // Z // static_cast<float>(rvec.at<double>(0)), // Rx // static_cast<float>(rvec.at<double>(1)), // Ry // static_cast<float>(rvec.at<double>(2)) // Rz static_cast<float>(roll), // Roll static_cast<float>(pitch), // Pitch static_cast<float>(yaw) // Yaw }; // // 将旋转向量转换为旋转矩阵 // cv::Mat rotationMatrix; // cv::Rodrigues(rvec, rotationMatrix); // // 计算棋盘格原点在相机坐标系中的坐标 // cv::Mat chessboardOrigin = -rotationMatrix.t() * tvec; // // 转换为float类型向量 // return { // static_cast<float>(chessboardOrigin.at<double>(0)), // X坐标 // static_cast<float>(chessboardOrigin.at<double>(1)), // Y坐标 // static_cast<float>(chessboardOrigin.at<double>(2)) // Z坐标 // }; } };这是啥如何使用
最新发布
07-17
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值