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
00020
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
00048
00049
00050
00051 dirty = false;
00052 if (nobs >= 6) {
00053
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
00060
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
00071
00072
00073 }
00074
00075
00076
00077 v.setZero();
00078 v[3] = b[3] / nobs;
00079 }
00080
00081 }