Revision: 6368
http://playerstage.svn.sourceforge.net/playerstage/?rev=6368&view=rev
Author: gbiggs
Date: 2008-04-20 17:49:39 -0700 (Sun, 20 Apr 2008)
Log Message:
-----------
Merging changes 6344:6350 from trunk
Modified Paths:
--------------
code/player/branches/cmake/libplayercore/configfile.cc
code/player/branches/cmake/server/drivers/localization/amcl/CMakeLists.txt
code/player/branches/cmake/server/drivers/localization/amcl/pf/pf.c
code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_pdf.c
code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_pdf.h
code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_vector.c
code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_vector.h
Added Paths:
-----------
code/player/branches/cmake/server/drivers/localization/amcl/pf/eig3.c
code/player/branches/cmake/server/drivers/localization/amcl/pf/eig3.h
Removed Paths:
-------------
code/player/branches/cmake/server/drivers/localization/amcl/pf/gsl_discrete.c
Modified: code/player/branches/cmake/libplayercore/configfile.cc
===================================================================
--- code/player/branches/cmake/libplayercore/configfile.cc 2008-04-19
01:58:34 UTC (rev 6367)
+++ code/player/branches/cmake/libplayercore/configfile.cc 2008-04-21
00:49:39 UTC (rev 6368)
@@ -246,10 +246,33 @@
const char* name,
const char* value)
{
+ // Work out what the length units are
+ if(!strcmp(name, "unit_length"))
+ {
+ if (strcmp(value, "m") == 0)
+ this->unit_length = 1.0;
+ else if (strcmp(value, "cm") == 0)
+ this->unit_length = 0.01;
+ else if (strcmp(value, "mm") == 0)
+ this->unit_length = 0.001;
+ return;
+ }
+
+ // Work out what the angle units are
+ if(!strcmp(name, "unit_angle"))
+ {
+ if (strcmp(value, "degrees") == 0)
+ this->unit_angle = M_PI / 180;
+ else if (strcmp(value, "radians") == 0)
+ this->unit_angle = 1;
+ return;
+ }
+
// AddField checks whether the field already exists
int field = this->AddField(-1,name,0);
this->AddToken(ConfigFile::TokenWord, value, 0);
this->AddFieldValue(field, index, this->token_count-1);
+
}
Modified:
code/player/branches/cmake/server/drivers/localization/amcl/CMakeLists.txt
===================================================================
--- code/player/branches/cmake/server/drivers/localization/amcl/CMakeLists.txt
2008-04-19 01:58:34 UTC (rev 6367)
+++ code/player/branches/cmake/server/drivers/localization/amcl/CMakeLists.txt
2008-04-21 00:49:39 UTC (rev 6368)
@@ -1,48 +1,32 @@
PLAYERDRIVER_OPTION (amcl build_amcl ON)
-PLAYERDRIVER_REQUIRE_HEADER (amcl build_amcl gsl/gsl_version.h)
-
-SET (amclSrcs amcl.h
- amcl.cc
- amcl_sensor.h
+SET (amclSrcs amcl.cc
amcl_sensor.cc
- amcl_odom.h
amcl_odom.cc
- amcl_laser.h
amcl_laser.cc
- models/odometry.h
models/odometry.c
- models/sonar.h
models/sonar.c
- models/laser.h
models/laser.c
- models/wifi.h
models/wifi.c
- models/gps.h
models/gps.c
- models/imu.h
models/imu.c
pf/pf.c
pf/pf_kdtree.c
pf/pf_pdf.c
pf/pf_vector.c
pf/pf_draw.c
- pf/pf.h
- pf/pf_pdf.h
- pf/pf_kdtree.h
- pf/pf_vector.h
+ pf/eig3.c
map/map.c
map/map_range.c
map/map_store.c
- map/map_draw.c
- map/map.h)
+ map/map_draw.c)
IF (INCLUDE_RTKGUI)
- SET (linkFlags "-lgsl -lgslcblas ${GTK_LINKFLAGS}")
+ SET (linkFlags "${GTK_LINKFLAGS}")
SET (cFlags "-I${PROJECT_SOURCE_DIR}/rtk2 ${GTK_CFLAGS} -DINCLUDE_RTKGUI")
ELSE (INCLUDE_RTKGUI)
- SET (linkFlags "-lgsl -lgslcblas")
- SET (cFlags "")
+ SET (linkFlags)
+ SET (cFlags)
ENDIF (INCLUDE_RTKGUI)
-PLAYERDRIVER_ADD_DRIVER (amcl build_amcl "" "" ${linkFlags} "${cFlags}"
${amclSrcs})
\ No newline at end of file
+PLAYERDRIVER_ADD_DRIVER (amcl build_amcl "" "" "${linkFlags}" "${cFlags}"
${amclSrcs})
\ No newline at end of file
Copied: code/player/branches/cmake/server/drivers/localization/amcl/pf/eig3.c
(from rev 6350, code/player/trunk/server/drivers/localization/amcl/pf/eig3.c)
===================================================================
--- code/player/branches/cmake/server/drivers/localization/amcl/pf/eig3.c
(rev 0)
+++ code/player/branches/cmake/server/drivers/localization/amcl/pf/eig3.c
2008-04-21 00:49:39 UTC (rev 6368)
@@ -0,0 +1,271 @@
+
+/* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
+ domain Java Matrix library JAMA. */
+
+#include <math.h>
+
+#ifndef MAX
+#define MAX(a, b) ((a)>(b)?(a):(b))
+#endif
+
+//#define n 3
+static int n = 3;
+
+static double hypot2(double x, double y) {
+ return sqrt(x*x+y*y);
+}
+
+// Symmetric Householder reduction to tridiagonal form.
+
+static void tred2(double V[n][n], double d[n], double e[n]) {
+
+// This is derived from the Algol procedures tred2 by
+// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
+// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
+// Fortran subroutine in EISPACK.
+
+ int i,j,k;
+ double f,g,h,hh;
+ for (j = 0; j < n; j++) {
+ d[j] = V[n-1][j];
+ }
+
+ // Householder reduction to tridiagonal form.
+
+ for (i = n-1; i > 0; i--) {
+
+ // Scale to avoid under/overflow.
+
+ double scale = 0.0;
+ double h = 0.0;
+ for (k = 0; k < i; k++) {
+ scale = scale + fabs(d[k]);
+ }
+ if (scale == 0.0) {
+ e[i] = d[i-1];
+ for (j = 0; j < i; j++) {
+ d[j] = V[i-1][j];
+ V[i][j] = 0.0;
+ V[j][i] = 0.0;
+ }
+ } else {
+
+ // Generate Householder vector.
+
+ for (k = 0; k < i; k++) {
+ d[k] /= scale;
+ h += d[k] * d[k];
+ }
+ f = d[i-1];
+ g = sqrt(h);
+ if (f > 0) {
+ g = -g;
+ }
+ e[i] = scale * g;
+ h = h - f * g;
+ d[i-1] = f - g;
+ for (j = 0; j < i; j++) {
+ e[j] = 0.0;
+ }
+
+ // Apply similarity transformation to remaining columns.
+
+ for (j = 0; j < i; j++) {
+ f = d[j];
+ V[j][i] = f;
+ g = e[j] + V[j][j] * f;
+ for (k = j+1; k <= i-1; k++) {
+ g += V[k][j] * d[k];
+ e[k] += V[k][j] * f;
+ }
+ e[j] = g;
+ }
+ f = 0.0;
+ for (j = 0; j < i; j++) {
+ e[j] /= h;
+ f += e[j] * d[j];
+ }
+ hh = f / (h + h);
+ for (j = 0; j < i; j++) {
+ e[j] -= hh * d[j];
+ }
+ for (j = 0; j < i; j++) {
+ f = d[j];
+ g = e[j];
+ for (k = j; k <= i-1; k++) {
+ V[k][j] -= (f * e[k] + g * d[k]);
+ }
+ d[j] = V[i-1][j];
+ V[i][j] = 0.0;
+ }
+ }
+ d[i] = h;
+ }
+
+ // Accumulate transformations.
+
+ for (i = 0; i < n-1; i++) {
+ V[n-1][i] = V[i][i];
+ V[i][i] = 1.0;
+ h = d[i+1];
+ if (h != 0.0) {
+ for (k = 0; k <= i; k++) {
+ d[k] = V[k][i+1] / h;
+ }
+ for (j = 0; j <= i; j++) {
+ g = 0.0;
+ for (k = 0; k <= i; k++) {
+ g += V[k][i+1] * V[k][j];
+ }
+ for (k = 0; k <= i; k++) {
+ V[k][j] -= g * d[k];
+ }
+ }
+ }
+ for (k = 0; k <= i; k++) {
+ V[k][i+1] = 0.0;
+ }
+ }
+ for (j = 0; j < n; j++) {
+ d[j] = V[n-1][j];
+ V[n-1][j] = 0.0;
+ }
+ V[n-1][n-1] = 1.0;
+ e[0] = 0.0;
+}
+
+// Symmetric tridiagonal QL algorithm.
+
+static void tql2(double V[n][n], double d[n], double e[n]) {
+
+// This is derived from the Algol procedures tql2, by
+// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
+// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
+// Fortran subroutine in EISPACK.
+
+ int i,j,m,l,k;
+ double g,p,r,dl1,h,f,tst1,eps;
+ double c,c2,c3,el1,s,s2;
+
+ for (i = 1; i < n; i++) {
+ e[i-1] = e[i];
+ }
+ e[n-1] = 0.0;
+
+ f = 0.0;
+ tst1 = 0.0;
+ eps = pow(2.0,-52.0);
+ for (l = 0; l < n; l++) {
+
+ // Find small subdiagonal element
+
+ tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l]));
+ m = l;
+ while (m < n) {
+ if (fabs(e[m]) <= eps*tst1) {
+ break;
+ }
+ m++;
+ }
+
+ // If m == l, d[l] is an eigenvalue,
+ // otherwise, iterate.
+
+ if (m > l) {
+ int iter = 0;
+ do {
+ iter = iter + 1; // (Could check iteration count here.)
+
+ // Compute implicit shift
+
+ g = d[l];
+ p = (d[l+1] - g) / (2.0 * e[l]);
+ r = hypot2(p,1.0);
+ if (p < 0) {
+ r = -r;
+ }
+ d[l] = e[l] / (p + r);
+ d[l+1] = e[l] * (p + r);
+ dl1 = d[l+1];
+ h = g - d[l];
+ for (i = l+2; i < n; i++) {
+ d[i] -= h;
+ }
+ f = f + h;
+
+ // Implicit QL transformation.
+
+ p = d[m];
+ c = 1.0;
+ c2 = c;
+ c3 = c;
+ el1 = e[l+1];
+ s = 0.0;
+ s2 = 0.0;
+ for (i = m-1; i >= l; i--) {
+ c3 = c2;
+ c2 = c;
+ s2 = s;
+ g = c * e[i];
+ h = c * p;
+ r = hypot2(p,e[i]);
+ e[i+1] = s * r;
+ s = e[i] / r;
+ c = p / r;
+ p = c * d[i] - s * g;
+ d[i+1] = h + s * (c * g + s * d[i]);
+
+ // Accumulate transformation.
+
+ for (k = 0; k < n; k++) {
+ h = V[k][i+1];
+ V[k][i+1] = s * V[k][i] + c * h;
+ V[k][i] = c * V[k][i] - s * h;
+ }
+ }
+ p = -s * s2 * c3 * el1 * e[l] / dl1;
+ e[l] = s * p;
+ d[l] = c * p;
+
+ // Check for convergence.
+
+ } while (fabs(e[l]) > eps*tst1);
+ }
+ d[l] = d[l] + f;
+ e[l] = 0.0;
+ }
+
+ // Sort eigenvalues and corresponding vectors.
+
+ for (i = 0; i < n-1; i++) {
+ k = i;
+ p = d[i];
+ for (j = i+1; j < n; j++) {
+ if (d[j] < p) {
+ k = j;
+ p = d[j];
+ }
+ }
+ if (k != i) {
+ d[k] = d[i];
+ d[i] = p;
+ for (j = 0; j < n; j++) {
+ p = V[j][i];
+ V[j][i] = V[j][k];
+ V[j][k] = p;
+ }
+ }
+ }
+}
+
+void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) {
+ int i,j;
+ double e[n];
+ for (i = 0; i < n; i++) {
+ for (j = 0; j < n; j++) {
+ V[i][j] = A[i][j];
+ }
+ }
+ tred2(V, d, e);
+ tql2(V, d, e);
+}
Copied: code/player/branches/cmake/server/drivers/localization/amcl/pf/eig3.h
(from rev 6350, code/player/trunk/server/drivers/localization/amcl/pf/eig3.h)
===================================================================
--- code/player/branches/cmake/server/drivers/localization/amcl/pf/eig3.h
(rev 0)
+++ code/player/branches/cmake/server/drivers/localization/amcl/pf/eig3.h
2008-04-21 00:49:39 UTC (rev 6368)
@@ -0,0 +1,11 @@
+
+/* Eigen-decomposition for symmetric 3x3 real matrices.
+ Public domain, copied from the public domain Java library JAMA. */
+
+#ifndef _eig_h
+
+/* Symmetric matrix A => eigenvectors in columns of V, corresponding
+ eigenvalues in d. */
+void eigen_decomposition(double A[3][3], double V[3][3], double d[3]);
+
+#endif
Deleted:
code/player/branches/cmake/server/drivers/localization/amcl/pf/gsl_discrete.c
===================================================================
---
code/player/branches/cmake/server/drivers/localization/amcl/pf/gsl_discrete.c
2008-04-19 01:58:34 UTC (rev 6367)
+++
code/player/branches/cmake/server/drivers/localization/amcl/pf/gsl_discrete.c
2008-04-21 00:49:39 UTC (rev 6368)
@@ -1,394 +0,0 @@
-/* randist/discrete.c
- *
- * Copyright (C) 1996, 1997, 1998, 1999, 2000 James Theiler, Brian Gough
- *
- * This program 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.
- *
- * This program 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 this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-/*
- Random Discrete Events
-
- Given K discrete events with different probabilities P[k]
- produce a value k consistent with its probability.
-
- This program 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.
-
- This program 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 this program;
- if not, write to the Free Foundation, Inc., 59 Temple Place, Suite
- 330, Boston, MA 02111-1307 USA
-*/
-
-/*
- * Based on: Alastair J Walker, An efficient method for generating
- * discrete random variables with general distributions, ACM Trans
- * Math Soft 3, 253-256 (1977). See also: D. E. Knuth, The Art of
- * Computer Programming, Volume 2 (Seminumerical algorithms), 3rd
- * edition, Addison-Wesley (1997), p120.
-
- * Walker's algorithm does some preprocessing, and provides two
- * arrays: floating point F[k] and integer A[k]. A value k is chosen
- * from 0..K-1 with equal likelihood, and then a uniform random number
- * u is compared to F[k]. If it is less than F[k], then k is
- * returned. Otherwise, A[k] is returned.
-
- * Walker's original paper describes an O(K^2) algorithm for setting
- * up the F and A arrays. I found this disturbing since I wanted to
- * use very large values of K. I'm sure I'm not the first to realize
- * this, but in fact the preprocessing can be done in O(K) steps.
-
- * A figure of merit for the preprocessing is the average value for
- * the F[k]'s (that is, SUM_k F[k]/K); this corresponds to the
- * probability that k is returned, instead of A[k], thereby saving a
- * redirection. Walker's O(K^2) preprocessing will generally improve
- * that figure of merit, compared to my cheaper O(K) method; from some
- * experiments with a perl script, I get values of around 0.6 for my
- * method and just under 0.75 for Walker's. Knuth has pointed out
- * that finding _the_ optimum lookup tables, which maximize the
- * average F[k], is a combinatorially difficult problem. But any
- * valid preprocessing will still provide O(1) time for the call to
- * gsl_ran_discrete(). I find that if I artificially set F[k]=1 --
- * ie, better than optimum! -- I get a speedup of maybe 20%, so that's
- * the maximum I could expect from the most expensive preprocessing.
- * Folding in the difference of 0.6 vs 0.75, I'd estimate that the
- * speedup would be less than 10%.
-
- * I've not implemented it here, but one compromise is to sort the
- * probabilities once, and then work from the two ends inward. This
- * requires O(K log K), still lots cheaper than O(K^2), and from my
- * experiments with the perl script, the figure of merit is within
- * about 0.01 for K up to 1000, and no sign of diverging (in fact,
- * they seemed to be converging, but it's hard to say with just a
- * handful of runs).
-
- * The O(K) algorithm goes through all the p_k's and decides if they
- * are "smalls" or "bigs" according to whether they are less than or
- * greater than the mean value 1/K. The indices to the smalls and the
- * bigs are put in separate stacks, and then we work through the
- * stacks together. For each small, we pair it up with the next big
- * in the stack (Walker always wanted to pair up the smallest small
- * with the biggest big). The small "borrows" from the big just
- * enough to bring the small up to mean. This reduces the size of the
- * big, so the (smaller) big is compared again to the mean, and if it
- * is smaller, it gets "popped" from the big stack and "pushed" to the
- * small stack. Otherwise, it stays put. Since every time we pop a
- * small, we are able to deal with it right then and there, and we
- * never have to pop more than K smalls, then the algorithm is O(K).
-
- * This implementation sets up two separate stacks, and allocates K
- * elements between them. Since neither stack ever grows, we do an
- * extra O(K) pass through the data to determine how many smalls and
- * bigs there are to begin with and allocate appropriately. In all
- * there are 2*K*sizeof(double) transient bytes of memory that are
- * used than returned, and K*(sizeof(int)+sizeof(double)) bytes used
- * in the lookup table.
-
- * Walker spoke of using two random numbers (an integer 0..K-1, and a
- * floating point u in [0,1]), but Knuth points out that one can just
- * use the integer and fractional parts of K*u where u is in [0,1].
- * In fact, Knuth further notes that taking F'[k]=(k+F[k])/K, one can
- * directly compare u to F'[k] without having to explicitly set
- * u=K*u-int(K*u).
-
- * Usage:
-
- * Starting with an array of probabilities P, initialize and do
- * preprocessing with a call to:
-
- * gsl_rng *r;
- * gsl_ran_discrete_t *f;
- * f = gsl_ran_discrete_preproc(K,P);
-
- * Then, whenever a random index 0..K-1 is desired, use
-
- * k = gsl_ran_discrete(r,f);
-
- * Note that several different randevent struct's can be
- * simultaneously active.
-
- * Aside: A very clever alternative approach is described in
- * Abramowitz and Stegun, p 950, citing: Marsaglia, Random variables
- * and computers, Proc Third Prague Conference in Probability Theory,
- * 1962. A more accesible reference is: G. Marsaglia, Generating
- * discrete random numbers in a computer, Comm ACM 6, 37-38 (1963).
- * If anybody is interested, I (jt) have also coded up this version as
- * part of another software package. However, I've done some
- * comparisons, and the Walker method is both faster and more stingy
- * with memory. So, in the end I decided not to include it with the
- * GSL package.
-
- * Written 26 Jan 1999, James Theiler, [EMAIL PROTECTED]
- * Adapted to GSL, 30 Jan 1999, jt
-
- */
-
-//#include <config.h>
-#include <stdio.h> /* used for NULL, also fprintf(stderr,...) */
-#include <stdlib.h> /* used for malloc's */
-#include <math.h>
-#include <gsl/gsl_rng.h>
-#include <gsl/gsl_randist.h>
-#define DEBUG 0
-#define KNUTH_CONVENTION 1 /* Saves a few steps of arithmetic
- * in the call to gsl_ran_discrete()
- */
-
-/*** Begin Stack (this code is used just in this file) ***/
-
-/* Stack code converted to use unsigned indices (i.e. s->i == 0 now
- means an empty stack, instead of -1), for consistency and to give a
- bigger allowable range. BJG */
-
-typedef struct {
- size_t N; /* max number of elts on stack */
- size_t *v; /* array of values on the stack */
- size_t i; /* index of top of stack */
-} gsl_stack_t;
-
-static gsl_stack_t *
-new_stack(size_t N) {
- gsl_stack_t *s;
- s = (gsl_stack_t *)malloc(sizeof(gsl_stack_t));
- s->N = N;
- s->i = 0; /* indicates stack is empty */
- s->v = (size_t *)malloc(sizeof(size_t)*N);
- return s;
-}
-
-static void
-push_stack(gsl_stack_t *s, size_t v)
-{
- if ((s->i) >= (s->N)) {
- fprintf(stderr,"Cannot push stack!\n");
- abort(); /* FIXME: fatal!! */
- }
- (s->v)[s->i] = v;
- s->i += 1;
-}
-
-static size_t pop_stack(gsl_stack_t *s)
-{
- if ((s->i) == 0) {
- fprintf(stderr,"Cannot pop stack!\n");
- abort(); /* FIXME: fatal!! */
- }
- s->i -= 1;
- return ((s->v)[s->i]);
-}
-
-static inline size_t size_stack(const gsl_stack_t *s)
-{
- return s->i;
-}
-
-static void free_stack(gsl_stack_t *s)
-{
- free((char *)(s->v));
- free((char *)s);
-}
-
-/*** End Stack ***/
-
-
-/*** Begin Walker's Algorithm ***/
-
-gsl_ran_discrete_t *
-gsl_ran_discrete_preproc(size_t Kevents, const double *ProbArray)
-{
- size_t k,b,s;
- gsl_ran_discrete_t *g;
- size_t nBigs, nSmalls;
- gsl_stack_t *Bigs;
- gsl_stack_t *Smalls;
- double *E;
- double pTotal = 0.0, mean, d;
-
- if (Kevents < 1) {
- /* Could probably treat Kevents=1 as a special case */
-
- GSL_ERROR_VAL ("number of events must be a positive integer",
- GSL_EINVAL, 0);
- }
-
- /* Make sure elements of ProbArray[] are positive.
- * Won't enforce that sum is unity; instead will just normalize
- */
-
- for (k=0; k<Kevents; ++k) {
- if (ProbArray[k] < 0) {
- GSL_ERROR_VAL ("probabilities must be non-negative",
- GSL_EINVAL, 0) ;
- }
- pTotal += ProbArray[k];
- }
-
- /* Begin setting up the main "object" (just a struct, no steroids) */
- g = (gsl_ran_discrete_t *)malloc(sizeof(gsl_ran_discrete_t));
- g->K = Kevents;
- g->F = (double *)malloc(sizeof(double)*Kevents);
- g->A = (size_t *)malloc(sizeof(size_t)*Kevents);
-
- E = (double *)malloc(sizeof(double)*Kevents);
-
- if (E==NULL) {
- GSL_ERROR_VAL ("Cannot allocate memory for randevent", ENOMEM, 0);
- }
-
- for (k=0; k<Kevents; ++k) {
- E[k] = ProbArray[k]/pTotal;
- }
-
- /* Now create the Bigs and the Smalls */
- mean = 1.0/Kevents;
- nSmalls=nBigs=0;
- for (k=0; k<Kevents; ++k) {
- if (E[k] < mean) ++nSmalls;
- else ++nBigs;
- }
- Bigs = new_stack(nBigs);
- Smalls = new_stack(nSmalls);
- for (k=0; k<Kevents; ++k) {
- if (E[k] < mean) {
- push_stack(Smalls,k);
- }
- else {
- push_stack(Bigs,k);
- }
- }
- /* Now work through the smalls */
- while (size_stack(Smalls) > 0) {
- s = pop_stack(Smalls);
- if (size_stack(Bigs) == 0) {
- /* Then we are on our last value */
- (g->A)[s]=s;
- (g->F)[s]=1.0;
- continue;
- }
- b = pop_stack(Bigs);
- (g->A)[s]=b;
- (g->F)[s]=Kevents*E[s];
-#if DEBUG
- fprintf(stderr,"s=%2d, A=%2d, F=%.4f\n",s,(g->A)[s],(g->F)[s]);
-#endif
- d = mean - E[s];
- E[s] += d; /* now E[s] == mean */
- E[b] -= d;
- if (E[b] < mean) {
- push_stack(Smalls,b); /* no longer big, join ranks of the small */
- }
- else if (E[b] > mean) {
- push_stack(Bigs,b); /* still big, put it back where you found it */
- }
- else {
- /* E[b]==mean implies it is finished too */
- (g->A)[b]=b;
- (g->F)[b]=1.0;
- }
- }
- while (size_stack(Bigs) > 0) {
- b = pop_stack(Bigs);
- (g->A)[b]=b;
- (g->F)[b]=1.0;
- }
- /* Stacks have been emptied, and A and F have been filled */
-
-
-#if 0
- /* if 1, then artificially set all F[k]'s to unity. This will
- * give wrong answers, but you'll get them faster. But, not
- * that much faster (I get maybe 20%); that's an upper bound
- * on what the optimal preprocessing would give.
- */
- for (k=0; k<Kevents; ++k) {
- (g->F)[k] = 1.0;
- }
-#endif
-
-#if KNUTH_CONVENTION
- /* For convenience, set F'[k]=(k+F[k])/K */
- /* This saves some arithmetic in gsl_ran_discrete(); I find that
- * it doesn't actually make much difference.
- */
- for (k=0; k<Kevents; ++k) {
- (g->F)[k] += k;
- (g->F)[k] /= Kevents;
- }
-#endif
-
- free_stack(Bigs);
- free_stack(Smalls);
- free((char *)E);
-
- return g;
-}
-
-size_t
-gsl_ran_discrete(const gsl_rng *r, const gsl_ran_discrete_t *g)
-{
- size_t c=0;
- double u,f;
- u = gsl_rng_uniform(r);
-#if KNUTH_CONVENTION
- c = (u*(g->K));
-#else
- u *= g->K;
- c = u;
- u -= c;
-#endif
- f = (g->F)[c];
- /* fprintf(stderr,"c,f,u: %d %.4f %f\n",c,f,u); */
- if (f == 1.0) return c;
-
- if (u < f) {
- return c;
- }
- else {
- return (g->A)[c];
- }
-}
-
-void gsl_ran_discrete_free(gsl_ran_discrete_t *g)
-{
- free((char *)(g->A));
- free((char *)(g->F));
- free((char *)g);
-}
-
-double
-gsl_ran_discrete_pdf(size_t k, const gsl_ran_discrete_t *g)
-{
- size_t i,K;
- double f,p=0;
- K= g->K;
- if (k>K) return 0;
- for (i=0; i<K; ++i) {
- f = (g->F)[i];
-#if KNUTH_CONVENTION
- f = K*f-i;
-#endif
- if (i==k) {
- p += f;
- } else if (k == (g->A)[i]) {
- p += 1.0 - f;
- }
- }
- return p/K;
-}
Modified: code/player/branches/cmake/server/drivers/localization/amcl/pf/pf.c
===================================================================
--- code/player/branches/cmake/server/drivers/localization/amcl/pf/pf.c
2008-04-19 01:58:34 UTC (rev 6367)
+++ code/player/branches/cmake/server/drivers/localization/amcl/pf/pf.c
2008-04-21 00:49:39 UTC (rev 6368)
@@ -217,15 +217,20 @@
{
int i;
double total;
- double *randlist;
+ //double *randlist;
pf_sample_set_t *set_a, *set_b;
pf_sample_t *sample_a, *sample_b;
- pf_pdf_discrete_t *pdf;
+ //pf_pdf_discrete_t *pdf;
+ double r,c,U;
+ int m;
+ double count_inv;
+
set_a = pf->sets + pf->current_set;
set_b = pf->sets + (pf->current_set + 1) % 2;
// Create the discrete distribution to sample from
+ /*
total = 0;
randlist = calloc(set_a->sample_count, sizeof(double));
for (i = 0; i < set_a->sample_count; i++)
@@ -233,21 +238,48 @@
total += set_a->samples[i].weight;
randlist[i] = set_a->samples[i].weight;
}
+ */
//printf("resample total %f\n", total);
// Initialize the random number generator
- pdf = pf_pdf_discrete_alloc(set_a->sample_count, randlist);
+ //pdf = pf_pdf_discrete_alloc(set_a->sample_count, randlist);
// Create the kd tree for adaptive sampling
pf_kdtree_clear(set_b->kdtree);
-
+
// Draw samples from set a to create set b.
total = 0;
set_b->sample_count = 0;
- while (set_b->sample_count < pf->max_samples)
+
+ // Low-variance resampler, taken from Probabilistic Robotics, p110
+ count_inv = 1.0/set_a->sample_count;
+ r = drand48() * count_inv;
+ c = set_a->samples[0].weight;
+ i = 0;
+ m = 0;
+ while(set_b->sample_count < pf->max_samples)
{
- i = pf_pdf_discrete_sample(pdf);
+ U = r + m * count_inv;
+ while(U>c)
+ {
+ i++;
+ // Handle wrap-around by resetting counters and picking a new random
+ // number
+ if(i >= set_a->sample_count)
+ {
+ r = drand48() * count_inv;
+ c = set_a->samples[0].weight;
+ i = 0;
+ m = 0;
+ U = r + m * count_inv;
+ continue;
+ }
+ c += set_a->samples[i].weight;
+ }
+
+ //i = pf_pdf_discrete_sample(pdf);
+
sample_a = set_a->samples + i;
//printf("%d %f\n", i, sample_a->weight);
@@ -263,17 +295,19 @@
pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
//fprintf(stderr, "resample %d %d %d\n", set_b->sample_count,
set_b->kdtree->leaf_count,
- // pf_resample_limit(pf, set_b->kdtree->leaf_count));
+ //pf_resample_limit(pf, set_b->kdtree->leaf_count));
// See if we have enough samples yet
if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
break;
+
+ m++;
}
//fprintf(stderr, "\n\n");
- pf_pdf_discrete_free(pdf);
- free(randlist);
+ //pf_pdf_discrete_free(pdf);
+ //free(randlist);
// Normalize weights
for (i = 0; i < set_b->sample_count; i++)
Modified:
code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_pdf.c
===================================================================
--- code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_pdf.c
2008-04-19 01:58:34 UTC (rev 6367)
+++ code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_pdf.c
2008-04-21 00:49:39 UTC (rev 6368)
@@ -10,8 +10,8 @@
#include <math.h>
#include <stdlib.h>
#include <string.h>
-#include <gsl/gsl_rng.h>
-#include <gsl/gsl_randist.h>
+//#include <gsl/gsl_rng.h>
+//#include <gsl/gsl_randist.h>
#include "pf_pdf.h"
@@ -33,7 +33,7 @@
pdf->x = x;
pdf->cx = cx;
- pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);
+ //pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);
// Decompose the convariance matrix into a rotation
// matrix and a diagonal matrix.
@@ -43,8 +43,9 @@
pdf->cd.v[2] = sqrt(cd.m[2][2]);
// Initialize the random number generator
- pdf->rng = gsl_rng_alloc(gsl_rng_taus);
- gsl_rng_set(pdf->rng, ++pf_pdf_seed);
+ //pdf->rng = gsl_rng_alloc(gsl_rng_taus);
+ //gsl_rng_set(pdf->rng, ++pf_pdf_seed);
+ srand48(++pf_pdf_seed);
return pdf;
}
@@ -53,12 +54,13 @@
// Destroy the pdf
void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf)
{
- gsl_rng_free(pdf->rng);
+ //gsl_rng_free(pdf->rng);
free(pdf);
return;
}
+/*
// Compute the value of the pdf at some point [x].
double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x)
{
@@ -77,6 +79,7 @@
return p;
}
+*/
// Generate a sample from the the pdf.
@@ -88,7 +91,10 @@
// Generate a random vector
for (i = 0; i < 3; i++)
- r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]);
+ {
+ //r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]);
+ r.v[i] = pf_ran_gaussian(pdf->cd.v[i]);
+ }
for (i = 0; i < 3; i++)
{
@@ -100,7 +106,28 @@
return x;
}
+// Draw randomly from a zero-mean Gaussian distribution, with standard
+// deviation sigma.
+// We use the polar form of the Box-Muller transformation, explained here:
+// http://www.taygeta.com/random/gaussian.html
+double pf_ran_gaussian(double sigma)
+{
+ double x1, x2, w, r;
+ do
+ {
+ do { r = drand48(); } while (r==0.0);
+ x1 = 2.0 * r - 1.0;
+ do { r = drand48(); } while (r==0.0);
+ x2 = 2.0 * r - 1.0;
+ w = x1*x1 + x2*x2;
+ } while(w > 1.0 || w==0.0);
+
+ return(sigma * x2 * sqrt(-2.0*log(w)/w));
+}
+
+#if 0
+
/**************************************************************************
* Discrete
* Note that GSL v1.3 and earlier contains a bug in the discrete
@@ -159,3 +186,5 @@
return i;
}
+
+#endif
Modified:
code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_pdf.h
===================================================================
--- code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_pdf.h
2008-04-19 01:58:34 UTC (rev 6367)
+++ code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_pdf.h
2008-04-21 00:49:39 UTC (rev 6368)
@@ -11,8 +11,8 @@
#include "pf_vector.h"
-#include <gsl/gsl_rng.h>
-#include <gsl/gsl_randist.h>
+//#include <gsl/gsl_rng.h>
+//#include <gsl/gsl_randist.h>
#ifdef __cplusplus
extern "C" {
@@ -28,7 +28,7 @@
// Mean, covariance and inverse covariance
pf_vector_t x;
pf_matrix_t cx;
- pf_matrix_t cxi;
+ //pf_matrix_t cxi;
double cxdet;
// Decomposed covariance matrix (rotation * diagonal)
@@ -36,7 +36,7 @@
pf_vector_t cd;
// A random number generator
- gsl_rng *rng;
+ //gsl_rng *rng;
} pf_pdf_gaussian_t;
@@ -48,12 +48,20 @@
void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf);
// Compute the value of the pdf at some point [z].
-double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z);
+//double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z);
+// Draw randomly from a zero-mean Gaussian distribution, with standard
+// deviation sigma.
+// We use the polar form of the Box-Muller transformation, explained here:
+// http://www.taygeta.com/random/gaussian.html
+double pf_ran_gaussian(double sigma);
+
// Generate a sample from the the pdf.
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf);
+#if 0
+
/**************************************************************************
* Discrete
*************************************************************************/
@@ -85,6 +93,7 @@
// Generate a sample from the the pdf.
int pf_pdf_discrete_sample(pf_pdf_discrete_t *pdf);
+#endif
#ifdef __cplusplus
}
Modified:
code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_vector.c
===================================================================
--- code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_vector.c
2008-04-19 01:58:34 UTC (rev 6367)
+++ code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_vector.c
2008-04-21 00:49:39 UTC (rev 6368)
@@ -7,11 +7,12 @@
*************************************************************************/
#include <math.h>
-#include <gsl/gsl_matrix.h>
-#include <gsl/gsl_eigen.h>
-#include <gsl/gsl_linalg.h>
+//#include <gsl/gsl_matrix.h>
+//#include <gsl/gsl_eigen.h>
+//#include <gsl/gsl_linalg.h>
#include "pf_vector.h"
+#include "eig3.h"
// Return a zero vector
@@ -156,6 +157,7 @@
}
+/*
// Compute the matrix inverse
pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det)
{
@@ -193,6 +195,7 @@
return ai;
}
+*/
// Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal
@@ -200,6 +203,7 @@
void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a)
{
int i, j;
+ /*
gsl_matrix *aa;
gsl_vector *eval;
gsl_matrix *evec;
@@ -208,27 +212,45 @@
aa = gsl_matrix_alloc(3, 3);
eval = gsl_vector_alloc(3);
evec = gsl_matrix_alloc(3, 3);
+ */
+ double aa[3][3];
+ double eval[3];
+ double evec[3][3];
+
for (i = 0; i < 3; i++)
+ {
for (j = 0; j < 3; j++)
- gsl_matrix_set(aa, i, j, a.m[i][j]);
+ {
+ //gsl_matrix_set(aa, i, j, a.m[i][j]);
+ aa[i][j] = a.m[i][j];
+ }
+ }
// Compute eigenvectors/values
+ /*
w = gsl_eigen_symmv_alloc(3);
gsl_eigen_symmv(aa, eval, evec, w);
gsl_eigen_symmv_free(w);
+ */
+ eigen_decomposition(aa,evec,eval);
+
*d = pf_matrix_zero();
for (i = 0; i < 3; i++)
{
- d->m[i][i] = gsl_vector_get(eval, i);
+ //d->m[i][i] = gsl_vector_get(eval, i);
+ d->m[i][i] = eval[i];
for (j = 0; j < 3; j++)
- r->m[i][j] = gsl_matrix_get(evec, i, j);
+ {
+ //r->m[i][j] = gsl_matrix_get(evec, i, j);
+ r->m[i][j] = evec[i][j];
+ }
}
- gsl_matrix_free(evec);
- gsl_vector_free(eval);
- gsl_matrix_free(aa);
+ //gsl_matrix_free(evec);
+ //gsl_vector_free(eval);
+ //gsl_matrix_free(aa);
return;
}
Modified:
code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_vector.h
===================================================================
--- code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_vector.h
2008-04-19 01:58:34 UTC (rev 6367)
+++ code/player/branches/cmake/server/drivers/localization/amcl/pf/pf_vector.h
2008-04-21 00:49:39 UTC (rev 6368)
@@ -62,7 +62,7 @@
// Compute the matrix inverse. Will also return the determinant,
// which should be checked for underflow (indicated singular matrix).
-pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det);
+//pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det);
// Decompose a covariance matrix [a] into a rotation matrix [r] and a
// diagonal matrix [d] such that a = r * d * r^T.
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
This SF.net email is sponsored by the 2008 JavaOne(SM) Conference
Don't miss this year's exciting event. There's still time to save $100.
Use priority code J8TL2D2.
http://ad.doubleclick.net/clk;198757673;13503038;p?http://java.sun.com/javaone
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
