gaussian.cpp

移动机器人SLAM,参考一位外国人的硕士论文

/*
  slam3 (c) 2006 Kris Beevers

  This file is part of slam3.

  slam3 is free software; you can redistribute it and/or modify it
  under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.

  slam3 is distributed in the hope that it will be useful, but WITHOUT
  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public
  License for more details.

  You should have received a copy of the GNU General Public License
  along with slam3; if not, write to the Free Software Foundation,
  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/

//
// Gaussian random numbers
// Kris Beevers ([e-mail])
// $Id: gaussian.cpp,v 1.4 2005/10/27 23:29:20 beevek Exp $
//

#include "slam.hpp"
#include <stdlib.h>
#include <math.h>


//////////////////////////////////////////////////////////////////////
// 1d gaussian
//////////////////////////////////////////////////////////////////////

// before using this function you MUST SEED THE RNG with srand48()

// note that this is not reentrant... if you want reentrant, look at
// the GSL implementation of the ratio method

// i clocked this at about 507 ns per call on a pentium-m 1.7 GHz
// averaged over 1000000 calls

double gaussian(double mean, double stddev)
{
  static bool cached = false;
  static double extra;

  if(cached) {
    cached = false;
    return extra * stddev + mean;
  }
  
  // pick random point in unit circle
  static double a, b, c, t;
  do {
    a = 2.0 * drand48() - 1.0;
    b = 2.0 * drand48() - 1.0;
    c = a * a + b * b;
  } while(c > 1.0 || c == 0);
  t = ::sqrt(-2.0 * ::log(c) / c);
  extra = t * a;
  cached = true;
  return t * b * stddev + mean;
}


//////////////////////////////////////////////////////////////////////
// 3d gaussian for sampling poses
//////////////////////////////////////////////////////////////////////

// explicitly written-out cholesky decomposition of a 3x3 symmetric
// (positive, but check for that anyway) definite matrix: find a
// triangular matrix L such that LL' = cov; this follows the
// Cholesky-Banachiewicz algorithm (see, e.g.,
// http://en.wikipedia.org/wiki/Cholesky_decomposition)
inline void cholesky(const cov3_t cov, cov3_t &out)
{
  out[0][0] = sqrt(cov[0][0]);
  if(out[0][0] > 0) {
    out[0][1] = cov[0][1] / out[0][0];
    out[0][2] = cov[0][2] / out[0][0];
  } else
    out[0][1] = out[0][2] = 0;
  out[1][1] = sqrt(cov[1][1] - out[0][1]*out[0][1] + 1e-20); // add an epsilon for numerical error
  if(out[1][1] > 0)
    out[1][2] = (cov[1][2] - out[0][2]*out[0][1]) / out[1][1];
  else
    out[1][2] = 0;
  out[2][2] = sqrt(cov[2][2] - out[0][2]*out[0][2] - out[1][2]*out[1][2] + 1e-20);
  out[1][0] = out[0][1];  out[2][0] = out[0][2];  out[2][1] = out[1][2];
}

pose_t gaussian(const pose_t &mean, const cov3_t &cov)
{
  // first, compute the cholesky decomposition to find an L such that
  // LL' = cov
  static cov3_t L;
  cholesky(cov, L);

  // now draw a vector z of n (3 in this case) independent values from
  // the standard normal distribution N(0,1)
  pose_t z(gaussian(0,1), gaussian(0,1), gaussian(0,1));

  // finally, return the sample as: mean + Lz
  return mean + L*z;
}