21 #include <opencv2/objdetect.hpp>
22 #include <opencv2/imgproc.hpp>
60 std::string markerConfigDataFilename;
62 switch (artoolkit::dictionary::get())
64 case artoolkit::Dict::AR_MATRIX_CODE_3x3: markerConfigDataFilename =
"markers0.dat";
break;
65 case artoolkit::Dict::AR_MATRIX_CODE_3x3_HAMMING63: markerConfigDataFilename =
"markers1.dat";
break;
66 case artoolkit::Dict::AR_MATRIX_CODE_3x3_PARITY65: markerConfigDataFilename =
"markers2.dat";
break;
67 default: markerConfigDataFilename =
"markers2.dat";
72 arParamChangeSize(&cparam,
w,
h, &cparam);
74 std::string
const CPARA_NAME =
77 if (arParamLoad(
absolutePath(CPARA_NAME).c_str(), 1, &cparam) < 0)
78 LERROR(
"Failed to load camera parameters " << CPARA_NAME <<
" -- IGNORED");
80 if ((
gCparamLT = arParamLTCreate(&cparam, AR_PARAM_LT_DEFAULT_OFFSET)) ==
nullptr)
LFATAL(
"Error in arParamLTCreate");
84 if ((
ar3DHandle = ar3DCreateHandle(&cparam)) ==
nullptr)
LFATAL(
"Error in ar3DCreateHandle");
86 if (arSetPixelFormat(
arHandle, pixformat) < 0)
LFATAL(
"Error in arSetPixelFormat");
88 if ((
arPattHandle = arPattCreateHandle()) ==
nullptr)
LFATAL(
"Error in arPattCreateHandle");
95 arSetPatternDetectionMode(
arHandle, AR_MATRIX_CODE_DETECTION);
97 switch (dictionary::get())
99 case artoolkit::Dict::AR_MATRIX_CODE_3x3:
100 arSetMatrixCodeType(
arHandle, AR_MATRIX_CODE_3x3);
break;
101 case artoolkit::Dict::AR_MATRIX_CODE_3x3_HAMMING63:
102 arSetMatrixCodeType(
arHandle, AR_MATRIX_CODE_3x3_HAMMING63);
break;
103 case artoolkit::Dict::AR_MATRIX_CODE_3x3_PARITY65:
104 arSetMatrixCodeType(
arHandle, AR_MATRIX_CODE_3x3_PARITY65);
break;
105 default: arSetMatrixCodeType(
arHandle, AR_MATRIX_CODE_3x3_PARITY65);
111 AR_LABELING_THRESH_MODE modea;
112 switch (artoolkit::threshmode::get())
114 case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_MANUAL:
115 modea = AR_LABELING_THRESH_MODE_MANUAL;
break;
116 case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_AUTO_MEDIAN:
117 modea = AR_LABELING_THRESH_MODE_AUTO_MEDIAN;
break;
118 case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_AUTO_OTSU:
119 modea = AR_LABELING_THRESH_MODE_AUTO_OTSU;
break;
120 case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_AUTO_ADAPTIVE:
121 modea = AR_LABELING_THRESH_MODE_AUTO_ADAPTIVE;
break;
122 case artoolkit::DictThreshMode::AR_LABELING_THRESH_MODE_AUTO_BRACKETING:
123 modea = AR_LABELING_THRESH_MODE_AUTO_BRACKETING;
break;
124 default: modea = AR_LABELING_THRESH_MODE_AUTO_OTSU;
126 arSetLabelingThreshMode(
arHandle, modea);
139 case V4L2_PIX_FMT_RGB565:
manualinit(
image.width,
image.height, AR_PIXEL_FORMAT_RGB_565);
break;
141 default:
LFATAL(
"Unsupported image format, should be V4L2_PIX_FMT_YUYV, V4L2_PIX_FMT_GREY, "
142 "V4L2_PIX_FMT_RGB565, or V4L2_PIX_FMT_BGR24");
156 switch (
image.type())
160 default:
LFATAL(
"Unsupported image format, should be CV_8UC3 for BGR or CV_8UC1 for gray");
173 if (arDetectMarker(
arHandle,
const_cast<unsigned char *
>(data)) < 0)
174 {
LERROR(
"Error trying to detect markers -- IGNORED");
return; }
176 double const confidence_thresh = artoolkit::confthresh::get();
177 int const numDetected = arGetMarkerNum(
arHandle);
178 int const useContPoseEstimation = artoolkit::contpose::get();
181 ARMarkerInfo * markerInfo = arGetMarker(
arHandle);
189 for (
int j = 0; j < numDetected; ++j)
195 if (markerInfo[j].cf >= confidence_thresh) k = j;
197 else if (markerInfo[j].cf > markerInfo[k].cf) k = j;
206 result.id = markerInfo[k].id;
209 result.p2d[0] = markerInfo[k].pos[0];
result.p2d[1] = markerInfo[k].pos[1];
219 if (err > 1.0)
continue;
223 for (
int i1 = 0; i1 < 4; ++i1)
225 auto const & v1 = markerInfo[k].vertex[i1];
226 result.corners.push_back(cv::Point(
int(v1[0] + 0.5
F),
int(v1[1] + 0.5
F)));
243 for (
int i = 0; i < 4; ++i)
245 auto const & v1 = r.corners[i];
246 auto const & v2 = r.corners[(i + 1) % 4];
254 if (txtx >= 0 && txty >= 0)
256 txtx, txty, jevois::yuyv::White);
263 static ImU32
const col = ImColor(255, 128, 128, 255);
267 helper.
drawCircle(r.p2d[0], r.p2d[1], 3.0F, col,
true);
269 std::vector<cv::Point2f> p;
270 for (
int i = 0; i < 4; ++i) p.emplace_back(cv::Point2f(r.corners[i].x, r.corners[i].y));
286 r.width, r.height, 1.0F,
287 r.q[0], r.q[1], r.q[2], r.q[3],