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);//解算前进行单位转换
|
|
// return Point2f(result[0], result[1]);
|
|
//}
|
|
|
|
//pnp位姿解算
|
|
void PNP(const cv::Mat & img, std::vector<cv::Point2f> points , cv::Mat cam, cv::Mat dis){
|
|
|
|
//给PNP参考的世界坐标点
|
|
std::vector<cv::Point3f> points_world;
|
|
|
|
//顺时针的世界坐标
|
|
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);
|
|
|
|
|
|
// 提取旋转向量的分量
|
|
double xr = rVec.at<double>(0);
|
|
double yr = rVec.at<double>(1);
|
|
double zr = rVec.at<double>(2);
|
|
|
|
// 提取平移向量的分量
|
|
double xt = tVec.at<double>(0);
|
|
double yt = tVec.at<double>(1);
|
|
double zt = tVec.at<double>(2);
|
|
|
|
// 构造旋转向量和平移向量的字符串表示
|
|
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);
|
|
|
|
|
|
}
|
|
|