相机姿态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;
相关推荐
weixin_6352348217 小时前
帧采集器连接相机,Halcon连接相机
数码相机
列兵阿甘1 天前
知微传感Dkam系列3D相机SDK例程篇:Python获取内外参
python·数码相机·3d
格林威1 天前
UV 紫外相机在半导体制造领域的应用
人工智能·数码相机·opencv·计算机视觉·视觉检测·制造·uv
柔情的菜刀2 天前
基于偏振相机---太阳子午线计算技术
数码相机
格林威2 天前
近红外相机在半导体制造领域的应用
大数据·人工智能·深度学习·数码相机·视觉检测·制造·工业相机
qq_526099132 天前
工业级图像采集卡:工业智能化进程中的核心视觉中枢
数码相机
ARM+FPGA+AI工业主板定制专家3 天前
【JETSON+FPGA+GMSL】实测分享 | 如何实现激光雷达与摄像头高精度时间同步?
人工智能·数码相机·机器学习·fpga开发·机器人·自动驾驶
ARM+FPGA+AI工业主板定制专家3 天前
基于JETSON+FPGA+GMSL相机 vs 传统工业相机:高动态范围与低延迟如何重塑机器感知视觉?
人工智能·数码相机·机器学习·自动驾驶
咸鱼の猫3 天前
相机帧解析
数码相机
放羊郎3 天前
基于RTAB-Map和RRT的自主导航方案
人工智能·数码相机·计算机视觉