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的乘积获得旋转矩阵R
double 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
drawChessboardCorners(img_corners, patternSize, corners, found);
SVDecomp(R, W, U, Vt);
findHomography
H /= norm;
Mat tvec = H.col(2);
