Skip to content

Commit a895e6a

Browse files
author
AleksandrPanov
committed
add aruco_dict_utils.cpp
1 parent ef6b2e6 commit a895e6a

File tree

1 file changed

+272
-0
lines changed

1 file changed

+272
-0
lines changed

‎modules/aruco/samples/aruco_dict_utils.cpp‎

Lines changed: 272 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,272 @@
1+
#include <opencv2/core/hal/hal.hpp>
2+
#include <opencv2/aruco.hpp>
3+
#include <iostream>
4+
5+
using namespace cv;
6+
using namespace std;
7+
8+
static inline Mat _generateRandomMarker(int markerSize, RNG &rng) {
9+
Mat marker(markerSize, markerSize, CV_8UC1, Scalar::all(0));
10+
rng.fill(marker, RNG::UNIFORM, 0, 2);
11+
return marker;
12+
}
13+
14+
static inline int _getSelfDistance(const Mat &marker) {
15+
Mat bytes = aruco::Dictionary::getByteListFromBits(marker);
16+
int minHamming = (int)marker.total() + 1;
17+
for(int r = 1; r < 4; r++) {
18+
int currentHamming = cv::hal::normHamming(bytes.ptr(), bytes.ptr() + bytes.cols*r, bytes.cols);
19+
if(currentHamming < minHamming) minHamming = currentHamming;
20+
}
21+
Mat b;
22+
flip(marker, b, 0);
23+
Mat flipBytes = aruco::Dictionary::getByteListFromBits(b);
24+
for(int r = 0; r < 4; r++) {
25+
int currentHamming = cv::hal::normHamming(flipBytes.ptr(), bytes.ptr() + bytes.cols*r, bytes.cols);
26+
if(currentHamming < minHamming) minHamming = currentHamming;
27+
}
28+
flip(marker, b, 1);
29+
flipBytes = aruco::Dictionary::getByteListFromBits(b);
30+
for(int r = 0; r < 4; r++) {
31+
int currentHamming = cv::hal::normHamming(flipBytes.ptr(), bytes.ptr() + bytes.cols*r, bytes.cols);
32+
if(currentHamming < minHamming) minHamming = currentHamming;
33+
}
34+
return minHamming;
35+
}
36+
37+
static inline int getFlipDistanceToId(Ptr<aruco::Dictionary> dict, InputArray bits, int id, bool allRotations = true) {
38+
Mat bytesList = dict->bytesList;
39+
CV_Assert(id >= 0 && id < bytesList.rows);
40+
41+
unsigned int nRotations = 4;
42+
if(!allRotations) nRotations = 1;
43+
44+
Mat candidateBytes = aruco::Dictionary::getByteListFromBits(bits.getMat());
45+
int currentMinDistance = int(bits.total() * bits.total());
46+
for(unsigned int r = 0; r < nRotations; r++) {
47+
int currentHamming = cv::hal::normHamming(
48+
bytesList.ptr(id) + r*candidateBytes.cols,
49+
candidateBytes.ptr(),
50+
candidateBytes.cols);
51+
52+
if(currentHamming < currentMinDistance) {
53+
currentMinDistance = currentHamming;
54+
}
55+
}
56+
Mat b;
57+
flip(bits.getMat(), b, 0);
58+
candidateBytes = aruco::Dictionary::getByteListFromBits(b);
59+
for(unsigned int r = 0; r < nRotations; r++) {
60+
int currentHamming = cv::hal::normHamming(
61+
bytesList.ptr(id) + r * candidateBytes.cols,
62+
candidateBytes.ptr(),
63+
candidateBytes.cols);
64+
if (currentHamming < currentMinDistance) {
65+
currentMinDistance = currentHamming;
66+
}
67+
}
68+
69+
flip(bits.getMat(), b, 1);
70+
candidateBytes = aruco::Dictionary::getByteListFromBits(b);
71+
for(unsigned int r = 0; r < nRotations; r++) {
72+
int currentHamming = cv::hal::normHamming(
73+
bytesList.ptr(id) + r * candidateBytes.cols,
74+
candidateBytes.ptr(),
75+
candidateBytes.cols);
76+
if (currentHamming < currentMinDistance) {
77+
currentMinDistance = currentHamming;
78+
}
79+
}
80+
return currentMinDistance;
81+
}
82+
83+
static inline Ptr<aruco::Dictionary> generateCustomAsymmetricDictionary(int nMarkers, int markerSize,
84+
const Ptr<aruco::Dictionary> &baseDictionary, int randomSeed) {
85+
RNG rng((uint64)(randomSeed));
86+
87+
Ptr<aruco::Dictionary> out = makePtr<aruco::Dictionary>();
88+
out->markerSize = markerSize;
89+
90+
// theoretical maximum intermarker distance
91+
// See S. Garrido-Jurado, R. Muñoz-Salinas, F. J. Madrid-Cuevas, and M. J. Marín-Jiménez. 2014.
92+
// "Automatic generation and detection of highly reliable fiducial markers under occlusion".
93+
// Pattern Recogn. 47, 6 (June 2014), 2280-2292. DOI=10.1016/j.patcog.2014.01.005
94+
int C = (int)std::floor(float(markerSize * markerSize) / 4.f);
95+
int tau = 2 * (int)std::floor(float(C) * 4.f / 3.f);
96+
97+
// if baseDictionary is provided, calculate its intermarker distance
98+
if(baseDictionary->bytesList.rows > 0) {
99+
CV_Assert(baseDictionary->markerSize == markerSize);
100+
out->bytesList = baseDictionary->bytesList.clone();
101+
102+
int minDistance = markerSize * markerSize + 1;
103+
for(int i = 0; i < out->bytesList.rows; i++) {
104+
Mat markerBytes = out->bytesList.rowRange(i, i + 1);
105+
Mat markerBits = aruco::Dictionary::getBitsFromByteList(markerBytes, markerSize);
106+
minDistance = min(minDistance, _getSelfDistance(markerBits));
107+
for(int j = i + 1; j < out->bytesList.rows; j++) {
108+
minDistance = min(minDistance, getFlipDistanceToId(out, markerBits, j));
109+
}
110+
}
111+
tau = minDistance;
112+
}
113+
114+
// current best option
115+
int bestTau = 0;
116+
Mat bestMarker;
117+
118+
// after these number of unproductive iterations, the best option is accepted
119+
const int maxUnproductiveIterations = 5000;
120+
int unproductiveIterations = 0;
121+
122+
while(out->bytesList.rows < nMarkers) {
123+
Mat currentMarker = _generateRandomMarker(markerSize, rng);
124+
125+
int selfDistance = _getSelfDistance(currentMarker);
126+
int minDistance = selfDistance;
127+
128+
// if self distance is better or equal than current best option, calculate distance
129+
// to previous accepted markers
130+
if(selfDistance >= bestTau) {
131+
for(int i = 0; i < out->bytesList.rows; i++) {
132+
int currentDistance = getFlipDistanceToId(out, currentMarker, i);
133+
minDistance = min(currentDistance, minDistance);
134+
if(minDistance <= bestTau) {
135+
break;
136+
}
137+
}
138+
}
139+
140+
// if distance is high enough, accept the marker
141+
if(minDistance >= tau) {
142+
unproductiveIterations = 0;
143+
bestTau = 0;
144+
Mat bytes = aruco::Dictionary::getByteListFromBits(currentMarker);
145+
out->bytesList.push_back(bytes);
146+
} else {
147+
unproductiveIterations++;
148+
149+
// if distance is not enough, but is better than the current best option
150+
if(minDistance > bestTau) {
151+
bestTau = minDistance;
152+
bestMarker = currentMarker;
153+
}
154+
155+
// if number of unproductive iterarions has been reached, accept the current best option
156+
if(unproductiveIterations == maxUnproductiveIterations) {
157+
unproductiveIterations = 0;
158+
tau = bestTau;
159+
bestTau = 0;
160+
Mat bytes = aruco::Dictionary::getByteListFromBits(bestMarker);
161+
out->bytesList.push_back(bytes);
162+
}
163+
}
164+
}
165+
166+
// update the maximum number of correction bits for the generated dictionary
167+
out->maxCorrectionBits = (tau - 1) / 2;
168+
169+
return out;
170+
}
171+
172+
static inline int getMinDistForDict(const Ptr<aruco::Dictionary>& dict) {
173+
const int dict_size = dict->bytesList.rows;
174+
const int marker_size = dict->markerSize;
175+
int minDist = marker_size * marker_size;
176+
for (int i = 0; i < dict_size; i++) {
177+
Mat row = dict->bytesList.row(i);
178+
Mat marker = dict->getBitsFromByteList(row, marker_size);
179+
for (int j = 0; j < dict_size; j++) {
180+
if (j != i) {
181+
minDist = min(dict->getDistanceToId(marker, j), minDist);
182+
}
183+
}
184+
}
185+
return minDist;
186+
}
187+
188+
static inline int getMinAsymDistForDict(const Ptr<aruco::Dictionary>& dict) {
189+
const int dict_size = dict->bytesList.rows;
190+
const int marker_size = dict->markerSize;
191+
int minDist = marker_size * marker_size;
192+
for (int i = 0; i < dict_size; i++)
193+
{
194+
Mat row = dict->bytesList.row(i);
195+
Mat marker = dict->getBitsFromByteList(row, marker_size);
196+
for (int j = 0; j < dict_size; j++)
197+
{
198+
if (j != i)
199+
{
200+
minDist = min(getFlipDistanceToId(dict, marker, j), minDist);
201+
}
202+
}
203+
}
204+
return minDist;
205+
}
206+
207+
const char* keys =
208+
"{@outfile |<none> | Output file with custom dict }"
209+
"{r | false | Calculate the metric considering flipped markers }"
210+
"{d | | Dictionary: DICT_4X4_50=0, DICT_4X4_100=1, DICT_4X4_250=2,"
211+
"DICT_4X4_1000=3, DICT_5X5_50=4, DICT_5X5_100=5, DICT_5X5_250=6, DICT_5X5_1000=7, "
212+
"DICT_6X6_50=8, DICT_6X6_100=9, DICT_6X6_250=10, DICT_6X6_1000=11, DICT_7X7_50=12,"
213+
"DICT_7X7_100=13, DICT_7X7_250=14, DICT_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16}"
214+
"{nMarkers | | Number of markers in the dictionary }"
215+
"{markerSize | | Marker size }"
216+
"{cd | | Input file with custom dictionary }";
217+
218+
const char* about =
219+
"This program can be used to calculate the ArUco dictionary metric.\n"
220+
"To calculate the metric considering flipped markers use -'r' flag.\n"
221+
"This program can be used to create and write the custom ArUco dictionary.\n";
222+
223+
int main(int argc, char *argv[])
224+
{
225+
CommandLineParser parser(argc, argv, keys);
226+
parser.about(about);
227+
228+
if(argc < 2) {
229+
parser.printMessage();
230+
return 0;
231+
}
232+
string outputFile = parser.get<String>(0);
233+
int nMarkers = parser.get<int>("nMarkers");
234+
int markerSize = parser.get<int>("markerSize");
235+
bool checkFlippedMarkers = parser.get<bool>("r");
236+
237+
Ptr<aruco::Dictionary> dictionary = aruco::getPredefinedDictionary(0);
238+
if (parser.has("d")) {
239+
int dictionaryId = parser.get<int>("d");
240+
dictionary = aruco::getPredefinedDictionary(aruco::PREDEFINED_DICTIONARY_NAME(dictionaryId));
241+
}
242+
else if (parser.has("cd")) {
243+
FileStorage fs(parser.get<std::string>("cd"), FileStorage::READ);
244+
bool readOk = dictionary->aruco::Dictionary::readDictionary(fs.root());
245+
if(!readOk) {
246+
cerr << "Invalid dictionary file" << endl;
247+
return 0;
248+
}
249+
}
250+
else if (outputFile.empty() || nMarkers == 0 || markerSize == 0) {
251+
cerr << "Dictionary not specified" << endl;
252+
return 0;
253+
}
254+
255+
if (!outputFile.empty() && nMarkers > 0 && markerSize > 0)
256+
{
257+
Ptr<FileStorage> fs = makePtr<FileStorage>(outputFile, FileStorage::WRITE);
258+
if (checkFlippedMarkers)
259+
dictionary = generateCustomAsymmetricDictionary(nMarkers, markerSize, makePtr<aruco::Dictionary>(), 0);
260+
else
261+
dictionary = aruco::generateCustomDictionary(nMarkers, markerSize, makePtr<aruco::Dictionary>(), 0);
262+
dictionary->writeDictionary(fs);
263+
}
264+
265+
if (checkFlippedMarkers) {
266+
cout << getMinAsymDistForDict(dictionary) << endl;
267+
}
268+
else {
269+
cout << getMinDistForDict(dictionary) << endl;
270+
}
271+
return 0;
272+
}

0 commit comments

Comments
 (0)