Apriltag 3 e pose estimation

 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;

}