mestimator.h

this is software for visual SLAM

// -*- c++ -*-
// Copyright 2008 Isis Innovation Limited

// MEstimator.h
//
// Defines various MEstimators which can be used by the Tracker and
// the Bundle adjuster. Not that some of the inputs are square
// quantities!

#ifndef __MESTIMATOR_H
#define __MESTIMATOR_H
#include <TooN/numerics.h>
#include <vector>
#include <algorithm>

struct Tukey
{
  inline static double FindSigmaSquared(std::vector<double> &vdErrorSquared);
  inline static double SquareRootWeight(double dErrorSquared, double dSigmaSquared);
  inline static double Weight(double dErrorSquared, double dSigmaSquared);
  inline static double ObjectiveScore(double dErrorSquared, double dSigmaSquared);
};

struct Cauchy
{
  inline static double FindSigmaSquared(std::vector<double> &vdErrorSquared);
  inline static double SquareRootWeight(double dErrorSquared, double dSigmaSquared);
  inline static double Weight(double dErrorSquared, double dSigmaSquared);
  inline static double ObjectiveScore(double dErrorSquared, double dSigmaSquared);
};

struct Huber
{
  inline static double FindSigmaSquared(std::vector<double> &vdErrorSquared);
  inline static double SquareRootWeight(double dErrorSquared, double dSigmaSquared);
  inline static double Weight(double dErrorSquared, double dSigmaSquared);
  inline static double ObjectiveScore(double dErrorSquared, double dSigmaSquared);
};

struct LeastSquares
{
  inline static double FindSigmaSquared(std::vector<double> &vdErrorSquared);
  inline static double SquareRootWeight(double dErrorSquared, double dSigmaSquared);
  inline static double Weight(double dErrorSquared, double dSigmaSquared);
  inline static double ObjectiveScore(double dErrorSquared, double dSigmaSquared);
};


inline double Tukey::Weight(double dErrorSquared, double dSigmaSquared)
{
  double dSqrt = SquareRootWeight(dErrorSquared, dSigmaSquared);
  return dSqrt * dSqrt;
}

inline double Tukey::SquareRootWeight(double dErrorSquared, double dSigmaSquared)
{
  if(dErrorSquared > dSigmaSquared)
    return 0.0;
  else
    return 1.0 - (dErrorSquared / dSigmaSquared);
}

inline double Tukey::ObjectiveScore(double dErrorSquared, const double dSigmaSquared)
{
  // NB All returned are scaled because
  // I'm not multiplying by sigmasquared/6.0
  if(dErrorSquared > dSigmaSquared)
    return 1.0;
  double d = 1.0 - dErrorSquared / dSigmaSquared;
  return (1.0 - d*d*d);
}


inline double Tukey::FindSigmaSquared(std::vector<double> &vdErrorSquared)
{ 
  double dSigmaSquared; 
  assert(vdErrorSquared.size() > 0);
  std::sort(vdErrorSquared.begin(), vdErrorSquared.end());
  double dMedianSquared = vdErrorSquared[vdErrorSquared.size() / 2];
  double dSigma = 1.4826 * (1 + 5.0 / (vdErrorSquared.size() * 2 - 6)) * sqrt(dMedianSquared);
  dSigma =  4.6851 * dSigma;
  dSigmaSquared = dSigma * dSigma;
  return dSigmaSquared;
}


///////////////////////////////////////
///////////////////////////////////////
///////////////////////////////////////
///////////////////////////////////////

inline double Cauchy::Weight(double dErrorSquared, double dSigmaSquared)
{
  return 1.0 / (1.0 + dErrorSquared / dSigmaSquared);
}

inline double Cauchy::SquareRootWeight(double dErrorSquared, double dSigmaSquared)
{
  return sqrt(Weight(dErrorSquared, dSigmaSquared));
}

inline double Cauchy::ObjectiveScore(double dErrorSquared, const double dSigmaSquared)
{
  return log(1.0 + dErrorSquared / dSigmaSquared);
}


inline double Cauchy::FindSigmaSquared(std::vector<double> &vdErrorSquared)
{ 
  double dSigmaSquared; 
  assert(vdErrorSquared.size() > 0);
  std::sort(vdErrorSquared.begin(), vdErrorSquared.end());
  double dMedianSquared = vdErrorSquared[vdErrorSquared.size() / 2];
  double dSigma = 1.4826 * (1 + 5.0 / (vdErrorSquared.size() * 2 - 6)) * sqrt(dMedianSquared);
  dSigma =  4.6851 * dSigma;
  dSigmaSquared = dSigma * dSigma;
  return dSigmaSquared;
}


///////////////////////////////////////
///////////////////////////////////////
///////////////////////////////////////
///////////////////////////////////////

inline double Huber::Weight(double dErrorSquared, double dSigmaSquared)
{
  if(dErrorSquared < dSigmaSquared)
    return 1;
  else
    return sqrt(dSigmaSquared / dErrorSquared);
}

inline double Huber::SquareRootWeight(double dErrorSquared, double dSigmaSquared)
{
  return sqrt(Weight(dErrorSquared, dSigmaSquared));
}

inline double Huber::ObjectiveScore(double dErrorSquared, const double dSigmaSquared)
{
  if(dErrorSquared< dSigmaSquared)
    return 0.5 * dErrorSquared;
  else
    {
      double dSigma = sqrt(dSigmaSquared);
      double dError = sqrt(dErrorSquared);
      return dSigma * ( dError - 0.5 * dSigma);
    }
}


inline double Huber::FindSigmaSquared(std::vector<double> &vdErrorSquared)
{ 
  double dSigmaSquared; 
  assert(vdErrorSquared.size() > 0);
  std::sort(vdErrorSquared.begin(), vdErrorSquared.end());
  double dMedianSquared = vdErrorSquared[vdErrorSquared.size() / 2];
  double dSigma = 1.4826 * (1 + 5.0 / (vdErrorSquared.size() * 2 - 6)) * sqrt(dMedianSquared);
  dSigma =  1.345 * dSigma;
  dSigmaSquared = dSigma * dSigma;
  return dSigmaSquared;
}

///////////////////////////////////////
///////////////////////////////////////
///////////////////////////////////////
///////////////////////////////////////

inline double LeastSquares::Weight(double dErrorSquared, double dSigmaSquared)
{
  return 1.0;
}

inline double LeastSquares::SquareRootWeight(double dErrorSquared, double dSigmaSquared)
{
  return 1.0;
}

inline double LeastSquares::ObjectiveScore(double dErrorSquared, const double dSigmaSquared)
{
  return dErrorSquared;
}


inline double LeastSquares::FindSigmaSquared(std::vector<double> &vdErrorSquared)
{ 
  if(vdErrorSquared.size() == 0)
    return 0.0;
  double dSum = 0.0;
  for(unsigned int i=0; i<vdErrorSquared.size(); i++)
    dSum+=vdErrorSquared[i];
  return dSum / vdErrorSquared.size();
}

#endif