参考:
1)https://github.com/AprilRobotics/apriltag
3) AprilTag标记跟踪 · OpenMV中文入门教程
4) 如何快速生成不同系列的Apriltag码?-CSDN博客
5)https://www.cnblogs.com/code-fun/p/17787552.html
二、生成AprilTag二维码方式
1.通过OpenMV生成
下载openmv软件:http://openmv.cc
2.官网下载
。。。。。。。。。。。。。。。。
三、相机标定
3.1 标定板制作
标定板:https://github.com/MegviiRobot/CamLaserCalibraTool/tree/master/doc
下的april_6x6_80x80cm_A0.pdf,然后打印出来,固定好相机采集待标定图像。
3.2 标定
标定代码:GitHub - ethz-asl/kalibr: The Kalibr visual-inertial calibration toolbox
标定主要获取相机的内参矩阵和畸变矩阵。
四、位姿估计
4.1 源码:
位姿估计代码:https://github.com/AprilRobotics/apriltag
4.2 环境配置
安装opencv4、Eigen3
4.3 更改源码
#include <iostream>
#include "opencv2/opencv.hpp"
#include <eigen3/Eigen/Dense> //necessary before including the opencv eigen lib
#include <opencv2/core/eigen.hpp> //include linear calculation lib
#include<opencv2/imgcodecs.hpp>
extern "C" {
#include "apriltag.h"
#include "apriltag_pose.h" //pose estimation lib
#include "tag36h11.h"
#include "tag25h9.h"
#include "tag16h5.h"
#include "tagCircle21h7.h"
#include "tagCircle49h12.h"
#include "tagCustom48h12.h"
#include "tagStandard41h12.h"
#include "tagStandard52h13.h"
#include "common/getopt.h"
}
using namespace std;
using namespace cv;
int main(int argc, char *argv[])
{
getopt_t *getopt = getopt_create();
getopt_add_bool(getopt, 'h', "help", 0, "Show this help");
getopt_add_bool(getopt, 'd', "debug", 0, "Enable debugging output (slow)");
getopt_add_bool(getopt, 'q', "quiet", 0, "Reduce output");
getopt_add_string(getopt, 'f', "family", "tag36h11", "Tag family to use");
getopt_add_int(getopt, 't', "threads", "1", "Use this many CPU threads");
getopt_add_double(getopt, 'x', "decimate", "2.0", "Decimate input image by this factor");
getopt_add_double(getopt, 'b', "blur", "0.0", "Apply low-pass blur to input");
getopt_add_bool(getopt, '0', "refine-edges", 1, "Spend more time trying to align edges of tags");
getopt_add_string(getopt, 'a', "axis", "world", "Choose axis to use (world/camera)");
if (!getopt_parse(getopt, argc, argv, 1) ||
getopt_get_bool(getopt, "help")) {
printf("Usage: %s [options]\n", argv[0]);
getopt_do_usage(getopt);
exit(0);
}
// Initialize camera
VideoCapture cap("/home/zc/apriltag/apriltag-with-pose-estimation-master/TP.mp4");
if (!cap.isOpened()) {
cerr << "Couldn't open video capture device" << endl;
return -1;
}
// Initialize tag detector with options
apriltag_family_t *tf = NULL;
const char *famname = getopt_get_string(getopt, "family");
if (!strcmp(famname, "tag36h11")) {
tf = tag36h11_create();
} else if (!strcmp(famname, "tag25h9")) {
tf = tag25h9_create();
} else if (!strcmp(famname, "tag16h5")) {
tf = tag16h5_create();
} else if (!strcmp(famname, "tagCircle21h7")) {
tf = tagCircle21h7_create();
} else if (!strcmp(famname, "tagCircle49h12")) {
tf = tagCircle49h12_create();
} else if (!strcmp(famname, "tagStandard41h12")) {
tf = tagStandard41h12_create();
} else if (!strcmp(famname, "tagStandard52h13")) {
tf = tagStandard52h13_create();
} else if (!strcmp(famname, "tagCustom48h12")) {
tf = tagCustom48h12_create();
} else {
printf("Unrecognized tag family name. Use e.g. \"tag36h11\".\n");
exit(-1);
}
const char *axisname = getopt_get_string(getopt, "axis");
unsigned int coordinate;
if(!strcmp(axisname,"world"))
coordinate = 1;
else
coordinate = 0;
// input camera intrinsic matrix
apriltag_detection_info_t info;
info.tagsize = 0.0335; //0.146-0.012*4; //The size of Tag
info.det = NULL;
info.fx = 553.762; //848.469; //focal length of x
info.fy = 577.223; //847.390; //focal length of y
// info.tagsize = 0.146-0.012*4; //The size of Tag
// info.det = NULL;
// info.fx = 848.469; //focal length of x
// info.fy = 847.390; //focal length of y
// info.cx = cap.get(CAP_PROP_FRAME_WIDTH)/2;
// info.cy = cap.get(CAP_PROP_FRAME_HEIGHT)/2;
info.cx = 761.361;
info.cy = 525.239;
// /**自己添加畸变参数*/
// // 相机内参矩阵
Mat camera_matrix = (Mat_<double>(3, 3) << 5.53762634e+02, 0., 7.61361084e+02, 0., 5.77223877e+02,
5.25239990e+02, 0., 0., 1.);
// 相机畸变系数
Mat dist_coeffs = (Mat_<double>(5, 1) << 2.04713028e-02, -1.52593590e-02, 5.94881538e-04,
-1.23523809e-02, 3.54151428e-03);
apriltag_detector_t *td = apriltag_detector_create();
apriltag_detector_add_family(td, tf);
td->quad_decimate = getopt_get_double(getopt, "decimate");
td->quad_sigma = getopt_get_double(getopt, "blur");
td->nthreads = getopt_get_int(getopt, "threads");
td->debug = getopt_get_bool(getopt, "debug");
td->refine_edges = getopt_get_bool(getopt, "refine-edges");
Mat frame, gray;
while (true) {
cap >> frame;
Mat frame_new,frameP;
frame_new=frame.clone();
int rows = frame_new.rows, clos = frame_new.cols;
cv::Size imageSize(clos, rows);
Mat map1, map2;
const double alpha = 0; //
Mat NewCameraMatrix = getOptimalNewCameraMatrix(camera_matrix,dist_coeffs, imageSize, alpha, imageSize, 0);
initUndistortRectifyMap(camera_matrix,dist_coeffs, cv::Mat(), NewCameraMatrix, imageSize, CV_16SC2, map1, map2);
remap(frame_new,frameP,map1, map2, INTER_LINEAR);
cvtColor(frameP, gray, COLOR_BGR2GRAY);
// Make an image_u8_t header for the Mat data
image_u8_t im = { .width = gray.cols,
.height = gray.rows,
.stride = gray.cols,
.buf = gray.data
};
zarray_t *detections = apriltag_detector_detect(td, &im);
// Draw detection outlines
for (int i = 0; i < zarray_size(detections); i++) {
apriltag_detection_t *det;
zarray_get(detections, i, &det);
info.det = det;
apriltag_pose_t pose;
estimate_tag_pose(&info, &pose);
//calculate cam position in world coordinate
if (coordinate)
{
Mat rvec(3, 3, CV_64FC1, pose.R->data); //rotation matrix
Mat tvec(3, 1, CV_64FC1, pose.t->data); //translation matrix
for(int i=0;i<3;i++)
{
for(int j=0;j<3;j++)
{
cout<<" "<<rvec.ptr<float>(i)[j];
}
cout<<endl;
}
cout<<"-------------------"<<endl;
for(int i=0;i<3;i++)
{
for(int j=0;j<3;j++)
{
cout<<" "<<tvec.ptr<float>(i)[j];
}
cout<<endl;
}
Mat Pos(3, 3, CV_64FC1);
//可解算出世界坐标系下相机的位置。如果需要计算相机坐标系下标签的位置,则不需要加上“-”号。
Pos = -rvec.inv() * tvec;
cout << "x: " << Pos.ptr<double>(0)[0] << endl;
cout << "y: " << Pos.ptr<double>(1)[0] << endl;
cout << "z: " << Pos.ptr<double>(2)[0] << endl;
cout << "-----------world--------------" << endl;
}
else
{
cout << "x: " << pose.t ->data[0] << endl;
cout << "y: " << pose.t ->data[1] << endl;
cout << "z: " << pose.t ->data[2] << endl;
cout << "-----------camera-------------" << endl;
}
//draw the line and show tag ID
line(frameP, Point(det->p[0][0], det->p[0][1]),
Point(det->p[1][0], det->p[1][1]),
Scalar(0, 0xff, 0), 2);
line(frameP, Point(det->p[0][0], det->p[0][1]),
Point(det->p[3][0], det->p[3][1]),
Scalar(0, 0, 0xff), 2);
line(frameP, Point(det->p[1][0], det->p[1][1]),
Point(det->p[2][0], det->p[2][1]),
Scalar(0xff, 0, 0), 2);
line(frameP, Point(det->p[2][0], det->p[2][1]),
Point(det->p[3][0], det->p[3][1]),
Scalar(0xff, 0, 0), 2);
stringstream ss;
ss << det->id;
String text = ss.str();
int fontface = FONT_HERSHEY_SCRIPT_SIMPLEX;
double fontscale = 1.0;
int baseline;
Size textsize = getTextSize(text, fontface, fontscale, 2,
&baseline);
putText(frameP, text, Point(det->c[0]-textsize.width/2,
det->c[1]+textsize.height/2),
fontface, fontscale, Scalar(0xff, 0x99, 0), 2);
}
apriltag_detections_destroy(detections);
imshow("Tag Detections", frameP);
if (waitKey(30) >= 0)
break;
}
apriltag_detector_destroy(td);
if (!strcmp(famname, "tag36h11")) {
tag36h11_destroy(tf);
} else if (!strcmp(famname, "tag25h9")) {
tag25h9_destroy(tf);
} else if (!strcmp(famname, "tag16h5")) {
tag16h5_destroy(tf);
} else if (!strcmp(famname, "tagCircle21h7")) {
tagCircle21h7_destroy(tf);
} else if (!strcmp(famname, "tagCircle49h12")) {
tagCircle49h12_destroy(tf);
} else if (!strcmp(famname, "tagStandard41h12")) {
tagStandard41h12_destroy(tf);
} else if (!strcmp(famname, "tagStandard52h13")) {
tagStandard52h13_destroy(tf);
} else if (!strcmp(famname, "tagCustom48h12")) {
tagCustom48h12_destroy(tf);
}
getopt_destroy(getopt);
return 0;
}
4.4 编译
mkdir build && cd build
cmake ..
make
sudo make install
4.5 执行
./opencv_demo f tag36h11 a world