使用开源库:https://github.com/PrieureDeSion/apriltags-cpp
mTagDetector = new AprilTags::TagDetector(AprilTags::tagCodes25h7);
string imgpath = "./1.jpg";
Mat image = imread(imgpath);
Mat imageGray;
cv::cvtColor(image, imageGray, cv::COLOR_BGR2GRAY);
vector<AprilTags::TagDetection> detections = mTagDetector->extractTags(imageGray);
cout << detections.size() << " tags detected:" << endl;
for (int i=0; i< detections.size(); i++) {
}
//计算坐标与姿态
Eigen::Vector3d translation;
Eigen::Matrix3d rotation;
//fx,fy,px,py相机参数 tagSize标签码尺寸
detection.getRelativeTranslationRotation(tagSize, fx, fy, px, py,
translation, rotation);
Eigen::Matrix3d F;
F <<
1, 0, 0,
0, -1, 0,
0, 0, 1;
Eigen::Matrix3d fixed = F*rotation;
double yaw, pitch, roll;
toEuler(fixed , yaw, pitch, roll);
qDebug() << " distance=" << translation.norm()
<< "m, x=" << translation(0)
<< ", y=" << translation(1)
<< ", z=" << translation(2)
<< ", yaw=" << yaw
<< ", pitch=" << pitch
<< ", roll=" << roll;
//绘制结果
void drawPred(AprilTags::TagDetection& detection, Mat& frame)
{
Point a(detection.p[0].first, detection.p[0].second);
Point b(detection.p[1].first, detection.p[1].second);
Point c(detection.p[2].first, detection.p[2].second);
Point d(detection.p[3].first, detection.p[3].second);
cv::line(frame, a, b, Scalar(0, 255, 0), 2, LINE_AA);
cv::line(frame, b, c, Scalar(0, 255, 0), 2, LINE_AA);
cv::line(frame, c, d, Scalar(0, 255, 0), 2, LINE_AA);
cv::line(frame, d, a, Scalar(0, 255, 0), 2, LINE_AA);
string label = format("%d", detection.id);
cv::putText(frame, label, a, cv::FONT_HERSHEY_SIMPLEX, 0.75, Scalar(0, 255, 0), 2);
}
void toEuler(const Eigen::Matrix3d& w, double& yaw, double& pitch, double& roll) {
yaw = rad(atan2(w(1,0), w(0,0)));
double c = cos(yaw);
double s = sin(yaw);
pitch = rad(atan2(-w(2,0), w(0,0)*c + w(1,0)*s));
roll = rad(atan2(w(0,2)*s - w(1,2)*c, -w(0,1)*s + w(1,1)*c));
}