59 lines
1.6 KiB
C++
59 lines
1.6 KiB
C++
|
#include "PnP.h"
|
|||
|
|
|
|||
|
|
|
|||
|
|
class PnPInfo {
|
|||
|
|
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
|
|||
|
|
//Point2f reproject(Vector3d& xyz)
|
|||
|
|
//{
|
|||
|
|
// Matrix3d mat_intrinsic;
|
|||
|
|
// cv2eigen(intrinsic, mat_intrinsic);
|
|||
|
|
// //(u,v,1)^T = (1/Z) * K * (X,Y,Z)^T
|
|||
|
|
// auto result = (1.f / xyz[2]) * mat_intrinsic * (xyz);//<2F><><EFBFBD><EFBFBD>ǰ<EFBFBD><C7B0><EFBFBD>е<EFBFBD>λת<CEBB><D7AA>
|
|||
|
|
// return Point2f(result[0], result[1]);
|
|||
|
|
//}
|
|||
|
|
|
|||
|
|
//pnpλ<70>˽<EFBFBD><CBBD><EFBFBD>
|
|||
|
|
void PNP(const cv::Mat & img, std::vector<cv::Point2f> points , cv::Mat cam, cv::Mat dis){
|
|||
|
|
|
|||
|
|
//<2F><>PNP<4E>ο<EFBFBD><CEBF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
std::vector<cv::Point3f> points_world;
|
|||
|
|
|
|||
|
|
//˳ʱ<CBB3><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
points_world = {
|
|||
|
|
{-161.5, 52.0, 0.0},
|
|||
|
|
{161.5, 52.0, 0.0},
|
|||
|
|
{167.5, -45.0, 0.0},
|
|||
|
|
{-167.5, -45.0, 0.0},
|
|||
|
|
{0.0, 0.0, 0.0} };
|
|||
|
|
|
|||
|
|
cv::Mat rVec = cv::Mat::zeros(3, 1, CV_64FC1);
|
|||
|
|
cv::Mat tVec = cv::Mat::zeros(3, 1, CV_64FC1);
|
|||
|
|
|
|||
|
|
if(points.size() > 0)
|
|||
|
|
solvePnP(points_world, points, cam, dis, rVec, tVec, false, cv::SOLVEPNP_IPPE);
|
|||
|
|
|
|||
|
|
|
|||
|
|
// <20><>ȡ<EFBFBD><C8A1>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD>
|
|||
|
|
double xr = rVec.at<double>(0);
|
|||
|
|
double yr = rVec.at<double>(1);
|
|||
|
|
double zr = rVec.at<double>(2);
|
|||
|
|
|
|||
|
|
// <20><>ȡƽ<C8A1><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><C4B7><EFBFBD>
|
|||
|
|
double xt = tVec.at<double>(0);
|
|||
|
|
double yt = tVec.at<double>(1);
|
|||
|
|
double zt = tVec.at<double>(2);
|
|||
|
|
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD>ʾ
|
|||
|
|
std::string rvec_str = "rotation_vector: (" + std::to_string(xr) + ", " + std::to_string(yr) + ", " + std::to_string(zr) + ")";
|
|||
|
|
std::string tvec_str = "translation_vector: (" + std::to_string(xt) + ", " + std::to_string(yt) + ", " + std::to_string(zt) + ")";
|
|||
|
|
|
|||
|
|
putText(img, rvec_str, cv::Point2f(5, 25), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255,255,255), 1);
|
|||
|
|
putText(img, tvec_str, cv::Point2f(5, 50), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1);
|
|||
|
|
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|