Con la versione 3 di Apriltag e' possibile avere la pose estimation del tag (ho usato 41h12)
il repository di riferimento e' https://github.com/zhenzhenxiang/apriltag3
(ve ne sono altri ma ho avuto problemi di compilazione all'interno di Debian Testing)
Si clona di github il progetto (al momento 3.4.3) e si crea un folder ap3 dove inserire il seguente Makefile e il successivo main.cpp
# Adjust these paths if needed
APRILTAG_DIR = ../
CXX = g++
CXXFLAGS = -Wall -O2 -std=c++11
# Paths for AprilTag
INCLUDES = -I$(APRILTAG_DIR) -I$(APRILTAG_DIR)/common
LIBS = -L$(APRILTAG_DIR) -lapriltag
# Use OpenCV4 pkg-config flags
OPENCV_CFLAGS := $(shell pkg-config --cflags opencv4)
OPENCV_LIBS := $(shell pkg-config --libs opencv4)
# Source file
SRC = main.cpp
OUT = pos_tag
all: $(OUT)
$(OUT): $(SRC)
$(CXX) $(CXXFLAGS) $(SRC) $(INCLUDES) $(LIBS) $(OPENCV_CFLAGS) $(OPENCV_LIBS) -o $(OUT)
clean:
rm -f $(OUT)
#include <opencv2/opencv.hpp>
#include <iostream>
extern "C" {
#include "apriltag.h"
#include "tagStandard41h12.h"
#include "common/getopt.h"
#include "apriltag_pose.h"
}
int main(int argc, char* argv[]) {
// --- Load image ---
if (argc < 2) {
std::cerr << "Usage: " << argv[0] << " <image_path>" << std::endl;
return 1;
}
std::string imagePath = argv[1];
cv::Mat image = cv::imread(imagePath, cv::IMREAD_GRAYSCALE);
if (image.empty()) {
std::cerr << "Failed to load image\n";
return -1;
}
// --- Setup detector ---
apriltag_family_t *tf = tagStandard41h12_create();
apriltag_detector_t *td = apriltag_detector_create();
apriltag_detector_add_family(td, tf);
td->quad_decimate = 1.0;
td->quad_sigma = 0.0;
td->nthreads = 4;
td->debug = false;
td->refine_edges = true;
// --- Convert OpenCV image to Apriltag image ---
image_u8_t im = {
.width = image.cols,
.height = image.rows,
.stride = image.cols,
.buf = image.data
};
// --- Detect tags ---
zarray_t *detections = apriltag_detector_detect(td, &im);
//std::cout << "Detected " << zarray_size(detections) << " tags.\n";
// --- Camera parameters (replace with calibrated values) ---
double fx = 663.64233754; // focal length x
double fy = 664.86615495; // focal length y
double cx = image.cols / 2.0;
double cy = image.rows / 2.0;
double tagsize = 0.150; // in meters
for (int i = 0; i < zarray_size(detections); i++) {
apriltag_detection_t *det;
zarray_get(detections, i, &det);
std::cout << det->id;
apriltag_detection_info_t info;
info.det = det;
info.tagsize = tagsize;
info.fx = fx;
info.fy = fy;
info.cx = cx;
info.cy = cy;
apriltag_pose_t pose;
estimate_tag_pose(&info, &pose);
// --- Translation ---
double x = pose.t->data[0];
double y = pose.t->data[1];
double z = pose.t->data[2];
std::cout << ";" << x << ";" << y << ";" << z;
// --- Convert rotation matrix to yaw-pitch-roll ---
double r00 = pose.R->data[0], r01 = pose.R->data[1], r02 = pose.R->data[2];
double r10 = pose.R->data[3], r11 = pose.R->data[4], r12 = pose.R->data[5];
double r20 = pose.R->data[6], r21 = pose.R->data[7], r22 = pose.R->data[8];
double yaw = atan2(r10, r00);
double pitch = atan2(-r20, sqrt(r21 * r21 + r22 * r22));
double roll = atan2(r21, r22);
std::cout << ";" << yaw * 180.0 / M_PI
<< ";" << pitch * 180.0 / M_PI
<< ";" << roll * 180.0 / M_PI << "\n";
matd_destroy(pose.R);
matd_destroy(pose.t);
}
apriltag_detections_destroy(detections);
apriltag_detector_destroy(td);
tagStandard41h12_destroy(tf);
return 0;
}
