amcl源码分析

1.pf.h

/************************************************************************** * Desc: Simple particle filter for localization. * Author: Andrew Howard * Date: 10 Dec 2002 * CVS: $Id: pf.h 3293 2005-11-19 08:37:45Z gerkey $ *************************************************************************/#ifndef PF_H#define PF_H#include "pf_vector.h"#include "pf_kdtree.h"#ifdef __cplusplusextern "C" {#endif// Forward declarationsstruct _pf_t;struct _rtk_fig_t;struct _pf_sample_set_t;// Function prototype for the initialization model; generates a sample pose from// an appropriate distribution.typedef pf_vector_t (*pf_init_model_fn_t) (void *init_data);// Function prototype for the action model; generates a sample pose from// an appropriate distributiontypedef void (*pf_action_model_fn_t) (void *action_data,                                       struct _pf_sample_set_t* set);// Function prototype for the sensor model; determines the probability// for the given set of sample poses.typedef double (*pf_sensor_model_fn_t) (void *sensor_data,                                         struct _pf_sample_set_t* set);// Information for a single sampletypedef struct{  // Pose represented by this sample  pf_vector_t pose;  // Weight for this pose  double weight;} pf_sample_t;// Information for a cluster of samplestypedef struct{  // Number of samples  int count;  // Total weight of samples in this cluster  double weight;  // Cluster statistics  pf_vector_t mean;  pf_matrix_t cov;  // Workspace  double m[4], c[2][2];} pf_cluster_t;// Information for a set of samplestypedef struct _pf_sample_set_t{  // The samples  int sample_count;  pf_sample_t *samples;  // A kdtree encoding the histogram  pf_kdtree_t *kdtree;  // Clusters  int cluster_count, cluster_max_count;  pf_cluster_t *clusters;  // Filter statistics  pf_vector_t mean;  pf_matrix_t cov;  int converged; } pf_sample_set_t;// Information for an entire filtertypedef struct _pf_t{  // This min and max number of samples  int min_samples, max_samples;  // Population size parameters  double pop_err, pop_z;  // The sample sets.  We keep two sets and use [current_set]  // to identify the active set.  int current_set;  pf_sample_set_t sets[2];  // Running averages, slow and fast, of likelihood  double w_slow, w_fast;  // Decay rates for running averages  double alpha_slow, alpha_fast;  // Function used to draw random pose samples  pf_init_model_fn_t random_pose_fn;  void *random_pose_data;  double dist_threshold; //distance threshold in each axis over which the pf is considered to not be converged  int converged; } pf_t;// Create a new filterpf_t *pf_alloc(int min_samples, int max_samples,               double alpha_slow, double alpha_fast,               pf_init_model_fn_t random_pose_fn, void *random_pose_data);// Free an existing filtervoid pf_free(pf_t *pf);// Initialize the filter using a guassianvoid pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov);// Initialize the filter using some modelvoid pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data);// Update the filter with some new actionvoid pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data);// Update the filter with some new sensor observationvoid pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);// Resample the distributionvoid pf_update_resample(pf_t *pf);// Compute the CEP statistics (mean and variance).void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var);// Compute the statistics for a particular cluster.  Returns 0 if// there is no such cluster.int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight,                         pf_vector_t *mean, pf_matrix_t *cov);// Display the sample setvoid pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples);// Draw the histogram (kdtree)void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig);// Draw the CEP statisticsvoid pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig);// Draw the cluster statisticsvoid pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig);//calculate if the particle filter has converged - //and sets the converged flag in the current set and the pf int pf_update_converged(pf_t *pf);//sets the current set and pf converged values to zerovoid pf_init_converged(pf_t *pf);#ifdef __cplusplus}#endif#endif

2.pf_kdtree.h

/************************************************************************** * Desc: KD tree functions * Author: Andrew Howard * Date: 18 Dec 2002 * CVS: $Id: pf_kdtree.h 6532 2008-06-11 02:45:56Z gbiggs $ *************************************************************************/#ifndef PF_KDTREE_H#define PF_KDTREE_H#ifdef INCLUDE_RTKGUI#include "rtk.h"#endif// Info for a node in the treetypedef struct pf_kdtree_node{  // Depth in the tree  int leaf, depth;  // Pivot dimension and value  int pivot_dim;  double pivot_value;  // The key for this node  int key[3];  // The value for this node  double value;  // The cluster label (leaf nodes)  int cluster;  // Child nodes  struct pf_kdtree_node *children[2];} pf_kdtree_node_t;// A kd treetypedef struct{  // Cell size  double size[3];  // The root node of the tree  pf_kdtree_node_t *root;  // The number of nodes in the tree  int node_count, node_max_count;  pf_kdtree_node_t *nodes;  // The number of leaf nodes in the tree  int leaf_count;} pf_kdtree_t;// Create a treeextern pf_kdtree_t *pf_kdtree_alloc(int max_size);// Destroy a treeextern void pf_kdtree_free(pf_kdtree_t *self);// Clear all entries from the treeextern void pf_kdtree_clear(pf_kdtree_t *self);// Insert a pose into the treeextern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value);// Cluster the leaves in the treeextern void pf_kdtree_cluster(pf_kdtree_t *self);// Determine the probability estimate for the given poseextern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose);// Determine the cluster label for the given poseextern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose);#ifdef INCLUDE_RTKGUI// Draw the treeextern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig);#endif#endif

3.pf_vector.h

/************************************************************************** * Desc: Vector functions * Author: Andrew Howard * Date: 10 Dec 2002 * CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $ *************************************************************************/#ifndef PF_VECTOR_H#define PF_VECTOR_H#ifdef __cplusplusextern "C" {#endif#include <stdio.h>// The basic vectortypedef struct{  double v[3];} pf_vector_t;// The basic matrixtypedef struct{  double m[3][3];} pf_matrix_t;// Return a zero vectorpf_vector_t pf_vector_zero();// Check for NAN or INF in any componentint pf_vector_finite(pf_vector_t a);// Print a vectorvoid pf_vector_fprintf(pf_vector_t s, FILE *file, const char *fmt);// Simple vector additionpf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b);// Simple vector subtractionpf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b);// Transform from local to global coords (a + b)pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);// Transform from global to local coords (a - b)pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b);// Return a zero matrixpf_matrix_t pf_matrix_zero();// Check for NAN or INF in any componentint pf_matrix_finite(pf_matrix_t a);// Print a matrixvoid pf_matrix_fprintf(pf_matrix_t s, FILE *file, const char *fmt);// 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);// Decompose a covariance matrix [a] into a rotation matrix [r] and a// diagonal matrix [d] such that a = r * d * r^T.void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a);#ifdef __cplusplus}#endif#endif