时间：2023-07-16 03:00:02 | 来源：网站运营
时间：2023-07-16 03:00:02 来源：网站运营
OpenCV 之 透视 n 点问题： 透视 n 点问题，源自相机标定，是计算机视觉的经典问题，广泛应用在机器人定位、SLAM、AR/VR、摄影测量等领域
在线咨询
// P3P has up to 4 solutions, and the solutions are sorted by reprojection errors(lowest to highest). int solveP3P ( InputArray objectPoints, // object points, 3x3 1-channel or 1x3/3x1 3-channel. vector<Point3f> can be also passed InputArray imagePoints, // corresponding image points, 3x2 1-channel or 1x3/3x1 2-channel. vector<Point2f> can be also passed InputArray cameraMatrix, // camera intrinsic matrix InputArray distCoeffs, // distortion coefficients.If NULL/empty, the zero distortion coefficients are assumed. OutputArrayOfArrays rvecs, // rotation vectors OutputArrayOfArrays tvecs, // translation vectors int flags // solving method ); bool solvePnP(InputArray opoints, InputArray ipoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess, int flags){ CV_INSTRUMENT_REGION(); vector<Mat> rvecs, tvecs; int solutions = solvePnPGeneric(opoints, ipoints, cameraMatrix, distCoeffs, rvecs, tvecs, useExtrinsicGuess, (SolvePnPMethod)flags, rvec, tvec); if (solutions > 0) { int rdepth = rvec.empty() ? CV_64F : rvec.depth(); int tdepth = tvec.empty() ? CV_64F : tvec.depth(); rvecs[0].convertTo(rvec, rdepth); tvecs[0].convertTo(tvec, tdepth); } return solutions > 0;} solvePnPGeneric() 除了求解相机位姿外，还可得到重投影误差，其定义如下： bool solvePnPGeneric ( InputArray objectPoints, // object points, Nx3 1-channel or 1xN/Nx1 3-channel, N is the number of points. vector<Point3d> can be also passed InputArray imagePoints, // corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, N is the number of points. vector<Point2d> can be also passed InputArray cameraMatrix, // camera intrinsic matrix InputArray distCoeffs, // distortion coefficients OutputArrayOfArrays rvec, // rotation vector OutputArrayOfArrays tvec, // translation vector bool useExtrinsicGuess = false, // used for SOLVEPNP_ITERATIVE. If true, use the provided rvec and tvec as initial approximations, and further optimize them. SolvePnPMethod flags = SOLVEPNP_ITERATIVE, // solving method InputArray rvec = noArray(), // initial rotation vector when using SOLVEPNP_ITERATIVE and useExtrinsicGuess is set to true InputArray tvec = noArray(), // initial translation vector when using SOLVEPNP_ITERATIVE and useExtrinsicGuess is set to true OutputArray reprojectionError = noArray() // optional vector of reprojection error, that is the RMS error ); bool solvePnPRansac ( InputArray objectPoints, // object points, Nx3 1-channel or 1xN/Nx1 3-channel, N is the number of points. vector<Point3d> can be also passed InputArray imagePoints, // corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel, N is the number of points. vector<Point2d> can be also passed InputArray cameraMatrix, // camera intrinsic matrix InputArray distCoeffs, // distortion coefficients OutputArray rvec, // rotation vector OutputArray tvec, // translation vector bool useExtrinsicGuess = false, // used for SOLVEPNP_ITERATIVE. If true, use the provided rvec and tvec as initial approximations, and further optimize them. int iterationsCount = 100, // number of iterations float reprojectionError = 8.0, // inlier threshold value. It is the maximum allowed distance between the observed and computed point projections to consider it an inlier double confidence = 0.99, // the probability that the algorithm produces a useful result OutputArray inliers = noArray(), // output vector that contains indices of inliers in objectPoints and imagePoints int flags = SOLVEPNP_ITERATIVE // solving method ); void solvePnPRefineLM ( InputArray objectPoints, // object points, Nx3 1-channel or 1xN/Nx1 3-channel, N is the number of points InputArray imagePoints, // corresponding image points, Nx2 1-channel or 1xN/Nx1 2-channel InputArray cameraMatrix, // camera intrinsic matrix InputArray distCoeffs, // distortion coefficients InputOutputArray rvec, // input/output rotation vector InputOutputArray tvec, // input/output translation vector TermCriteria criteria = TermCriteria(TermCriteria::EPS+TermCriteria::COUNT, 20, FLT_EPSILON) // Criteria when to stop the LM iterative algorithm); #include "opencv2/imgproc.hpp"#include "opencv2/highgui.hpp"#include "opencv2/calib3d.hpp"using namespace std;using namespace cv;Size kPatternSize = Size(9, 6);float kSquareSize = 0.025;// camera intrinsic parameters and distortion coefficientconst Mat cameraMatrix = (Mat_<double>(3, 3) << 5.3591573396163199e+02, 0.0, 3.4228315473308373e+02, 0.0, 5.3591573396163199e+02, 2.3557082909788173e+02, 0.0, 0.0, 1.0);const Mat distCoeffs = (Mat_<double>(5, 1) << -2.6637260909660682e-01, -3.8588898922304653e-02, 1.7831947042852964e-03, -2.8122100441115472e-04, 2.3839153080878486e-01);int main(){ // 1) read image Mat src = imread("left07.jpg"); if (src.empty()) return -1; // prepare for subpixel corner Mat src_gray; cvtColor(src, src_gray, COLOR_BGR2GRAY); // 2) find chessboard corners and subpixel refining vector<Point2f> corners; bool patternfound = findChessboardCorners(src, kPatternSize, corners); if (patternfound) { cornerSubPix(src_gray, corners, Size(11, 11), Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::MAX_ITER, 30, 0.1)); } else { return -1; } // 3) object coordinates vector<Point3f> objectPoints; for (int i = 0; i < kPatternSize.height; i++) { for (int j = 0; j < kPatternSize.width; j++) { objectPoints.push_back(Point3f(float(j * kSquareSize), float(i * kSquareSize), 0)); } } // 4) Rotation and Translation vectors Mat rvec, tvec; solvePnP(objectPoints, corners, cameraMatrix, distCoeffs, rvec, tvec); // 5) project estimated pose on the image drawFrameAxes(src, cameraMatrix, distCoeffs, rvec, tvec, 2*kSquareSize); imshow("Pose estimation", src); waitKey();} 当标定板旋转不同角度时，相机所对应的位姿如下： class PnPProblem { public: explicit PnPProblem(const double param[]); // custom constructor virtual ~PnPProblem(); cv::Point2f backproject3DPoint(const cv::Point3f& point3d); void estimatePoseRANSAC(const std::vector<cv::Point3f>& list_points3d, const std::vector<cv::Point2f>& list_points2d, int flags, cv::Mat& inliers, int iterationsCount, float reprojectionError, double confidence); // ... } // Custom constructor given the intrinsic camera parameters PnPProblem::PnPProblem(const double params[]) { // intrinsic camera parameters _A_matrix = cv::Mat::zeros(3, 3, CV_64FC1); _A_matrix.at<double>(0, 0) = params[0]; // [ fx 0 cx ] _A_matrix.at<double>(1, 1) = params[1]; // [ 0 fy cy ] _A_matrix.at<double>(0, 2) = params[2]; // [ 0 0 1 ] _A_matrix.at<double>(1, 2) = params[3]; _A_matrix.at<double>(2, 2) = 1; // rotation matrix, translation matrix, rotation-translation matrix _R_matrix = cv::Mat::zeros(3, 3, CV_64FC1); _t_matrix = cv::Mat::zeros(3, 1, CV_64FC1); _P_matrix = cv::Mat::zeros(3, 4, CV_64FC1); } // Estimate the pose given a list of 2D/3D correspondences with RANSAC and the method to use void PnPProblem::estimatePoseRANSAC ( const std::vector<Point3f>& list_points3d, // list with model 3D coordinates const std::vector<Point2f>& list_points2d, // list with scene 2D coordinates int flags, Mat& inliers, int iterationsCount, // PnP method; inliers container float reprojectionError, float confidence) // RANSAC parameters { // distortion coefficients, rotation vector and translation vector Mat distCoeffs = Mat::zeros(4, 1, CV_64FC1); Mat rvec = Mat::zeros(3, 1, CV_64FC1); Mat tvec = Mat::zeros(3, 1, CV_64FC1); // no initial approximations bool useExtrinsicGuess = false; // PnP + RANSAC solvePnPRansac(list_points3d, list_points2d, _A_matrix, distCoeffs, rvec, tvec, useExtrinsicGuess, iterationsCount, reprojectionError, confidence, inliers, flags); // converts Rotation Vector to Matrix Rodrigues(rvec, _R_matrix); _t_matrix = tvec; // set translation matrix this->set_P_matrix(_R_matrix, _t_matrix); // set rotation-translation matrix } // Backproject a 3D point to 2D using the estimated pose parameters cv::Point2f PnPProblem::backproject3DPoint(const cv::Point3f& point3d) { // 3D point vector [x y z 1]' cv::Mat point3d_vec = cv::Mat(4, 1, CV_64FC1); point3d_vec.at<double>(0) = point3d.x; point3d_vec.at<double>(1) = point3d.y; point3d_vec.at<double>(2) = point3d.z; point3d_vec.at<double>(3) = 1; // 2D point vector [u v 1]' cv::Mat point2d_vec = cv::Mat(4, 1, CV_64FC1); point2d_vec = _A_matrix * _P_matrix * point3d_vec; // Normalization of [u v]' cv::Point2f point2d; point2d.x = (float)(point2d_vec.at<double>(0) / point2d_vec.at<double>(2)); point2d.y = (float)(point2d_vec.at<double>(1) / point2d_vec.at<double>(2)); return point2d; } PnPProblem 类的调用如下：实例化 -> estimatePoseRansac() 估计位姿 -> backproject3DPoint() 画出位姿// Intrinsic camera parameters: UVC WEBCAMdouble f = 55; // focal length in mmdouble sx = 22.3, sy = 14.9; // sensor sizedouble width = 640, height = 480; // image sizedouble params_WEBCAM[] = { width * f / sx, // fx height * f / sy, // fy width / 2, // cx height / 2 }; // cy// instantiate PnPProblem classPnPProblem pnp_detection(params_WEBCAM);// RANSAC parametersint iterCount = 500; // number of Ransac iterations.float reprojectionError = 2.0; // maximum allowed distance to consider it an inlier.float confidence = 0.95; // RANSAC successful confidence.// OpenCV requires solvePnPRANSAC to minimally have 4 set of pointsif (good_matches.size() >= 4){ // -- Step 3: Estimate the pose using RANSAC approach pnp_detection.estimatePoseRANSAC(list_points3d_model_match, list_points2d_scene_match, pnpMethod, inliers_idx, iterCount, reprojectionError, confidence); // ... ..}// ... ... float fp = 5;vector<Point2f> pose2d;pose2d.push_back(pnp_detect_est.backproject3DPoint(Point3f(0, 0, 0))); // axis centerpose2d.push_back(pnp_detect_est.backproject3DPoint(Point3f(fp, 0, 0))); // axis xpose2d.push_back(pnp_detect_est.backproject3DPoint(Point3f(0, fp, 0))); // axis ypose2d.push_back(pnp_detect_est.backproject3DPoint(Point3f(0, 0, fp))); // axis zdraw3DCoordinateAxes(frame_vis, pose2d); // draw axes// ... ...实时目标跟踪的效果如下： 关键词：透视
