相机姿态slovePnP

opencv slovePnP 物体的姿态 估计

物体的姿态(位置和方向)

通过已知的图像坐标点数组,和对应的世界坐标点数组,相机的内参,畸变参数,求解相机姿态,即旋转向量和平移向量,

例如:

cpp 复制代码
    cv::Mat im = cv::imread("headPose.jpg");
 
    // 2D image points. If you change the image, you need to change vector
    std::vector<cv::Point2d> image_points;
    image_points.push_back( cv::Point2d(359, 391) );    // Nose tip
    image_points.push_back( cv::Point2d(399, 561) );    // Chin
    image_points.push_back( cv::Point2d(337, 297) );     // Left eye left corner
    image_points.push_back( cv::Point2d(513, 301) );    // Right eye right corner
    image_points.push_back( cv::Point2d(345, 465) );    // Left Mouth corner
    image_points.push_back( cv::Point2d(453, 469) );    // Right mouth corner
 
    // 3D model points.
    std::vector<cv::Point3d> model_points;
    model_points.push_back(cv::Point3d(0.0f, 0.0f, 0.0f));               // Nose tip
    model_points.push_back(cv::Point3d(0.0f, -330.0f, -65.0f));          // Chin
    model_points.push_back(cv::Point3d(-225.0f, 170.0f, -135.0f));       // Left eye left corner
    model_points.push_back(cv::Point3d(225.0f, 170.0f, -135.0f));        // Right eye right corner
    model_points.push_back(cv::Point3d(-150.0f, -150.0f, -125.0f));      // Left Mouth corner
    model_points.push_back(cv::Point3d(150.0f, -150.0f, -125.0f));       // Right mouth corner
 
    // Camera internals
    double focal_length = im.cols; // Approximate focal length.
    Point2d center = cv::Point2d(im.cols/2,im.rows/2);
    cv::Mat camera_matrix = (cv::Mat_<double>(3,3) << focal_length, 0, center.x, 0 , focal_length, center.y, 0, 0, 1);
    cv::Mat dist_coeffs = cv::Mat::zeros(4,1,cv::DataType<double>::type); // Assuming no lens distortion
 
    cout << "Camera Matrix " << endl << camera_matrix << endl ;
    // Output rotation and translation
    cv::Mat rotation_vector; // Rotation in axis-angle form
    cv::Mat translation_vector;
 
    // Solve for pose
    cv::solvePnP(model_points, image_points, camera_matrix, dist_coeffs, rotation_vector, translation_vector);
 
    // Project a 3D point (0, 0, 1000.0) onto the image plane.
    // We use this to draw a line sticking out of the nose
 
    vector<Point3d> nose_end_point3D;
    vector<Point2d> nose_end_point2D;
    nose_end_point3D.push_back(Point3d(0,0,1000.0));
 
    projectPoints(nose_end_point3D, rotation_vector, translation_vector, camera_matrix, dist_coeffs, nose_end_point2D);
 
    for(int i=0; i < image_points.size(); i++)
    {
        circle(im, image_points[i], 3, Scalar(0,0,255), -1);
    }
 
    cv::line(im,image_points[0], nose_end_point2D[0], cv::Scalar(255,0,0), 2);
 
    cout << "Rotation Vector " << endl << rotation_vector << endl;
    cout << "Translation Vector" << endl << translation_vector << endl;
 
    cout <<  nose_end_point2D << endl;

计算相机到被测物中心的实际距离

cpp 复制代码
float distance = sqrt(translation_vector.at<double>(0,0) * translation_vector.at<double>(0,0) + translation_vector.at<double>(1,0) * translation_vector.at<double>(1,0) + translation_vector.at<double>(2,0) * translation_vector.at<double>(2,0)) / 10; 

#求解旋转平移矩阵

cpp 复制代码
 cv::solvePnP(model_points, image_points, camera_matrix, dist_coeffs, rotation_vector, translation_vector);

Rodrigues()

Rodrigues()可以将旋转向量转化为旋转矩阵,也可以将旋转矩阵转化为旋转向量。

求解欧拉角

cpp 复制代码
///*
//double rm[9];
//cv::Mat rotM(3, 3, CV_64FC1, rm);
//Rodrigues(rvec, rotM);*/
cv::Rodrigues(rotation_vec, rotation_mat);
cv::hconcat(rotation_mat, translation_vec, pose_mat);
cv::decomposeProjectionMatrix(pose_mat, out_intrinsics, out_rotation, out_translation, cv::noArray(), cv::noArray(), cv::noArray(), euler_angle);
//

相机的位置计算

公式计算相机在世界框架中的位置:

cam_worl_pos = - 逆® * tvec。(这个公式在很多博客都验证过)

Calculate camera position

camera_position = -np.matrix(rotation_mat).T * np.matrix(translation_vector)

坐标轴如何画

三维姿态欧式角解算

  1. 旋转矩阵R


可以用下面代码求解 以上的旋转矩阵转换成三维姿态角(滚轮角、偏航角、俯仰角),atan()求反三角函数

r11-r33从旋转矩阵中提出,为double型变量

cpp 复制代码
/计算出相机坐标系的三轴旋转欧拉角,旋转后可以转出世界坐标系。
//旋转顺序为z、y、x
const double PI = 3.141592653;
double thetaz = atan2(r21, r11) / PI * 180;
double thetay = atan2(-1 * r31, sqrt(r32*r32 + r33*r33)) / PI * 180;
double thetax = atan2(r32, r33) / PI * 180;
相关推荐
lingling00911 天前
2025 年焊接相机十大品牌测评:抗光耐高温解决方案深度解析
数码相机
人类发明了工具11 天前
【机器人-深度估计】双目深度估计原理解析
数码相机·机器人·深度估计·双目深度估计
朝风工作室12 天前
从零理解鱼眼相机的标定与矫正(含 OpenCV 代码与原理讲解)
人工智能·数码相机·opencv
东风西巷12 天前
MolyCamCCD复古胶片相机:复古质感,时尚出片
android·数码相机·智能手机·软件需求
lingling00915 天前
锌锭工业相机:迁移科技驱动金属制造自动化新高度
数码相机
三维频道17 天前
多相机数字图像相关 DIC 技术:精准测量桁架静载扭转全场变形,赋能建筑结构安全评估
数码相机·安全·多相机数字图像相关 dic·桁架静载扭转测试·全场变形测量·建筑结构安全评估’·非接触式测量技术
江山如画,佳人北望18 天前
VINS-Fusion 简介、安装、编译、数据集/相机实测
数码相机
SHARE赛尔20 天前
赛尔发布SHARE 5系列航测相机,外业更高效,建模更优质
数码相机
元器猫21 天前
工业自动化发展:工业相机在机器视觉领域的应用趋势
运维·数码相机·自动化
橙色小博22 天前
摄影入门:相机基本参数解析
数码相机·相机·摄影