相机姿态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;
相关推荐
简单说鸭12 小时前
ScanNet: Richly-annotated 3D Reconstructions of Indoor Scenes 数据集构建
数码相机
文弱_书生21 小时前
关于对鱼眼相机图片进行畸变校正的两种思路
数码相机·鱼眼畸变校正·传统几何方法·深度学习方法
疾风铸境21 小时前
qt+halcon开发相机拍照软件步骤
数码相机·qt·halcon·拍照
北岛三生2 天前
ISP(图像信号处理器)
图像处理·数码相机·测试工具·计算机视觉·测试用例·模块测试
兔子不吃草~2 天前
鱼眼相机模型
数码相机
北岛三生2 天前
Camera tuning flow相机调试流程
图像处理·数码相机·测试工具·模块测试
南山二毛2 天前
机器人控制器开发(传感器层——奥比大白相机适配)
数码相机·机器人
格林威2 天前
棱镜的技术加持:线扫相机如何同时拍RGB和SWIR?
人工智能·深度学习·数码相机·yolo·计算机视觉
北岛三生3 天前
光学概念-相机模组(Camera Module)以及成像原理
图像处理·数码相机·模块测试
房开民3 天前
使用海康机器人相机SDK实现基本参数配置(C语言示例)
c语言·数码相机·机器人