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 }