Skip to content

Commit 1b85f01

Browse files
AleksandrPanovAleksandrPanov
committed
add aruco_dict_utils.cpp
1 parent ffc76e6 commit 1b85f01

File tree

1 file changed

+276
-0
lines changed

1 file changed

+276
-0
lines changed

‎modules/aruco/samples/aruco_dict_utils.cpp

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

0 commit comments

Comments
 (0)