OpenCV actual combat (24) - camera pose estimation

0. Preface

Once the camera is calibrated , it's time to relate the captured image to the physical world. If 3Dthe structure is known, then it is possible to predict how the object is projected onto the camera's sensor, and the process of image formation is described by the projection equation. When most of the terms of the equation are known, it is possible to infer the values of other elements ( 2DOR 3D) by looking at some images. Camera pose estimation is to solve the coordinates and rotation angle of the camera in the coordinate system through several feature points with known coordinates and the imaging positions of these points in the photo. In this section, we investigate the problem of camera pose estimation when observing a known 3Dstructure .

1. Camera Pose Estimation

We consider a simple object - a bench in a park, and took the following image using the camera and lens system we calibrated in the previous section, and manually identified 82 estimate( camera pose estimation):

image with markers
By accessing this object, many 3Dphysical . The bench consists of 242.5cmx53.5cmx9cma seat and 242.5cmx24cmx9cma backrest fixed 12cmabove . Using this information, we can easily 8deduce 3Dthe coordinates of an identified point in an object-centered reference frame (fixing the origin at the left end of the intersection between the two planes).

(1) The first thing to do in the algorithm is to create a cv::Point3fvector :

// 输入图像点
std::vector<cv::Point2f> imagePoints;
imagePoints.push_back(cv::Point2f(207, 60));
imagePoints.push_back(cv::Point2f(670, 103));
imagePoints.push_back(cv::Point2f(637, 240));
imagePoints.push_back(cv::Point2f(183, 156));
imagePoints.push_back(cv::Point2f(185, 179));
imagePoints.push_back(cv::Point2f(633, 273));
imagePoints.push_back(cv::Point2f(541, 332));
imagePoints.push_back(cv::Point2f(73, 213));

(2) Now the question is where is the camera relative to these points when the above photo was taken. Since the image coordinates of these known points on 2Dthe image plane are also known, cv::solvePnPit is easy to estimate the camera position using the function. Here, we manually 3Destablish 2Dthe correspondence between and points, but we can also get this information automatically:

// 输入对象点
std::vector<cv::Point3f> objectPoints;
objectPoints.push_back(cv::Point3f(0, 45, 0));
objectPoints.push_back(cv::Point3f(242.5, 45, 0));
objectPoints.push_back(cv::Point3f(242.5, 21, 0));
objectPoints.push_back(cv::Point3f(0, 21, 0));
objectPoints.push_back(cv::Point3f(0, 9, -9));
objectPoints.push_back(cv::Point3f(242.5, 9, -9));
objectPoints.push_back(cv::Point3f(242.5, 9, 44.5));
objectPoints.push_back(cv::Point3f(0, 9, 44.5));
// 根据 3D/2D 点计算相机姿态
cv::Mat rvec, tvec;
cv::solvePnP(objectPoints, imagePoints,         // 对应的 3D/2D 点
                cameraMatrix, cameraDistCoeffs, // 标定
                rvec, tvec);                    // 输出姿态
cv::Mat rotation;
// 将向量 rvec 转换为 3x3 旋转矩阵 
cv::Rodrigues(rvec, rotation);

cv::solvePnPThe function computes a rigid transformation (i.e. rotation and translation components) that brings the object coordinates into a camera-centered frame of reference (i.e. the frame of reference with the focal point as the origin). The rotation computed by this function is given in the form of 3Da vector where the rotation to be applied is described by a unit vector (i.e. the axis of rotation) around which the object is rotated by a certain angle, this representation is also known as the Rodrigues rotation formula ( ) Rodrigues' rotation formula. OpenCVIn , the angle of rotation corresponds to the norm of the output rotation vector, which is aligned with the axis of rotation. The 3D rotation matrix that appears in the projection equation can be obtained using cv::Rodriguesthe function .

(3) The camera pose recovery process is very simple. In order to know whether we have obtained the correct camera and object pose information, we can use cv::vizthe module visually evaluate the results, and cv::vizthe module can visualize 3Dthe information . Use this module to display simple 3Drepresentations :

3D representation
It may be difficult to judge the quality of the pose recovery just by looking at this image, but you can choose to use your mouse to move around 3Din the to get a better idea of the resulting solution.
In this section, we assume that 3Dthe structure , the correspondence between the object point set and the image point set are known, and the intrinsic parameters of the camera can also be known through calibration (ie, the elements of the first matrix); according to the projection Equations, which means that there are some points whose coordinates are known, such as $(X, Y, Z)$ 和 $(x, y)$ . Only the second matrix is unknown, which is a matrix containing the extrinsic parameters of the camera (i.e. camera/object pose information). Therefore, our goal is to recover these unknown parameters from observations of 3DsceneThis problem is calledPnP(Perspective-n-Point) problem.
Rotation has three degrees of freedom (it can rotate about three axes), as does translation, so there are six unknowns in total. For each object point and image point correspondence, the projection equation gives three algebraic equations, but since the projective equation has a scaling factor, there are only two independent equations. Therefore, at least three points are required to solve this system of equations, and more points give more reliable estimates.
OpenCVSeveral different algorithms are implemented in cv::solvePnPthe functionThe default method is based on optimizing reprojection errors, and minimizing such errors is the best strategy for obtaining accurate 3DinformationIn PnPthe problemthe distance3D between the projected point (obtained by applying the projection equation) and the input observed image pointAlso provided is the function,) to solve the problemThis means that the correspondence of some object points and image points may be wrong, the function returns the correspondences identified as outliers, which is very useful for automatically obtained correspondences.2D
OpenCVcv::solvePnPRansacRANdom SAmple ConsensusRANSACPnP

2. 3D visualization module cv::Viz

When processing 3Dinformation , it is often difficult to verify the obtained solutions. To this end, OpenCVa powerful visualization module is provided to facilitate the development and debugging of 3Dvision algorithms. It allows the insertion of points, lines, cameras, and other objects in the virtual 3Denvironment , as well as interactive visualization from different perspectives.
cv::Vizis a module of OpenCVthe library , built on top of the Visualization Toolkit ( Visualization Toolkit, VTK), a powerful framework for 3Dcomputer graphics. Using cv::viz, a 3Dvirtual into which various objects can be added. Creates a visualization window showing the environment from the given perspective. In this section, learn how to display content in cv::viza window , which can respond to mouse events (by rotation and translation), and this section will introduce cv::vizthe basic usage of the module.

(1) First create a visualization window, for example, with a white background:

// 创建 viz 窗口
cv::viz::Viz3d visualizer("Viz window");
visualizer.setBackgroundColor(cv::viz::Color::white());

(2) Next, create dummy objects and insert them into the scene. OpenCVcontains a variety of predefined objects that contain tools for creating virtual pinhole cameras:

// 创建虚拟相机
cv::viz::WCameraPosition cam(cMatrix,   // 内在矩阵 
                        image,          // 图像平面
                        30.0,           // 缩放因子
                        cv::viz::Color::black());
visualizer.showWidget("Camera", cam);

(3) cMatrix The variable is cv::Matx33dthe instance , i.e. cv::Matx<double,3,3>, it contains the intrinsic camera parameters obtained from the calibration. By default, the camera is inserted at the origin of the coordinate system, to represent the bench we use two rectangular cuboid objects:

// 使用长方体创建虚拟长凳
cv::viz::WCube plane1(cv::Point3f(0.0, 45.0, 0.0), 
                        cv::Point3f(242.5, 21.0, -9.0), 
                        true,           // 显示线框
                        cv::viz::Color::blue());
plane1.setRenderingProperty(cv::viz::LINE_WIDTH, 4.0);
cv::viz::WCube plane2(cv::Point3f(0.0, 9.0, -9.0), 
                        cv::Point3f(242.5, 0.0, 44.5), 
                        true,           // 显示线框
                        cv::viz::Color::blue());
plane2.setRenderingProperty(cv::viz::LINE_WIDTH, 4.0);
// 在环境中添加虚拟对象
visualizer.showWidget("top", plane1);
visualizer.showWidget("bottom", plane2);

(4) The virtual bench is also added at the origin, and then it is moved to the position centered on the camera (using cv::solvePnPthe function ). setWidgetPosemethod can be used to add operations, this function applies the rotation and translation components of the estimated motion:

cv::Mat rotation;
// 将向量 rvec 转换为 3x3 旋转矩阵 
cv::Rodrigues(rvec, rotation);
// 移动长凳
cv::Affine3d pose(rotation, tvec);
visualizer.setWidgetPose("top", pose);
visualizer.setWidgetPose("bottom", pose);

(5) Finally, create a loop that continuously displays the visualization window, and 1mspause to listen to mouse events:

// 可视化虚拟环境
while(cv::waitKey(100)==-1 && !visualizer.wasStopped())
{
    
    
    visualizer.spinOnce(1,      // 暂停 1ms 
                        true);  // 重绘
}

The loop stops when the visualization window is closed or a key is pressed on the OpenCVimage window. Animation is created by applying motion to the object in this loop.

3. Complete code

The complete code cameraPose.cppis as follows:

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/viz.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <iostream>

int main() {
    
    
    // 读取相机标定参数
    cv::Mat cameraMatrix;
    cv::Mat cameraDistCoeffs;
    cv::FileStorage fs("calib.xml", cv::FileStorage::READ);
    fs["Intrinsic"] >> cameraMatrix;
    fs["Distortion"] >> cameraDistCoeffs;
    std::cout << " Camera intrinsic: " << cameraMatrix.rows << "x" << cameraMatrix.cols << std::endl;
    std::cout << cameraMatrix.at<double>(0, 0) << " " << cameraMatrix.at<double>(0, 1) << " " << cameraMatrix.at<double>(0, 2) << std::endl;
    std::cout << cameraMatrix.at<double>(1, 0) << " " << cameraMatrix.at<double>(1, 1) << " " << cameraMatrix.at<double>(1, 2) << std::endl;
    std::cout << cameraMatrix.at<double>(2, 0) << " " << cameraMatrix.at<double>(2, 1) << " " << cameraMatrix.at<double>(2, 2) << std::endl << std::endl;
    cv::Matx33d cMatrix(cameraMatrix);
    // 输入图像点
    std::vector<cv::Point2f> imagePoints;
    imagePoints.push_back(cv::Point2f(207, 60));
    imagePoints.push_back(cv::Point2f(670, 103));
    imagePoints.push_back(cv::Point2f(637, 240));
    imagePoints.push_back(cv::Point2f(183, 156));
    imagePoints.push_back(cv::Point2f(185, 179));
    imagePoints.push_back(cv::Point2f(633, 273));
    imagePoints.push_back(cv::Point2f(541, 332));
    imagePoints.push_back(cv::Point2f(73, 213));
    // 输入对象点
    std::vector<cv::Point3f> objectPoints;
    objectPoints.push_back(cv::Point3f(0, 45, 0));
    objectPoints.push_back(cv::Point3f(242.5, 45, 0));
    objectPoints.push_back(cv::Point3f(242.5, 21, 0));
    objectPoints.push_back(cv::Point3f(0, 21, 0));
    objectPoints.push_back(cv::Point3f(0, 9, -9));
    objectPoints.push_back(cv::Point3f(242.5, 9, -9));
    objectPoints.push_back(cv::Point3f(242.5, 9, 44.5));
    objectPoints.push_back(cv::Point3f(0, 9, 44.5));
    // 读取图像
    cv::Mat image = cv::imread("example_1.jpg");
    // 绘制图像点
    for (int i = 0; i < 8; i++) {
    
    
        cv::circle(image, imagePoints[i], 3, cv::Scalar(0, 0, 0),2);
    }
    cv::imshow("An image of a bench", image);
    // 创建 viz 窗口
    cv::viz::Viz3d visualizer("Viz window");
    visualizer.setBackgroundColor(cv::viz::Color::white());
    // 创建虚拟相机
    cv::viz::WCameraPosition cam(cMatrix,   // 内在矩阵 
                            image,          // 图像平面
                            30.0,           // 缩放因子
                            cv::viz::Color::black());
    // 使用长方体创建虚拟长凳
    cv::viz::WCube plane1(cv::Point3f(0.0, 45.0, 0.0), 
                            cv::Point3f(242.5, 21.0, -9.0), 
                            true,           // 显示线框
                            cv::viz::Color::blue());
    plane1.setRenderingProperty(cv::viz::LINE_WIDTH, 4.0);
    cv::viz::WCube plane2(cv::Point3f(0.0, 9.0, -9.0), 
                            cv::Point3f(242.5, 0.0, 44.5), 
                            true,           // 显示线框
                            cv::viz::Color::blue());
    plane2.setRenderingProperty(cv::viz::LINE_WIDTH, 4.0);
    // 在环境中添加虚拟对象
    visualizer.showWidget("top", plane1);
    visualizer.showWidget("bottom", plane2);
    visualizer.showWidget("Camera", cam);
    // 根据 3D/2D 点计算相机姿态
    cv::Mat rvec, tvec;
    cv::solvePnP(objectPoints, imagePoints,         // 对应的 3D/2D 点
                    cameraMatrix, cameraDistCoeffs, // 标定
                    rvec, tvec);                    // 输出姿态
    std::cout << " rvec: " << rvec.rows << "x" << rvec.cols << std::endl;
    std::cout << " tvec: " << tvec.rows << "x" << tvec.cols << std::endl;
    cv::Mat rotation;
    // 将向量 rvec 转换为 3x3 旋转矩阵 
    cv::Rodrigues(rvec, rotation);
    // 移动长凳
    cv::Affine3d pose(rotation, tvec);
    visualizer.setWidgetPose("top", pose);
    visualizer.setWidgetPose("bottom", pose);
    // 可视化虚拟环境
    while(cv::waitKey(100)==-1 && !visualizer.wasStopped())
    {
    
    
        visualizer.spinOnce(1,      // 暂停 1ms 
                            true);  // 重绘
    }
    cv::waitKey();
    return 0;
}

summary

Camera pose estimation is to solve the coordinates and rotation angle of the camera in the coordinate system through several feature points with known coordinates and the imaging positions of these points in the photo. This section uses cv::solvePnPthe function Visualize the scene OpenCVin 3D.

series link