TagFamily.cc
Go to the documentation of this file.
00001 #include <iostream>
00002 
00003 #include "TagFamily.h"
00004 
00025 using namespace std;
00026 
00027 namespace AprilTags {
00028 
00029 TagFamily::TagFamily(const TagCodes& tagCodes)
00030   : blackBorder(1), bits(tagCodes.bits), dimension((int)std::sqrt((float)bits)),
00031     minimumHammingDistance(tagCodes.minHammingDistance),
00032     errorRecoveryBits(1), codes() {
00033   if ( bits != dimension*dimension )
00034     cerr << "Error: TagFamily constructor called with bits=" << bits << "; must be a square number!" << endl;
00035   codes = tagCodes.codes;
00036 }
00037 
00038 void TagFamily::setErrorRecoveryBits(int b) {
00039   errorRecoveryBits = b;
00040 }
00041 
00042 void TagFamily::setErrorRecoveryFraction(float v) {
00043   errorRecoveryBits = (int) (((int) (minimumHammingDistance-1)/2)*v);
00044 }
00045 
00046 unsigned long long TagFamily::rotate90(unsigned long long w, int d) {
00047   unsigned long long wr = 0;
00048   const unsigned long long oneLongLong = 1;
00049 
00050   for (int r = d-1; r>=0; r--) {
00051     for (int c = 0; c<d; c++) {
00052       int b = r + d*c;
00053       wr = wr<<1;
00054       
00055       if ((w & (oneLongLong<<b)) != 0)
00056         wr |= 1;
00057     }
00058   }
00059   return wr;
00060 }
00061 
00062 int TagFamily::hammingDistance(unsigned long long a, unsigned long long b) {
00063   return popCount(a^b);
00064 }
00065 
00066 unsigned char TagFamily::popCountReal(unsigned long long w) {
00067   unsigned char cnt = 0;
00068   while (w != 0) {
00069     w &= (w-1);
00070     ++cnt;
00071   }
00072   return cnt;
00073 }
00074 
00075 int TagFamily::popCount(unsigned long long w) {
00076   int count = 0;
00077   while (w != 0) {
00078     count += popCountTable[(unsigned int) (w & (popCountTableSize-1))];
00079     w >>= popCountTableShift;
00080   }
00081   return count;
00082 }
00083 
00084 void TagFamily::decode(TagDetection& det, unsigned long long rCode) const {
00085   int  bestId = -1;
00086   int  bestHamming = INT_MAX;
00087   int  bestRotation = 0;
00088   unsigned long long bestCode = 0;
00089 
00090   unsigned long long rCodes[4];
00091   rCodes[0] = rCode;
00092   rCodes[1] = rotate90(rCodes[0], dimension);
00093   rCodes[2] = rotate90(rCodes[1], dimension);
00094   rCodes[3] = rotate90(rCodes[2], dimension);
00095 
00096   for (unsigned int id = 0; id < codes.size(); id++) {
00097     for (unsigned int rot = 0; rot < 4; rot++) {
00098       int thisHamming = hammingDistance(rCodes[rot], codes[id]);
00099       if (thisHamming < bestHamming) {
00100         bestHamming = thisHamming;
00101         bestRotation = rot;
00102         bestId = id;
00103         bestCode = codes[id];
00104       }
00105     }
00106   }
00107   det.id = bestId;
00108   det.hammingDistance = bestHamming;
00109   det.rotation = bestRotation;
00110   det.good = (det.hammingDistance <= errorRecoveryBits);
00111   det.obsCode = rCode;
00112   det.code = bestCode;
00113 }
00114 
00115 void TagFamily::printHammingDistances() const {
00116   vector<int> hammings(dimension*dimension+1);
00117   for (unsigned i = 0; i < codes.size(); i++) {
00118     unsigned long long r0 = codes[i];
00119     unsigned long long r1 = rotate90(r0, dimension);
00120     unsigned long long r2 = rotate90(r1, dimension);
00121     unsigned long long r3 = rotate90(r2, dimension);
00122     for (unsigned int j = i+1; j < codes.size(); j++) {
00123       int d = min(min(hammingDistance(r0, codes[j]),
00124                       hammingDistance(r1, codes[j])),
00125                   min(hammingDistance(r2, codes[j]),
00126                       hammingDistance(r3, codes[j])));
00127       hammings[d]++;
00128     }
00129   }
00130 
00131   for (unsigned int i = 0; i < hammings.size(); i++)
00132     printf("hammings: %u = %d\n", i, hammings[i]);
00133 }
00134 
00135 unsigned char TagFamily::popCountTable[TagFamily::popCountTableSize];
00136 
00137 TagFamily::TableInitializer TagFamily::initializer;
00138 
00139 } // namespace


apriltags
Author(s): Michael Kaess, Hordur Johannson
autogenerated on Thu Jun 6 2019 20:53:23