Revision: 6345
http://playerstage.svn.sourceforge.net/playerstage/?rev=6345&view=rev
Author: gerkey
Date: 2008-04-16 18:36:39 -0700 (Wed, 16 Apr 2008)
Log Message:
-----------
removed GSL dep
Modified Paths:
--------------
code/player/trunk/server/drivers/localization/amcl/Makefile.am
code/player/trunk/server/drivers/localization/amcl/pf/Makefile.am
code/player/trunk/server/drivers/localization/amcl/pf/pf.c
code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.c
code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.h
code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.c
code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.h
Removed Paths:
-------------
code/player/trunk/server/drivers/localization/amcl/pf/gsl_discrete.c
Modified: code/player/trunk/server/drivers/localization/amcl/Makefile.am
===================================================================
--- code/player/trunk/server/drivers/localization/amcl/Makefile.am
2008-04-16 20:07:01 UTC (rev 6344)
+++ code/player/trunk/server/drivers/localization/amcl/Makefile.am
2008-04-17 01:36:39 UTC (rev 6345)
@@ -40,6 +40,8 @@
pf/pf_pdf.h \
pf/pf_kdtree.h \
pf/pf_vector.h \
+ pf/eig3.h \
+ pf/eig3.c \
map/map.c \
map/map_range.c \
map/map_store.c \
Modified: code/player/trunk/server/drivers/localization/amcl/pf/Makefile.am
===================================================================
--- code/player/trunk/server/drivers/localization/amcl/pf/Makefile.am
2008-04-16 20:07:01 UTC (rev 6344)
+++ code/player/trunk/server/drivers/localization/amcl/pf/Makefile.am
2008-04-17 01:36:39 UTC (rev 6345)
@@ -10,4 +10,4 @@
AM_CPPFLAGS = -I$(top_srcdir)/server @GSL_CFLAGS@
endif
-libpf_a_SOURCES = pf.c pf_kdtree.c pf_pdf.c pf_vector.c pf_draw.c
gsl_discrete.c pf.h pf_pdf.h pf_kdtree.h pf_vector.h
+libpf_a_SOURCES = pf.c pf_kdtree.c pf_pdf.c pf_vector.c pf_draw.c
gsl_discrete.c pf.h pf_pdf.h pf_kdtree.h pf_vector.h eig3.c eig3.h
Deleted: code/player/trunk/server/drivers/localization/amcl/pf/gsl_discrete.c
===================================================================
--- code/player/trunk/server/drivers/localization/amcl/pf/gsl_discrete.c
2008-04-16 20:07:01 UTC (rev 6344)
+++ code/player/trunk/server/drivers/localization/amcl/pf/gsl_discrete.c
2008-04-17 01:36:39 UTC (rev 6345)
@@ -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/trunk/server/drivers/localization/amcl/pf/pf.c
===================================================================
--- code/player/trunk/server/drivers/localization/amcl/pf/pf.c 2008-04-16
20:07:01 UTC (rev 6344)
+++ code/player/trunk/server/drivers/localization/amcl/pf/pf.c 2008-04-17
01:36:39 UTC (rev 6345)
@@ -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/trunk/server/drivers/localization/amcl/pf/pf_pdf.c
===================================================================
--- code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.c
2008-04-16 20:07:01 UTC (rev 6344)
+++ code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.c
2008-04-17 01:36:39 UTC (rev 6345)
@@ -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,26 @@
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;
+ do
+ {
+ x1 = 2.0 * drand48() - 1.0;
+ x2 = 2.0 * drand48() - 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 +184,5 @@
return i;
}
+
+#endif
Modified: code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.h
===================================================================
--- code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.h
2008-04-16 20:07:01 UTC (rev 6344)
+++ code/player/trunk/server/drivers/localization/amcl/pf/pf_pdf.h
2008-04-17 01:36:39 UTC (rev 6345)
@@ -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/trunk/server/drivers/localization/amcl/pf/pf_vector.c
===================================================================
--- code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.c
2008-04-16 20:07:01 UTC (rev 6344)
+++ code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.c
2008-04-17 01:36:39 UTC (rev 6345)
@@ -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/trunk/server/drivers/localization/amcl/pf/pf_vector.h
===================================================================
--- code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.h
2008-04-16 20:07:01 UTC (rev 6344)
+++ code/player/trunk/server/drivers/localization/amcl/pf/pf_vector.h
2008-04-17 01:36:39 UTC (rev 6345)
@@ -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
