00001 #include "TagFamily.h"
00002
00003 namespace AprilTags {
00004
00005 TagFamily::TagFamily(int bitsArg, int minHammingDistanceArg, const unsigned long long *codesArg)
00006 : blackBorder(1), bits(bitsArg), dimension((int)std::sqrt((float)bits)),
00007 minimumHammingDistance(minHammingDistanceArg), errorRecoveryBits(1), codes() {
00008 if ( bits != dimension*dimension )
00009 cerr << "Error: TagFamily constructor called with bits=" << bits << "; must be a square number!" << endl;
00010 unsigned int length = 0;
00011 while ( codesArg[length] ) ++length;
00012 codes.resize(length);
00013 for (unsigned int i = 0; i < length; i++)
00014 codes[i] = codesArg[i];
00015 tagFamilyRegistry[std::pair<int,int>(bits,minimumHammingDistance)] = this;
00016 }
00017
00018 void TagFamily::setErrorRecoveryBits(int b) {
00019 errorRecoveryBits = b;
00020 }
00021
00022 void TagFamily::setErrorRecoveryFraction(float v) {
00023 errorRecoveryBits = (int) (((int) (minimumHammingDistance-1)/2)*v);
00024 }
00025
00026 unsigned long long TagFamily::rotate90(unsigned long long w, int d) {
00027 unsigned long long wr = 0;
00028 const unsigned long long oneLongLong = 1;
00029
00030 for (int r = d-1; r>=0; r--) {
00031 for (int c = 0; c<d; c++) {
00032 int b = r + d*c;
00033 wr = wr<<1;
00034
00035 if ((w & (oneLongLong<<b)) != 0)
00036 wr |= 1;
00037 }
00038 }
00039 return wr;
00040 }
00041
00042 int TagFamily::hammingDistance(unsigned long long a, unsigned long long b) {
00043 return popCount(a^b);
00044 }
00045
00046 unsigned char TagFamily::popCountReal(unsigned long long w) {
00047 unsigned char cnt = 0;
00048 while (w != 0) {
00049 w &= (w-1);
00050 ++cnt;
00051 }
00052 return cnt;
00053 }
00054
00055 int TagFamily::popCount(unsigned long long w) {
00056 int count = 0;
00057 while (w != 0) {
00058 count += popCountTable[(unsigned int) (w & (popCountTableSize-1))];
00059 w >>= popCountTableShift;
00060 }
00061 return count;
00062 }
00063
00064 void TagFamily::decode(TagDetection& det, unsigned long long rCode) const {
00065 int bestId = -1;
00066 int bestHamming = INT_MAX;
00067 int bestRotation = 0;
00068 unsigned long long bestCode = 0;
00069
00070 unsigned long long rCodes[4];
00071 rCodes[0] = rCode;
00072 rCodes[1] = rotate90(rCodes[0], dimension);
00073 rCodes[2] = rotate90(rCodes[1], dimension);
00074 rCodes[3] = rotate90(rCodes[2], dimension);
00075
00076 for (unsigned int id = 0; id < codes.size(); id++) {
00077 for (unsigned int rot = 0; rot < 4; rot++) {
00078 int thisHamming = hammingDistance(rCodes[rot], codes[id]);
00079 if (thisHamming < bestHamming) {
00080 bestHamming = thisHamming;
00081 bestRotation = rot;
00082 bestId = id;
00083 bestCode = codes[id];
00084 }
00085 }
00086 }
00087
00088 det.id = bestId;
00089 det.hammingDistance = bestHamming;
00090 det.rotation = bestRotation;
00091 det.good = (det.hammingDistance <= errorRecoveryBits);
00092 det.obsCode = rCode;
00093 det.code = bestCode;
00094 }
00095
00096 void TagFamily::printHammingDistances() const {
00097 vector<int> hammings(dimension*dimension+1);
00098 for (unsigned i = 0; i < codes.size(); i++) {
00099 unsigned long long r0 = codes[i];
00100 unsigned long long r1 = rotate90(r0, dimension);
00101 unsigned long long r2 = rotate90(r1, dimension);
00102 unsigned long long r3 = rotate90(r2, dimension);
00103 for (unsigned int j = i+1; j < codes.size(); j++) {
00104 int d = min(min(hammingDistance(r0, codes[j]),
00105 hammingDistance(r1, codes[j])),
00106 min(hammingDistance(r2, codes[j]),
00107 hammingDistance(r3, codes[j])));
00108 hammings[d]++;
00109 }
00110 }
00111
00112 for (unsigned int i = 0; i < hammings.size(); i++)
00113 printf("hammings: %u = %d\n", i, hammings[i]);
00114 }
00115
00116 unsigned char TagFamily::popCountTable[TagFamily::popCountTableSize];
00117
00118 TagFamily::TableInitializer TagFamily::initializer;
00119
00120 std::map<std::pair<int,int>, TagFamily*> TagFamily::tagFamilyRegistry;
00121
00122
00123
00124
00125 const unsigned long long tag16h5Codes[] =
00126 { 0x231bLL, 0x2ea5LL, 0x346aLL, 0x45b9LL, 0x6857LL, 0x7f6bLL, 0xad93LL, 0xb358LL,
00127 0xb91dLL, 0xe745LL, 0x156dLL, 0xd3d2LL, 0xdf5cLL, 0x4736LL, 0x8c72LL, 0x5a02LL,
00128 0xd32bLL, 0x1867LL, 0x468fLL, 0xdc91LL, 0x4940LL, 0xa9edLL, 0x2bd5LL, 0x599aLL,
00129 0x9009LL, 0x61f6LL, 0x3850LL, 0x8157LL, 0xbfcaLL, 0x987cLL, 0LL };
00130
00131 TagFamily *tag16h5 = new TagFamily(16, 5, tag16h5Codes);
00132
00133
00134 const unsigned long long tag36h11Codes[] =
00135 { 0xd5d628584LL, 0xd97f18b49LL, 0xdd280910eLL, 0xe479e9c98LL, 0xebcbca822LL, 0xf31dab3acLL, 0x056a5d085LL, 0x10652e1d4LL,
00136 0x17b70ed5eLL, 0x22b1dfeadLL, 0x265ad0472LL, 0x34fe91b86LL, 0x3ff962cd5LL, 0x43a25329aLL, 0x474b4385fLL, 0x4e9d243e9LL,
00137 0x5246149aeLL, 0x5997f5538LL, 0x683bb6c4cLL, 0x6be4a7211LL, 0x7e3158eeaLL, 0x81da494afLL, 0x858339a74LL, 0x8cd51a5feLL,
00138 0x9f21cc2d7LL, 0xa2cabc89cLL, 0xadc58d9ebLL, 0xb16e7dfb0LL, 0xb8c05eb3aLL, 0xd25ef139dLL, 0xd607e1962LL, 0xe4aba3076LL,
00139 0x2dde6a3daLL, 0x43d40c678LL, 0x5620be351LL, 0x64c47fa65LL, 0x686d7002aLL, 0x6c16605efLL, 0x6fbf50bb4LL, 0x8d06d39dcLL,
00140 0x9baa950f0LL, 0x9f53856b5LL, 0xadf746dc9LL, 0xbc9b084ddLL, 0xd290aa77bLL, 0xd9e28b305LL, 0xe4dd5c454LL, 0xfad2fe6f2LL,
00141 0x181a8151aLL, 0x26be42c2eLL, 0x2e10237b8LL, 0x405cd5491LL, 0x6ff109f92LL, 0x7742eab1cLL, 0x85e6ac230LL, 0x8d388cdbaLL,
00142 0x9f853ea93LL, 0xc41ea2445LL, 0xcf1973594LL, 0x14a34a333LL, 0x31eacd15bLL, 0x44377ee34LL, 0x6c79d2dabLL, 0x73cbb3935LL,
00143 0x89c155bd3LL, 0x8d6a46198LL, 0x91133675dLL, 0xa708d89fbLL, 0xae5ab9585LL, 0xb9558a6d4LL, 0xb98743ab2LL, 0xd6cec68daLL,
00144 0x1506bcaefLL, 0x4becd217aLL, 0x4f95c273fLL, 0x658b649ddLL, 0xa76c4b1b7LL, 0xecf621f56LL, 0x1c8a56a57LL, 0x3628e92baLL,
00145 0x53706c0e2LL, 0x7809cfa94LL, 0xc88e77982LL, 0xe97eead6fLL, 0x5af40604aLL, 0xffa6463ebLL, 0x5eceaf9edLL, 0x7c1632815LL,
00146 0xc1a0095b4LL, 0xe9e25d52bLL, 0x3a6705419LL, 0xa8333012fLL, 0x4ce5704d0LL, 0x508e60a95LL, 0x877476120LL, 0xa864e950dLL,
00147 0xea45cfce7LL, 0x19da047e8LL, 0x24d4d5937LL, 0x54690a438LL, 0x6e079cc9bLL, 0x99f2e11d7LL, 0x499ff26c7LL, 0x50f1d3251LL,
00148 0x66e7754efLL, 0x49d1abaa5LL, 0x96ad633ceLL, 0x9a5653993LL, 0xaca30566cLL, 0x8be44b65dLL, 0xb4269f5d4LL, 0xdc68f354bLL,
00149 0x4dde0e826LL, 0xd548cbd9fLL, 0xfd8b1fd16LL, 0x76521bb7bLL, 0xd1d194bb8LL, 0xd57a8517dLL, 0xd92375742LL, 0xcab16d40cLL,
00150 0x730c9dd72LL, 0xad9ba39c2LL, 0xb14493f87LL, 0x185409cadLL, 0x77ae2c68dLL, 0x94f5af4b5LL, 0x0a13bad55LL, 0x61ea437cdLL,
00151 0xa022399e2LL, 0xbd69bc80aLL, 0x203b163d1LL, 0x7bba8f40eLL, 0x784358227LL, 0xc92b728d1LL, 0x92a8cfa02LL, 0x9da3a0b51LL,
00152 0xdcd4350bcLL, 0x4aa05fdd2LL, 0x60c7bb44eLL, 0x4b358b96cLL, 0x612b2dc0aLL, 0x775289286LL, 0x7ea469e10LL, 0x09e9d0d2cLL,
00153 0x067299b45LL, 0xb9c89b5faLL, 0x9560f1026LL, 0x62b8f7afaLL, 0xbac139950LL, 0x58b6c4d01LL, 0xa5927c62aLL, 0xe77362e04LL,
00154 0xf29fed331LL, 0x903205f26LL, 0xc36f2afecLL, 0xae72270a4LL, 0x3d2ec51a7LL, 0x82ea55324LL, 0x1ca1c4576LL, 0xa40c81aefLL,
00155 0xbddccd730LL, 0x0e617561eLL, 0x585b218faLL, 0x969317b0fLL, 0x588cdacd8LL, 0x67309c3ecLL, 0x8c5f2b938LL, 0x4142f72ddLL,
00156 0x06e5aaa6bLL, 0x626523aa8LL, 0xb6c475339LL, 0xc56836a4dLL, 0x4647f83b4LL, 0x0908a04f5LL, 0x7862950fbLL, 0xd445808f4LL,
00157 0x28d68b563LL, 0x04d25374bLL, 0xc8bd52fc0LL, 0x06f5491d5LL, 0x27e5bc5c2LL, 0x2bc065f65LL, 0x96dc3ea0cLL, 0xfdb9fb354LL,
00158 0x47b3a7630LL, 0xd7372a6abLL, 0x372678c25LL, 0xe768b5cafLL, 0x437d5a886LL, 0x2b091f757LL, 0x91b522cc1LL, 0x62846097cLL,
00159 0x3aa57f1c1LL, 0x263da6e13LL, 0xfa841bcb5LL, 0x157ebf02aLL, 0xf586e9f93LL, 0xecaf0e8ceLL, 0x82ef46939LL, 0x847d10829LL,
00160 0x68919e513LL, 0x2aeed3e98LL, 0x11265760cLL, 0x1ce80d6d3LL, 0x0e4c1b374LL, 0x68b0db3e7LL, 0x1c389627aLL, 0xfb79dc26bLL,
00161 0x9379975a4LL, 0x064ac3391LL, 0x706dfdae2LL, 0x44edfb117LL, 0x86a4f78c8LL, 0xaebd61816LL, 0xf53fd690bLL, 0xb8f91cda2LL,
00162 0xf0a6173a5LL, 0x12c0e1ec6LL, 0xd1a6e0664LL, 0x6bf37b450LL, 0x62b82d5cfLL, 0xe5f46d153LL, 0x0d1438d4bLL, 0x5af82d134LL,
00163 0x6ea0ef91fLL, 0x9ff4a76eeLL, 0xde5e56ce1LL, 0xb5c82ed18LL, 0x5b50f279bLL, 0xd7f297fa3LL, 0xef444ad53LL, 0xa8c9a5013LL,
00164 0x3d300e4f1LL, 0x33fc8fa25LL, 0x43d277c22LL, 0x9d2c1d435LL, 0x5f8952dbaLL, 0xf0cc59103LL, 0xd2f779af6LL, 0xb5e97f461LL,
00165 0x7f0b3918bLL, 0xe42e63e1aLL, 0x769bc1897LL, 0x97bdee062LL, 0x792229addLL, 0x816ca89bdLL, 0xd41446335LL, 0x3f572b065LL,
00166 0x2a93af8b0LL, 0xcadde9ac9LL, 0x7176b93c1LL, 0x84b15c29bLL, 0x9b9f9c88fLL, 0x537511febLL, 0xee8891d4fLL, 0x5dc83b096LL,
00167 0x05bd1b4a0LL, 0x2e073e7ccLL, 0x0b5f174c6LL, 0xb184d5e98LL, 0xd0ef4e74aLL, 0x0ddad25b7LL, 0xbd16e63f6LL, 0x4aa64f025LL,
00168 0xa252eda74LL, 0xd940d24b4LL, 0x9745a041bLL, 0x055322c79LL, 0x7022f6a83LL, 0xa31416eacLL, 0x96b2880f6LL, 0x48d385322LL,
00169 0x14d46c8f9LL, 0x11e4d63b7LL, 0x379614f93LL, 0x71eda5cc5LL, 0xaa05e1e39LL, 0xcee09d333LL, 0x52c976570LL, 0x023252178LL,
00170 0x599fac8f4LL, 0xbb0a48854LL, 0x98cd630bfLL, 0x2d8f6f9a4LL, 0xf5c05a72dLL, 0x9ed9d672bLL, 0x50d8b8ce3LL, 0xe59ac55c8LL,
00171 0xe09d938a6LL, 0x4ada4f38bLL, 0xbb85a794eLL, 0x5544e5f55LL, 0x9a3db8244LL, 0xe3784e95dLL, 0x796c81d2bLL, 0x6cebb60a1LL,
00172 0x27b2d55b4LL, 0x6de945a0cLL, 0x4a69d0af9LL, 0x6afea0adfLL, 0x158987899LL, 0x1b528fb48LL, 0x6d183275eLL, 0x73afeed3aLL,
00173 0x1a7a77a10LL, 0x4be59d2feLL, 0x2ad522b12LL, 0xa82d445fdLL, 0xbbcb59c93LL, 0xe71e94895LL, 0x75b14896fLL, 0xb0afb721aLL,
00174 0x065d8e6c8LL, 0x372810d9cLL, 0xb77603728LL, 0xad78c1f44LL, 0x90ca91da0LL, 0x2e74184b4LL, 0xc2964c0aaLL, 0xb07f7a899LL,
00175 0x8ee694eddLL, 0x1ad7caf87LL, 0x2035916c5LL, 0xcd1670631LL, 0x1611c2a77LL, 0x8a1a06962LL, 0xdb970149aLL, 0x5778c6bb4LL,
00176 0x3fab695feLL, 0x014b9cc35LL, 0x604be4377LL, 0xfd49501f1LL, 0xe2b710c4dLL, 0x6bf7f4a88LL, 0x0adf98124LL, 0xc5ee49adeLL,
00177 0xe4c34b0eaLL, 0x9b5e0047dLL, 0x4002b5929LL, 0x4e9a35492LL, 0x908aedae9LL, 0xa0bc790edLL, 0xd12b583baLL, 0x431b08264LL,
00178 0xb7b33afc8LL, 0xd115672f8LL, 0x253296b16LL, 0xbd5e4f6edLL, 0xf1276fc55LL, 0x5feaa426bLL, 0x2d0955cbfLL, 0xcb2ade90eLL,
00179 0x08b0fe749LL, 0x2709d6730LL, 0x0edc7ec97LL, 0xa7c74d431LL, 0x1536402eaLL, 0x61936f66dLL, 0x7ec973bc9LL, 0xa00a12d3dLL,
00180 0xc6ed2ccf6LL, 0x8f87c4b9aLL, 0xf049ee52bLL, 0x0d1fa9777LL, 0x85175a497LL, 0x2d917c5c5LL, 0xfc61287b4LL, 0x63ce55156LL,
00181 0x659cac663LL, 0xbb4b8174fLL, 0x70bd5be0bLL, 0xfb3da5f18LL, 0x917b001e3LL, 0x516870f16LL, 0x03bb5ac33LL, 0x2a510ec0cLL,
00182 0x07ecd1ae2LL, 0x06642c91aLL, 0xcc7c83662LL, 0xb88d2c60eLL, 0x40d35e87eLL, 0x452f5656eLL, 0xf8c5e5640LL, 0xc68372145LL,
00183 0x6b61cb49eLL, 0x066ce5035LL, 0x151c05dd0LL, 0xad92f9119LL, 0x8fa874156LL, 0xd7d545982LL, 0x2602c7a8eLL, 0x0d9054ac8LL,
00184 0xb85332ce0LL, 0xa8e7f583cLL, 0xa2534a713LL, 0x77ce78732LL, 0x6b605c6c5LL, 0x8101603e6LL, 0x9573cd8f0LL, 0xa18bba0abLL,
00185 0xf5d224ae1LL, 0x9ecb4dfd4LL, 0x2e48c9e03LL, 0x79b4c6ae0LL, 0x60e6b0713LL, 0xea6a8420dLL, 0xa22971a8fLL, 0x605c053fdLL,
00186 0x57678633aLL, 0xea85c3395LL, 0x7fda1da74LL, 0x9824459caLL, 0xb2eee31f2LL, 0x4a34b0db1LL, 0xb5bbbd933LL, 0x583d9c190LL,
00187 0xc93e6091cLL, 0xdca7c6e3bLL, 0x214d69b74LL, 0x525894f7fLL, 0x21be0e083LL, 0xf0dbd2784LL, 0x0ffac88d9LL, 0x57f7e33e5LL,
00188 0x4a7301d85LL, 0x8887af6f6LL, 0x1b8ccb3a1LL, 0x68c1b2878LL, 0x78b6bf950LL, 0x63b9aa851LL, 0x7ed12f23aLL, 0x350eb35b2LL,
00189 0x561503189LL, 0x3f16ac63cLL, 0xd2fd4b06cLL, 0xa7c49627eLL, 0x36b9f5d0aLL, 0xbca21c149LL, 0xba5bc28efLL, 0xce2c2b89cLL,
00190 0x776bc0448LL, 0xce170f268LL, 0x8f57303c3LL, 0x74e5fcc9eLL, 0x46de67b7eLL, 0x2b98bd7aaLL, 0xb5c41dc2eLL, 0x12e1e50f8LL,
00191 0x875f6fcdaLL, 0xf90ea702dLL, 0x7ed051595LL, 0x9d8da07b8LL, 0xbc30d09edLL, 0x77ad8306bLL, 0x82d4a0885LL, 0xf4e1b7a04LL,
00192 0xb427eabdaLL, 0xdb1b28f5eLL, 0xe5f911de5LL, 0xb8ff4c115LL, 0x3185fbcf4LL, 0xefda16bdfLL, 0xeaa3f6c63LL, 0x9a3f4f520LL,
00193 0x6317c6e21LL, 0xde1ac8909LL, 0xb962e4d06LL, 0x8a8cc1536LL, 0x0abebf2d5LL, 0x6a3787f5fLL, 0x62cc2622fLL, 0x3196aa59fLL,
00194 0x9b6816c6bLL, 0x95f398661LL, 0x2b1673eb7LL, 0xc9cf19ba7LL, 0x44394782aLL, 0xd02e2d199LL, 0xe517f16dcLL, 0x433df5666LL,
00195 0xdbfb6521fLL, 0x6316ed9fbLL, 0x0681b072aLL, 0x24e7e3614LL, 0x3049f22daLL, 0x245b47d67LL, 0x032e59d5dLL, 0x78f512121LL,
00196 0xf76e98aacLL, 0x20e313ad6LL, 0x9947bd319LL, 0x0719aab9fLL, 0xabe40e6b2LL, 0x9bbec96c6LL, 0xcf05e6446LL, 0xee3c76b79LL,
00197 0xe9317cc92LL, 0x9bf9aa92bLL, 0x13fe98495LL, 0xd931239a6LL, 0x7264aced9LL, 0x04ed957b4LL, 0xbb7021cbbLL, 0x4609308b5LL,
00198 0x2d5c52c38LL, 0xceb22ad4fLL, 0x82a47a446LL, 0x04d68909aLL, 0x832cdf368LL, 0x242ed1118LL, 0xd1dda2014LL, 0x901b6a04eLL,
00199 0x19c1e9514LL, 0x3e9c0c97fLL, 0x1e814bce3LL, 0x55b40b44dLL, 0xeff162399LL, 0x4012da58aLL, 0x1d4d90e43LL, 0x50e79facdLL,
00200 0x35ef088baLL, 0x774709d00LL, 0xd32e575b3LL, 0x54c5cfff2LL, 0x59bbd73fbLL, 0x3b2dc7e8bLL, 0x13a74d09fLL, 0xc5a21e37fLL,
00201 0x7aab49b28LL, 0x2793aa17aLL, 0x3a6673575LL, 0x78c27371eLL, 0x1788da29cLL, 0x8b5bb6078LL, 0xa5c506a80LL, 0xc2c487d6aLL,
00202 0xf238647acLL, 0x601f5e6e2LL, 0x2db5d412aLL, 0xd7c46f24aLL, 0xe4e0b67f5LL, 0xe94793093LL, 0xe07e85846LL, 0xa04f6f205LL,
00203 0xe47cbc125LL, 0x9022fb419LL, 0xc2127ead8LL, 0xc670f0e63LL, 0xd282518cfLL, 0x63e8d8335LL, 0x2aa63408eLL, 0xb011851aaLL,
00204 0x2df8c686fLL, 0xa31f8c5f1LL, 0xe8c09cecbLL, 0x4cd645fb9LL, 0xa63b5d9f5LL, 0xd74dd32d6LL, 0xf6869a32aLL, 0xb725dae3eLL,
00205 0xc5b27c981LL, 0x67d0118b8LL, 0xb3fdb04faLL, 0xf11838f31LL, 0x7e73b5fb9LL, 0x24ec2067aLL, 0x8aaf3bceaLL, 0x04a06dca0LL,
00206 0x70a03ed6bLL, 0x70dc29b18LL, 0x4d75699a9LL, 0xaedc558d9LL, 0x62590b9afLL, 0x5f9258453LL, 0xf04a9a9ceLL, 0xcd1feb280LL,
00207 0x7300b05a7LL, 0xadb7ab52cLL, 0x59afeb236LL, 0xf1de62e03LL, 0xf103ba210LL, 0x7cf6472d5LL, 0xf84bf1908LL, 0xa4952dc03LL,
00208 0x43d506f47LL, 0x8e90eed24LL, 0x9c04974e9LL, 0x953aef583LL, 0x3d839c1bcLL, 0x0348ac64fLL, 0x2a1284fc1LL, 0x9fc565ccdLL,
00209 0x57118e8c4LL };
00210
00211 TagFamily *tag36h11 = new TagFamily(36, 11, tag36h11Codes);
00212
00213 }
