GrayModel.cc
Go to the documentation of this file.
00001 #include <iostream>
00002 
00003 #include <Eigen/Dense>
00004 #include <Eigen/LU>
00005 
00006 #include "AprilTags/GrayModel.h"
00007 
00008 namespace AprilTags {
00009 
00010 GrayModel::GrayModel() : A(), v(), b(), nobs(0), dirty(false) {
00011   A.setZero();
00012   v.setZero();
00013   b.setZero();
00014 }
00015 
00016 void GrayModel::addObservation(float x, float y, float gray) {
00017   float xy = x*y;
00018 
00019   // update only upper-right elements. A'A is symmetric,
00020   // we'll fill the other elements in later.
00021   A(0,0) += x*x;
00022   A(0,1) += x*y;
00023   A(0,2) += x*xy;
00024   A(0,3) += x;
00025   A(1,1) += y*y;
00026   A(1,2) += y*xy;
00027   A(1,3) += y;
00028   A(2,2) += xy*xy;
00029   A(2,3) += xy;
00030   A(3,3) += 1;
00031   
00032   b[0] += x*gray;
00033   b[1] += y*gray;
00034   b[2] += xy*gray;
00035   b[3] += gray;
00036 
00037   nobs++;
00038   dirty = true;
00039 }
00040 
00041 float GrayModel::interpolate(float x, float y) {
00042   if (dirty) compute();
00043   return v[0]*x + v[1]*y + v[2]*x*y + v[3];
00044 }
00045 
00046 void GrayModel::compute() {
00047   // we really only need 4 linearly independent observations to fit our answer, but we'll be very
00048   // sensitive to noise if we don't have an over-determined system. Thus, require at least 6
00049   // observations (or we'll use a constant model below).
00050 
00051   dirty = false;
00052   if (nobs >= 6) {
00053     // make symmetric
00054     Eigen::Matrix4d Ainv;
00055     for (int i = 0; i < 4; i++)
00056       for (int j = i+1; j < 4; j++)
00057         A(j,i) = A(i,j);
00058 
00059     //    try {
00060     //      Ainv = A.inverse();
00061     bool invertible;
00062     double det_unused;
00063     A.computeInverseAndDetWithCheck(Ainv, det_unused, invertible);
00064     if (invertible) {
00065       v = Ainv * b;
00066       return;
00067     }
00068     std::cerr << "AprilTags::GrayModel::compute() has underflow in matrix inverse\n";
00069     //    }
00070     //    catch (std::underflow_error&) {
00071     //      std::cerr << "AprilTags::GrayModel::compute() has underflow in matrix inverse\n";
00072     //    }
00073   }
00074 
00075   // If we get here, either nobs < 6 or the matrix inverse generated
00076   // an underflow, so use a constant model.
00077   v.setZero();   // need the cast to avoid operator= ambiguity wrt. const-ness
00078   v[3] = b[3] / nobs;      
00079 }
00080 
00081 } // namespace


apriltags
Author(s): Mitchell Wills
autogenerated on Thu Aug 27 2015 12:23:28