AprilTags c++识别

使用开源库：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));

}