pf.h
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #ifndef PF_H
00032 #define PF_H
00033
00034 #include "pf_vector.h"
00035 #include "pf_kdtree.h"
00036
00037 #ifdef __cplusplus
00038 extern "C" {
00039 #endif
00040
00041
00042 struct _pf_t;
00043 struct _rtk_fig_t;
00044 struct _pf_sample_set_t;
00045
00046
00047
00048 typedef pf_vector_t (*pf_init_model_fn_t) (void *init_data);
00049
00050
00051
00052 typedef void (*pf_action_model_fn_t) (void *action_data,
00053 struct _pf_sample_set_t* set);
00054
00055
00056
00057 typedef double (*pf_sensor_model_fn_t) (void *sensor_data,
00058 struct _pf_sample_set_t* set);
00059
00060
00061
00062 typedef struct
00063 {
00064
00065 pf_vector_t pose;
00066
00067
00068 double weight;
00069
00070 } pf_sample_t;
00071
00072
00073
00074 typedef struct
00075 {
00076
00077 int count;
00078
00079
00080 double weight;
00081
00082
00083 pf_vector_t mean;
00084 pf_matrix_t cov;
00085
00086
00087 double m[4], c[2][2];
00088
00089 } pf_cluster_t;
00090
00091
00092
00093 typedef struct _pf_sample_set_t
00094 {
00095
00096 int sample_count;
00097 pf_sample_t *samples;
00098
00099
00100 pf_kdtree_t *kdtree;
00101
00102
00103 int cluster_count, cluster_max_count;
00104 pf_cluster_t *clusters;
00105
00106 } pf_sample_set_t;
00107
00108
00109
00110 typedef struct _pf_t
00111 {
00112
00113 int min_samples, max_samples;
00114
00115
00116 double pop_err, pop_z;
00117
00118
00119
00120 int current_set;
00121 pf_sample_set_t sets[2];
00122
00123 } pf_t;
00124
00125
00126
00127 pf_t *pf_alloc(int min_samples, int max_samples);
00128
00129
00130 void pf_free(pf_t *pf);
00131
00132
00133 void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov);
00134
00135
00136 void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data);
00137
00138
00139 void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data);
00140
00141
00142 void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);
00143
00144
00145 void pf_update_resample(pf_t *pf);
00146
00147
00148 void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var);
00149
00150
00151
00152 int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight,
00153 pf_vector_t *mean, pf_matrix_t *cov);
00154
00155
00156 void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples);
00157
00158
00159 void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig);
00160
00161
00162 void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig);
00163
00164
00165 void pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig);
00166
00167 #ifdef __cplusplus
00168 }
00169 #endif
00170
00171
00172 #endif