【opencv】教程代码 —features2D（7）根据单应性矩阵估计相机坐标系下的物体位姿...

pose_from_homography.cpp从图像中找到棋盘角点并进行姿态估计

从图像中找到棋盘角点并显示

计算角点在世界坐标系中的位置

读取相机内参和畸变系数并校正图像中的角点

计算从3D点到2D点的单应性矩阵

通过奇异值分解(SVD)优化对旋转矩阵的估计

基于单应矩阵分解及其优化结果，估计姿态，绘制世界坐标系

#include <iostream>                         // 包含标准输入输出头文件
#include <opencv2/core.hpp>                 // 包含OpenCV核心功能的头文件
#include <opencv2/imgproc.hpp>              // 包含OpenCV图像处理功能的头文件
#include <opencv2/calib3d.hpp>              // 包含OpenCV摄像机标定和3D重建的头文件
#include <opencv2/highgui.hpp>              // 包含OpenCV图像显示功能的头文件using namespace std;                        // 使用命名空间std，避免每次使用标准库时都要加std::
using namespace cv;                         // 使用命名空间cv，避免每次调用OpenCV函数时都要加cv::namespace                                   // 匿名命名空间，内部的变量和函数仅在本文件内有效
{
enum Pattern { CHESSBOARD, CIRCLES_GRID, ASYMMETRIC_CIRCLES_GRID }; // 枚举类型Pattern，代表不同的标定板类型// 计算棋盘角点的函数
void calcChessboardCorners(Size boardSize, float squareSize, vector<Point3f>& corners, Pattern patternType = CHESSBOARD)
{corners.resize(0);                      // 清空角点向量switch (patternType)                    // 根据不同的标定板类型进行计算{case CHESSBOARD:case CIRCLES_GRID://! [compute-chessboard-object-points]for( int i = 0; i < boardSize.height; i++ )for( int j = 0; j < boardSize.width; j++ )corners.push_back(Point3f(float(j*squareSize), // 将每个角点的3D位置按行和列添加到corners向量中float(i*squareSize), 0));//! [compute-chessboard-object-points]break;case ASYMMETRIC_CIRCLES_GRID:for( int i = 0; i < boardSize.height; i++ )for( int j = 0; j < boardSize.width; j++ )corners.push_back(Point3f(float((2*j + i % 2)*squareSize), // 对于非对称圆形网格，计算并存储3D位置float(i*squareSize), 0));break;default:CV_Error(Error::StsBadArg, "Unknown pattern type\n"); // 如果不是上述类型，默认抛出异常}
}// 从共面点估计姿态
void poseEstimationFromCoplanarPoints(const string &imgPath, const string &intrinsicsPath, const Size &patternSize,const float squareSize)
{Mat img = imread( samples::findFile( imgPath) );         // 读取图像文件Mat img_corners = img.clone(), img_pose = img.clone();   // 克隆图像用于角点检测和姿态估计的显示//! [find-chessboard-corners]vector<Point2f> corners;                                 // 存储2D角点的容器bool found = findChessboardCorners(img, patternSize, corners); // 使用OpenCV函数查找棋盘角点//! [find-chessboard-corners]if (!found)                                              // 如果没有找到角点{cout << "Cannot find chessboard corners." << endl;   // 打印错误信息return;                                              // 直接返回}drawChessboardCorners(img_corners, patternSize, corners, found); // 在图像上绘制检测到的角点imshow("Chessboard corners detection", img_corners);     // 显示带有角点的图像//! [compute-object-points]vector<Point3f> objectPoints;                            // 存储3D角点的容器// 使用自定义函数计算3D角点的位置calcChessboardCorners(patternSize, squareSize, objectPoints);vector<Point2f> objectPointsPlanar; // 用于存储投影后的2D点                     for (size_t i = 0; i < objectPoints.size(); i++){objectPointsPlanar.push_back(Point2f(objectPoints[i].x, objectPoints[i].y)); // 把3D点投影到2D平面}//! [compute-object-points]//! [load-intrinsics]FileStorage fs( samples::findFile( intrinsicsPath ), FileStorage::READ); // 打开摄像机内参数文件Mat cameraMatrix, distCoeffs;                           // 定义摄像机的内参数矩阵和畸变系数fs["camera_matrix"] >> cameraMatrix;                    // 从文件中读取摄像机矩阵fs["distortion_coefficients"] >> distCoeffs;            // 从文件中读取畸变系数//! [load-intrinsics]//! [compute-image-points]vector<Point2f> imagePoints;  // 存储畸变矫正后的2D点的容器undistortPoints(corners, imagePoints, cameraMatrix, distCoeffs); // 畸变矫正//! [compute-image-points]//! // 计算从3D点到2D点的单应性矩阵 [estimate-homography]Mat H = findHomography(objectPointsPlanar, imagePoints);cout << "H:\n" << H << endl; // 打印出单应性矩阵//! [estimate-homography]//! [pose-from-homography]// 归一化确保||c1|| = 1 // 计算归一化的因子double norm = sqrt(H.at<double>(0,0)*H.at<double>(0,0) +  H.at<double>(1,0)*H.at<double>(1,0) +H.at<double>(2,0)*H.at<double>(2,0));H /= norm;    // 归一化单应性矩阵Mat c1  = H.col(0);   // 获取单应性矩阵的第一列Mat c2  = H.col(1);     // 获取单应性矩阵的第二列Mat c3 = c1.cross(c2);   // 计算第三个轴的方向Mat tvec = H.col(2);  // 获取平移向量Mat R(3, 3, CV_64F);    // 定义旋转矩阵for (int i = 0; i < 3; i++){    // 分别设置旋转矩阵R的列R.at<double>(i,0) = c1.at<double>(i,0);            R.at<double>(i,1) = c2.at<double>(i,0);R.at<double>(i,2) = c3.at<double>(i,0);}//! [pose-from-homography]//!旋转矩阵的极分解 [polar-decomposition-of-the-rotation-matrix]cout << "R (before polar decomposition):\n" << R << "\ndet(R): " << determinant(R) << endl; // 打印未分解的旋转矩阵和其行列式Mat_<double> W, U, Vt;    // 定义用于奇异值分解的矩阵SVDecomp(R, W, U, Vt);   // 奇异值分解R = U*Vt;       // 通过U和Vt的乘积获得旋转矩阵Rdouble det = determinant(R);   // 计算R的行列式if (det < 0)    // 如果行列式小于0，调整Vt矩阵再重新计算R{Vt.at<double>(2,0) *= -1;Vt.at<double>(2,1) *= -1;Vt.at<double>(2,2) *= -1;R = U*Vt;}// 打印分解后的旋转矩阵和其行列式cout << "R (after polar decomposition):\n" << R << "\ndet(R): " << determinant(R) << endl; //! [polar-decomposition-of-the-rotation-matrix]//! [display-pose]Mat rvec;                                               // 定义旋转向量Rodrigues(R, rvec);                                     // 将旋转矩阵转换为旋转向量drawFrameAxes(img_pose, cameraMatrix, distCoeffs, rvec, tvec, 2*squareSize); // 在图像上绘制坐标轴imshow("Pose from coplanar points", img_pose);          // 显示带有姿态的图像waitKey();                                              // 等待用户按键//! [display-pose]
}const char* params= "{ help h         |       | print usage }"             // 命令行参数的初始化和解析"{ image          | left04.jpg | path to a chessboard image }""{ intrinsics     | left_intrinsics.yml | path to camera intrinsics }""{ width bw       | 9     | chessboard width }""{ height bh      | 6     | chessboard height }""{ square_size    | 0.025 | chessboard square size }";
}int main(int argc, char *argv[])                            // 主函数
{CommandLineParser parser(argc, argv, params);           // 初始化命令行解析器if (parser.has("help"))                                 // 如果有帮助信息请求{parser.about("Code for homography tutorial.\n""Example 1: pose from homography with coplanar points.\n"); // 打印该程序的简介parser.printMessage();                              // 打印帮助信息return 0;}Size patternSize(parser.get<int>("width"), parser.get<int>("height")); // 获取棋盘的尺寸float squareSize = (float) parser.get<double>("square_size");          // 获取棋盘的方格大小poseEstimationFromCoplanarPoints(parser.get<String>("image"),          // 调用姿态估计函数parser.get<String>("intrinsics"),patternSize, squareSize);return 0;
}

此代码段的功能是利用OpenCV库从图像中找到棋盘角点并进行姿态估计。它包括以下主要部分：

使用摄像头拍摄的含有标定板（棋盘）的图像来找到角点；
使用棋盘角点，并结合已知的棋盘格尺寸来计算这些角点在空间中的位置；
根据摄像头的内参数文件来读取摄像头内参数并校正图像畸变；
计算2D图像点与3D空间点的单应性矩阵；
通过奇异值分解(SVD)优化对旋转矩阵的估计；
基于单应矩阵分解及其优化结果，估计姿态，并在图像上显示检测到的棋盘角点和计算出的姿态；

该程序适用于计算机视觉和机器人视觉领域中，特别是在相机校准和空间位置估计场景中。通过对摄像头捕获的物体进行准确的姿态估计，可以实现机器人的精准定位和路径规划。

终端输出：

H:
[2.933817142664291, -0.03262345741700864, -0.2971965462459956;-0.0447851314919854, 3.002917156419064, -0.203610190903598;-0.7132714521558404, -0.3361256089579286, 0.9999999999999999]
R (before polar decomposition):
[0.9715880815225828, -0.01080386433887985, 0.2365582243054619;-0.0148314287738872, 0.9944718355305832, 0.1107036064935512;-0.2362130998987821, -0.1113142434165035, 0.9660567460669759]
det(R): 1.00148
R (after polar decomposition):
[0.9715941332941491, -0.01130419642660607, 0.2363832804822285;-0.0153517308169317, 0.9937440091258498, 0.110621736957108;-0.2361549586803454, -0.1111083231339211, 0.9653423102822399]
det(R): 1