MEstimator.h
Go to the documentation of this file.
00001 // -*- c++ -*-
00002 // Copyright 2008 Isis Innovation Limited
00003 
00004 // MEstimator.h
00005 //
00006 // Defines various MEstimators which can be used by the Tracker and
00007 // the Bundle adjuster. Not that some of the inputs are square
00008 // quantities!
00009 
00010 #ifndef __MESTIMATOR_H
00011 #define __MESTIMATOR_H
00012 #include <TooN/TooN.h>
00013 using namespace TooN;
00014 #include <vector>
00015 #include <algorithm>
00016 #include <cassert>
00017 
00018 struct Tukey
00019 {
00020   inline static double FindSigmaSquared(std::vector<double> &vdErrorSquared);
00021   inline static double SquareRootWeight(double dErrorSquared, double dSigmaSquared);
00022   inline static double Weight(double dErrorSquared, double dSigmaSquared);
00023   inline static double ObjectiveScore(double dErrorSquared, double dSigmaSquared);
00024 };
00025 
00026 struct Cauchy
00027 {
00028   inline static double FindSigmaSquared(std::vector<double> &vdErrorSquared);
00029   inline static double SquareRootWeight(double dErrorSquared, double dSigmaSquared);
00030   inline static double Weight(double dErrorSquared, double dSigmaSquared);
00031   inline static double ObjectiveScore(double dErrorSquared, double dSigmaSquared);
00032 };
00033 
00034 struct Huber
00035 {
00036   inline static double FindSigmaSquared(std::vector<double> &vdErrorSquared);
00037   inline static double SquareRootWeight(double dErrorSquared, double dSigmaSquared);
00038   inline static double Weight(double dErrorSquared, double dSigmaSquared);
00039   inline static double ObjectiveScore(double dErrorSquared, double dSigmaSquared);
00040 };
00041 
00042 struct LeastSquares
00043 {
00044   inline static double FindSigmaSquared(std::vector<double> &vdErrorSquared);
00045   inline static double SquareRootWeight(double dErrorSquared, double dSigmaSquared);
00046   inline static double Weight(double dErrorSquared, double dSigmaSquared);
00047   inline static double ObjectiveScore(double dErrorSquared, double dSigmaSquared);
00048 };
00049 
00050 
00051 inline double Tukey::Weight(double dErrorSquared, double dSigmaSquared)
00052 {
00053   double dSqrt = SquareRootWeight(dErrorSquared, dSigmaSquared);
00054   return dSqrt * dSqrt;
00055 }
00056 
00057 inline double Tukey::SquareRootWeight(double dErrorSquared, double dSigmaSquared)
00058 {
00059   if(dErrorSquared > dSigmaSquared)
00060     return 0.0;
00061   else
00062     return 1.0 - (dErrorSquared / dSigmaSquared);
00063 }
00064 
00065 inline double Tukey::ObjectiveScore(double dErrorSquared, const double dSigmaSquared)
00066 {
00067   // NB All returned are scaled because
00068   // I'm not multiplying by sigmasquared/6.0
00069   if(dErrorSquared > dSigmaSquared)
00070     return 1.0;
00071   double d = 1.0 - dErrorSquared / dSigmaSquared;
00072   return (1.0 - d*d*d);
00073 }
00074 
00075 
00076 inline double Tukey::FindSigmaSquared(std::vector<double> &vdErrorSquared)
00077 { 
00078   double dSigmaSquared; 
00079   assert(vdErrorSquared.size() > 0);
00080   std::sort(vdErrorSquared.begin(), vdErrorSquared.end());
00081   double dMedianSquared = vdErrorSquared[vdErrorSquared.size() / 2];
00082   double dSigma = 1.4826 * (1 + 5.0 / (vdErrorSquared.size() * 2 - 6)) * sqrt(dMedianSquared);
00083   dSigma =  4.6851 * dSigma;
00084   dSigmaSquared = dSigma * dSigma;
00085   return dSigmaSquared;
00086 }
00087 
00088 
00093 
00094 inline double Cauchy::Weight(double dErrorSquared, double dSigmaSquared)
00095 {
00096   return 1.0 / (1.0 + dErrorSquared / dSigmaSquared);
00097 }
00098 
00099 inline double Cauchy::SquareRootWeight(double dErrorSquared, double dSigmaSquared)
00100 {
00101   return sqrt(Weight(dErrorSquared, dSigmaSquared));
00102 }
00103 
00104 inline double Cauchy::ObjectiveScore(double dErrorSquared, const double dSigmaSquared)
00105 {
00106   return log(1.0 + dErrorSquared / dSigmaSquared);
00107 }
00108 
00109 
00110 inline double Cauchy::FindSigmaSquared(std::vector<double> &vdErrorSquared)
00111 { 
00112   double dSigmaSquared; 
00113   assert(vdErrorSquared.size() > 0);
00114   std::sort(vdErrorSquared.begin(), vdErrorSquared.end());
00115   double dMedianSquared = vdErrorSquared[vdErrorSquared.size() / 2];
00116   double dSigma = 1.4826 * (1 + 5.0 / (vdErrorSquared.size() * 2 - 6)) * sqrt(dMedianSquared);
00117   dSigma =  4.6851 * dSigma;
00118   dSigmaSquared = dSigma * dSigma;
00119   return dSigmaSquared;
00120 }
00121 
00122 
00127 
00128 inline double Huber::Weight(double dErrorSquared, double dSigmaSquared)
00129 {
00130   if(dErrorSquared < dSigmaSquared)
00131     return 1;
00132   else
00133     return sqrt(dSigmaSquared / dErrorSquared);
00134 }
00135 
00136 inline double Huber::SquareRootWeight(double dErrorSquared, double dSigmaSquared)
00137 {
00138   return sqrt(Weight(dErrorSquared, dSigmaSquared));
00139 }
00140 
00141 inline double Huber::ObjectiveScore(double dErrorSquared, const double dSigmaSquared)
00142 {
00143   if(dErrorSquared< dSigmaSquared)
00144     return 0.5 * dErrorSquared;
00145   else
00146     {
00147       double dSigma = sqrt(dSigmaSquared);
00148       double dError = sqrt(dErrorSquared);
00149       return dSigma * ( dError - 0.5 * dSigma);
00150     }
00151 }
00152 
00153 
00154 inline double Huber::FindSigmaSquared(std::vector<double> &vdErrorSquared)
00155 { 
00156   double dSigmaSquared; 
00157   assert(vdErrorSquared.size() > 0);
00158   std::sort(vdErrorSquared.begin(), vdErrorSquared.end());
00159   double dMedianSquared = vdErrorSquared[vdErrorSquared.size() / 2];
00160   double dSigma = 1.4826 * (1 + 5.0 / (vdErrorSquared.size() * 2 - 6)) * sqrt(dMedianSquared);
00161   dSigma =  1.345 * dSigma;
00162   dSigmaSquared = dSigma * dSigma;
00163   return dSigmaSquared;
00164 }
00165 
00170 
00171 inline double LeastSquares::Weight(double dErrorSquared, double dSigmaSquared)
00172 {
00173   return 1.0;
00174 }
00175 
00176 inline double LeastSquares::SquareRootWeight(double dErrorSquared, double dSigmaSquared)
00177 {
00178   return 1.0;
00179 }
00180 
00181 inline double LeastSquares::ObjectiveScore(double dErrorSquared, const double dSigmaSquared)
00182 {
00183   return dErrorSquared;
00184 }
00185 
00186 
00187 inline double LeastSquares::FindSigmaSquared(std::vector<double> &vdErrorSquared)
00188 { 
00189   if(vdErrorSquared.size() == 0)
00190     return 0.0;
00191   double dSum = 0.0;
00192   for(unsigned int i=0; i<vdErrorSquared.size(); i++)
00193     dSum+=vdErrorSquared[i];
00194   return dSum / vdErrorSquared.size();
00195 }
00196 
00197 #endif
00198 
00199 
00200 
00201 
00202 
00203 
00204 
00205 
00206 
00207 
00208 


tum_ardrone
Author(s):
autogenerated on Sat Jun 8 2019 20:27:23