四.3D-2D:PnP问题求解 线性法

调用solvePnP函数

bool cv::solvePnP(objectPoints,imagePoints,cameraMatrix,distCoeffs,

OutputArray r, OutputArray t,

bool useExtrinsicGuess = false,int flags = SOLVEPNP_ITERATIVE )

缺点:P3P只利用了3个点信息(后续提出EPNP、UPNP利用更多信息)

           如果3D或2D点受到噪声影响,或者误匹配,算法失效。

主代码:

#include <iostream>

#include <vector>

#include <opencv2/opencv.hpp>

using namespace std;

using namespace cv;

 void find_feature_matches(

const Mat& img_1, const Mat& img_2,

std::vector<KeyPoint>& keypoints_1,

std::vector<KeyPoint>& keypoints_2,

std::vector< DMatch >& matches);

 Point2d pixel2cam(const Point2d& p, const Mat& K);

 int main()

{

Mat img1 = imread("1.png");

Mat img2 = imread("2.png");

Mat d1 = imread("1_depth.png");       // 深度图为16位无符号数,单通道图像

//1. 找到特征点和匹配关系

vector<KeyPoint> keypoint1, keypoint2;

vector<DMatch> matches;

find_feature_matches(img1,img2,keypoint1,keypoint2,matches);//自定义函数找特征匹配

        Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

//2. 第一张图像给出3D坐标,第二张给像素2d坐标

vector<Point3f> pts_3d;

vector<Point2f> pts_2d;

for (DMatch m : matches)

{

//3. 3D坐标由第一张的像素坐标转为相机坐标,再乘上深度,为世界坐标

       ushort d = d1.ptr<unsigned short> (int(keypoint1[m.queryIdx].pt.y))[int (keypoint1[m.queryIdx].pt.x)];

//d1是深度图,Mat类型的ptr是指针,获得像素点深度,首先得到行地址,之后列指针  (.pt.y)[.pt.x]

d = d / 1000;

Point2d p1 = pixel2cam(keypoint1[m.queryIdx].pt, K);//自定义函数像素坐标转相机坐标

/***************************************************************************************************************/

Point2d pixel2cam ( const Point2d& p, const Mat& K )

{

    return Point2d

           (

               ( p.x - K.at<double> ( 0,2 ) ) / K.at<double> ( 0,0 ),

               ( p.y - K.at<double> ( 1,2 ) ) / K.at<double> ( 1,1 )

           );

}

/***************************************************************************************************************/

Point3f p2(p1.x*d, p1.y*d, d);

pts_3d.push_back(p2);

pts_2d.push_back(keypoint2[m.trainIdx].pt);

cout << "3d-2d pairs" << pts_3d.size() << endl;

//4. 调用函数p3p,求解r,t

Mat R,r, t;

solvePnP(pts_3d, pts_2d, K, Mat(), r, t, false, SOLVEPNP_EPNP);

              Rodrigues(r, R); //solvepnp得到的是旋转向量r,需要通过罗德里格斯公式换为旋转矩阵R。即李代数向李群的转换。

 

}

}

  • 0
    点赞
  • 4
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值