triangle/PnP/pnp.cpp

//
// Created by zx on 22-11-2.
//

#include "pnp.h"
#include <chrono>

cv::Point2d pixel2cam ( const cv::Point2d& p, const cv::Mat& K )
{
    return cv::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 )
            );
}

P3d2d::P3d2d(cv::Mat K)
{
    K_=K.clone();
}
P3d2d::~P3d2d()
{

}

void P3d2d::Calibrate(std::vector<cv::Point3d> pts_3d,std::vector<cv::Point2d> pts_2d)
{
    cv::Mat r;
    solvePnP ( pts_3d, pts_2d, K_, cv::Mat(), r, t_, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    cv::Rodrigues ( r, R_ ); // r为旋转向量形式，用Rodrigues公式转换为矩阵
}

std::vector<cv::Point3d> P3d2d::execute(std::vector<cv::Point2d> pts_2d)
{

    cv::Mat TR=cv::Mat::zeros(3,3,R_.type());

    std::vector<cv::Point3d> world_points;


    for(int i=0;i<pts_2d.size();i++)
    {
        cv::Point2d pt = pixel2cam(pts_2d[i], K_);
        double a = pt.x;
        double b = pt.y;

        TR.at<double>(0, 0) = a * R_.at<double>(0, 0) + b * R_.at<double>(0, 1) + R_.at<double>(0, 2);
        TR.at<double>(0, 1) = -1;

        TR.at<double>(1, 0) = a * R_.at<double>(1, 0) + b * R_.at<double>(1, 1) + R_.at<double>(1, 2);
        TR.at<double>(2, 0) = a * R_.at<double>(2, 0) + b * R_.at<double>(2, 1) + R_.at<double>(2, 2);
        TR.at<double>(2, 2) = -1;

        cv::Mat T = -t_;
        cv::Mat dxz = TR.inv() * T;

        world_points.push_back(cv::Point3d(dxz.at<double>(1,0),0,dxz.at<double>(2,0)));

    }
    return world_points;

}