m5p3nc3r/agv_pro_ros2
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add humble-navigation2

Browse Source
This commit is contained in:
X-lanni
2025-05-27 19:03:40 +08:00
parent 974abb5e1e
commit e74ec539c2
1280 changed files with 204114 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
navigation2/nav2_amcl/src/pf/CMakeLists.txt
+25
View File
@@ -0,0 +1,25 @@
if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wno-gnu-folding-constant)
endif()
add_library(pf_lib SHARED
pf.c
pf_kdtree.c
pf_pdf.c
pf_vector.c
eig3.c
pf_draw.c
)
target_include_directories(pf_lib PRIVATE ../include)
if(HAVE_DRAND48)
target_compile_definitions(pf_lib PRIVATE "HAVE_DRAND48")
endif()
target_link_libraries(pf_lib m)
install(TARGETS
pf_lib
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
navigation2/nav2_amcl/src/pf/eig3.c
+282
View File
@@ -0,0 +1,282 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/* 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
#ifdef _MSC_VER
#define n 3
#else
static int n = 3;
#endif
// 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 = hypot(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 = hypot(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]; // NOLINT
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);
}
navigation2/nav2_amcl/src/pf/pf.c
+646
View File
@@ -0,0 +1,646 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Simple particle filter for localization.
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf.c 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#include <float.h>
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include "nav2_amcl/pf/pf.hpp"
#include "nav2_amcl/pf/pf_pdf.hpp"
#include "nav2_amcl/pf/pf_kdtree.hpp"
#include "nav2_amcl/portable_utils.hpp"
// Compute the required number of samples, given that there are k bins
// with samples in them.
static int pf_resample_limit(pf_t * pf, int k);
// Create a new filter
pf_t * pf_alloc(
int min_samples, int max_samples,
double alpha_slow, double alpha_fast,
pf_init_model_fn_t random_pose_fn)
{
int i, j;
pf_t * pf;
pf_sample_set_t * set;
pf_sample_t * sample;
srand48(time(NULL));
pf = calloc(1, sizeof(pf_t));
pf->random_pose_fn = random_pose_fn;
pf->min_samples = min_samples;
pf->max_samples = max_samples;
// Control parameters for the population size calculation. [err] is
// the max error between the true distribution and the estimated
// distribution. [z] is the upper standard normal quantile for (1 -
// p), where p is the probability that the error on the estimated
// distrubition will be less than [err].
pf->pop_err = 0.01;
pf->pop_z = 3;
pf->dist_threshold = 0.5;
pf->current_set = 0;
for (j = 0; j < 2; j++) {
set = pf->sets + j;
set->sample_count = max_samples;
set->samples = calloc(max_samples, sizeof(pf_sample_t));
for (i = 0; i < set->sample_count; i++) {
sample = set->samples + i;
sample->pose.v[0] = 0.0;
sample->pose.v[1] = 0.0;
sample->pose.v[2] = 0.0;
sample->weight = 1.0 / max_samples;
}
// HACK: is 3 times max_samples enough?
set->kdtree = pf_kdtree_alloc(3 * max_samples);
set->cluster_count = 0;
set->cluster_max_count = max_samples;
set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));
set->mean = pf_vector_zero();
set->cov = pf_matrix_zero();
}
pf->w_slow = 0.0;
pf->w_fast = 0.0;
pf->alpha_slow = alpha_slow;
pf->alpha_fast = alpha_fast;
// set converged to 0
pf_init_converged(pf);
return pf;
}
// Free an existing filter
void pf_free(pf_t * pf)
{
int i;
for (i = 0; i < 2; i++) {
free(pf->sets[i].clusters);
pf_kdtree_free(pf->sets[i].kdtree);
free(pf->sets[i].samples);
}
free(pf);
}
// Initialize the filter using a guassian
void pf_init(pf_t * pf, pf_vector_t mean, pf_matrix_t cov)
{
int i;
pf_sample_set_t * set;
pf_sample_t * sample;
pf_pdf_gaussian_t * pdf;
set = pf->sets + pf->current_set;
// Create the kd tree for adaptive sampling
pf_kdtree_clear(set->kdtree);
set->sample_count = pf->max_samples;
pdf = pf_pdf_gaussian_alloc(mean, cov);
// Compute the new sample poses
for (i = 0; i < set->sample_count; i++) {
sample = set->samples + i;
sample->weight = 1.0 / pf->max_samples;
sample->pose = pf_pdf_gaussian_sample(pdf);
// Add sample to histogram
pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
}
pf->w_slow = pf->w_fast = 0.0;
pf_pdf_gaussian_free(pdf);
// Re-compute cluster statistics
pf_cluster_stats(pf, set);
// set converged to 0
pf_init_converged(pf);
}
// Initialize the filter using some model
void pf_init_model(pf_t * pf, pf_init_model_fn_t init_fn, void * init_data)
{
int i;
pf_sample_set_t * set;
pf_sample_t * sample;
set = pf->sets + pf->current_set;
// Create the kd tree for adaptive sampling
pf_kdtree_clear(set->kdtree);
set->sample_count = pf->max_samples;
// Compute the new sample poses
for (i = 0; i < set->sample_count; i++) {
sample = set->samples + i;
sample->weight = 1.0 / pf->max_samples;
sample->pose = (*init_fn)(init_data);
// Add sample to histogram
pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
}
pf->w_slow = pf->w_fast = 0.0;
// Re-compute cluster statistics
pf_cluster_stats(pf, set);
// set converged to 0
pf_init_converged(pf);
}
void pf_init_converged(pf_t * pf)
{
pf_sample_set_t * set;
set = pf->sets + pf->current_set;
set->converged = 0;
pf->converged = 0;
}
int pf_update_converged(pf_t * pf)
{
int i;
pf_sample_set_t * set;
pf_sample_t * sample;
set = pf->sets + pf->current_set;
double mean_x = 0, mean_y = 0;
for (i = 0; i < set->sample_count; i++) {
sample = set->samples + i;
mean_x += sample->pose.v[0];
mean_y += sample->pose.v[1];
}
mean_x /= set->sample_count;
mean_y /= set->sample_count;
for (i = 0; i < set->sample_count; i++) {
sample = set->samples + i;
if (fabs(sample->pose.v[0] - mean_x) > pf->dist_threshold ||
fabs(sample->pose.v[1] - mean_y) > pf->dist_threshold)
{
set->converged = 0;
pf->converged = 0;
return 0;
}
}
set->converged = 1;
pf->converged = 1;
return 1;
}
// Update the filter with some new action
// void pf_update_action(pf_t * pf, pf_action_model_fn_t action_fn, void * action_data)
// {
// pf_sample_set_t * set;
// set = pf->sets + pf->current_set;
// (*action_fn)(action_data, set);
// }
// Update the filter with some new sensor observation
void pf_update_sensor(pf_t * pf, pf_sensor_model_fn_t sensor_fn, void * sensor_data)
{
int i;
pf_sample_set_t * set;
pf_sample_t * sample;
double total;
set = pf->sets + pf->current_set;
// Compute the sample weights
total = (*sensor_fn)(sensor_data, set);
if (total > 0.0) {
// Normalize weights
double w_avg = 0.0;
for (i = 0; i < set->sample_count; i++) {
sample = set->samples + i;
w_avg += sample->weight;
sample->weight /= total;
}
// Update running averages of likelihood of samples (Prob Rob p258)
w_avg /= set->sample_count;
if (pf->w_slow == 0.0) {
pf->w_slow = w_avg;
} else {
pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
}
if (pf->w_fast == 0.0) {
pf->w_fast = w_avg;
} else {
pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
}
} else {
// Handle zero total
for (i = 0; i < set->sample_count; i++) {
sample = set->samples + i;
sample->weight = 1.0 / set->sample_count;
}
}
}
// Resample the distribution
void pf_update_resample(pf_t * pf, void * random_pose_data)
{
int i;
double total;
pf_sample_set_t * set_a, * set_b;
pf_sample_t * sample_a, * sample_b;
// double r,c,U;
// int m;
// double count_inv;
double * c;
double w_diff;
set_a = pf->sets + pf->current_set;
set_b = pf->sets + (pf->current_set + 1) % 2;
// Build up cumulative probability table for resampling.
// TODO(?): Replace this with a more efficient procedure
// (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html)
c = (double *)malloc(sizeof(double) * (set_a->sample_count + 1));
c[0] = 0.0;
for (i = 0; i < set_a->sample_count; i++) {
c[i + 1] = c[i] + set_a->samples[i].weight;
}
// 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;
w_diff = 1.0 - pf->w_fast / pf->w_slow;
if (w_diff < 0.0) {
w_diff = 0.0;
}
// printf("w_diff: %9.6f\n", w_diff);
// Can't (easily) combine low-variance sampler with KLD adaptive
// sampling, so we'll take the more traditional route.
/*
// 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) {
sample_b = set_b->samples + set_b->sample_count++;
if (drand48() < w_diff) {
sample_b->pose = (pf->random_pose_fn)(random_pose_data);
} else {
// Can't (easily) combine low-variance sampler with KLD adaptive
// sampling, so we'll take the more traditional route.
/*
// Low-variance resampler, taken from Probabilistic Robotics, p110
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;
}
m++;
*/
// Naive discrete event sampler
double r;
r = drand48();
for (i = 0; i < set_a->sample_count; i++) {
if ((c[i] <= r) && (r < c[i + 1])) {
break;
}
}
assert(i < set_a->sample_count);
sample_a = set_a->samples + i;
assert(sample_a->weight > 0);
// Add sample to list
sample_b->pose = sample_a->pose;
}
sample_b->weight = 1.0;
total += sample_b->weight;
// Add sample to histogram
pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
// See if we have enough samples yet
if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count)) {
break;
}
}
// Reset averages, to avoid spiraling off into complete randomness.
if (w_diff > 0.0) {
pf->w_slow = pf->w_fast = 0.0;
}
// fprintf(stderr, "\n\n");
// Normalize weights
for (i = 0; i < set_b->sample_count; i++) {
sample_b = set_b->samples + i;
sample_b->weight /= total;
}
// Re-compute cluster statistics
pf_cluster_stats(pf, set_b);
// Use the newly created sample set
pf->current_set = (pf->current_set + 1) % 2;
pf_update_converged(pf);
free(c);
}
// Compute the required number of samples, given that there are k bins
// with samples in them. This is taken directly from Fox et al.
int pf_resample_limit(pf_t * pf, int k)
{
double a, b, c, x;
int n;
if (k <= 1) {
return pf->max_samples;
}
a = 1;
b = 2 / (9 * ((double) k - 1));
c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z;
x = a - b + c;
n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x);
if (n < pf->min_samples) {
return pf->min_samples;
}
if (n > pf->max_samples) {
return pf->max_samples;
}
return n;
}
// Re-compute the cluster statistics for a sample set
void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set)
{
(void)pf;
int i, j, k, cidx;
pf_sample_t * sample;
pf_cluster_t * cluster;
// Workspace
double m[4], c[2][2];
double weight;
// Cluster the samples
pf_kdtree_cluster(set->kdtree);
// Initialize cluster stats
set->cluster_count = 0;
for (i = 0; i < set->cluster_max_count; i++) {
cluster = set->clusters + i;
cluster->weight = 0;
cluster->mean = pf_vector_zero();
cluster->cov = pf_matrix_zero();
for (j = 0; j < 4; j++) {
cluster->m[j] = 0.0;
}
for (j = 0; j < 2; j++) {
for (k = 0; k < 2; k++) {
cluster->c[j][k] = 0.0;
}
}
}
// Initialize overall filter stats
weight = 0.0;
set->mean = pf_vector_zero();
set->cov = pf_matrix_zero();
for (j = 0; j < 4; j++) {
m[j] = 0.0;
}
for (j = 0; j < 2; j++) {
for (k = 0; k < 2; k++) {
c[j][k] = 0.0;
}
}
// Compute cluster stats
for (i = 0; i < set->sample_count; i++) {
sample = set->samples + i;
// printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]);
// Get the cluster label for this sample
cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose);
assert(cidx >= 0);
if (cidx >= set->cluster_max_count) {
continue;
}
if (cidx + 1 > set->cluster_count) {
set->cluster_count = cidx + 1;
}
cluster = set->clusters + cidx;
cluster->weight += sample->weight;
weight += sample->weight;
// Compute mean
cluster->m[0] += sample->weight * sample->pose.v[0];
cluster->m[1] += sample->weight * sample->pose.v[1];
cluster->m[2] += sample->weight * cos(sample->pose.v[2]);
cluster->m[3] += sample->weight * sin(sample->pose.v[2]);
m[0] += sample->weight * sample->pose.v[0];
m[1] += sample->weight * sample->pose.v[1];
m[2] += sample->weight * cos(sample->pose.v[2]);
m[3] += sample->weight * sin(sample->pose.v[2]);
// Compute covariance in linear components
for (j = 0; j < 2; j++) {
for (k = 0; k < 2; k++) {
cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
}
}
}
// Normalize
for (i = 0; i < set->cluster_count; i++) {
cluster = set->clusters + i;
cluster->mean.v[0] = cluster->m[0] / cluster->weight;
cluster->mean.v[1] = cluster->m[1] / cluster->weight;
cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]);
cluster->cov = pf_matrix_zero();
// Covariance in linear components
for (j = 0; j < 2; j++) {
for (k = 0; k < 2; k++) {
cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
cluster->mean.v[j] * cluster->mean.v[k];
}
}
// Covariance in angular components; I think this is the correct
// formula for circular statistics.
cluster->cov.m[2][2] = -2 * log(
sqrt(
cluster->m[2] * cluster->m[2] +
cluster->m[3] * cluster->m[3]));
// printf("cluster %d %d %f (%f %f %f)\n", i, cluster->count, cluster->weight,
// cluster->mean.v[0], cluster->mean.v[1], cluster->mean.v[2]);
// pf_matrix_fprintf(cluster->cov, stdout, "%e");
}
// Compute overall filter stats
set->mean.v[0] = m[0] / weight;
set->mean.v[1] = m[1] / weight;
set->mean.v[2] = atan2(m[3], m[2]);
// Covariance in linear components
for (j = 0; j < 2; j++) {
for (k = 0; k < 2; k++) {
set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];
}
}
// Covariance in angular components; I think this is the correct
// formula for circular statistics.
set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));
}
// Compute the CEP statistics (mean and variance).
// void pf_get_cep_stats(pf_t * pf, pf_vector_t * mean, double * var)
// {
// int i;
// double mn, mx, my, mrr;
// pf_sample_set_t * set;
// pf_sample_t * sample;
// set = pf->sets + pf->current_set;
// mn = 0.0;
// mx = 0.0;
// my = 0.0;
// mrr = 0.0;
// for (i = 0; i < set->sample_count; i++) {
// sample = set->samples + i;
// mn += sample->weight;
// mx += sample->weight * sample->pose.v[0];
// my += sample->weight * sample->pose.v[1];
// mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0];
// mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1];
// }
// mean->v[0] = mx / mn;
// mean->v[1] = my / mn;
// mean->v[2] = 0.0;
// *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn));
// }
// Get the statistics for a particular cluster.
int pf_get_cluster_stats(
pf_t * pf, int clabel, double * weight,
pf_vector_t * mean, pf_matrix_t * cov)
{
pf_sample_set_t * set;
pf_cluster_t * cluster;
set = pf->sets + pf->current_set;
if (clabel >= set->cluster_count) {
return 0;
}
cluster = set->clusters + clabel;
*weight = cluster->weight;
*mean = cluster->mean;
*cov = cluster->cov;
return 1;
}
navigation2/nav2_amcl/src/pf/pf_draw.c
+150
View File
@@ -0,0 +1,150 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Particle filter; drawing routines
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
*************************************************************************/
#pragma GCC diagnostic ignored "-Wpedantic"
#ifdef INCLUDE_RTKGUI
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <rtk.h>
#include "nav2_amcl/pf/pf.hpp"
#include "nav2_amcl/pf/pf_pdf.hpp"
#include "nav2_amcl/pf/pf_kdtree.hpp"
// Draw the statistics
void pf_draw_statistics(pf_t * pf, rtk_fig_t * fig);
// Draw the sample set
void pf_draw_samples(pf_t * pf, rtk_fig_t * fig, int max_samples)
{
int i;
double px, py, pa;
pf_sample_set_t * set;
pf_sample_t * sample;
set = pf->sets + pf->current_set;
max_samples = MIN(max_samples, set->sample_count);
for (i = 0; i < max_samples; i++) {
sample = set->samples + i;
px = sample->pose.v[0];
py = sample->pose.v[1];
pa = sample->pose.v[2];
// printf("%f %f\n", px, py);
rtk_fig_point(fig, px, py);
rtk_fig_arrow(fig, px, py, pa, 0.1, 0.02);
// rtk_fig_rectangle(fig, px, py, 0, 0.1, 0.1, 0);
}
}
// Draw the hitogram (kd tree)
void pf_draw_hist(pf_t * pf, rtk_fig_t * fig)
{
pf_sample_set_t * set;
set = pf->sets + pf->current_set;
rtk_fig_color(fig, 0.0, 0.0, 1.0);
pf_kdtree_draw(set->kdtree, fig);
}
// Draw the CEP statistics
// void pf_draw_cep_stats(pf_t * pf, rtk_fig_t * fig)
// {
// pf_vector_t mean;
// double var;
// pf_get_cep_stats(pf, &mean, &var);
// var = sqrt(var);
// rtk_fig_color(fig, 0, 0, 1);
// rtk_fig_ellipse(fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0);
// }
// Draw the cluster statistics
void pf_draw_cluster_stats(pf_t * pf, rtk_fig_t * fig)
{
int i;
pf_cluster_t * cluster;
pf_sample_set_t * set;
pf_vector_t mean;
pf_matrix_t cov;
pf_matrix_t r, d;
double weight, o, d1, d2;
set = pf->sets + pf->current_set;
for (i = 0; i < set->cluster_count; i++) {
cluster = set->clusters + i;
weight = cluster->weight;
mean = cluster->mean;
cov = cluster->cov;
// Compute unitary representation S = R D R^T
pf_matrix_unitary(&r, &d, cov);
/* Debugging
printf("mean = \n");
pf_vector_fprintf(mean, stdout, "%e");
printf("cov = \n");
pf_matrix_fprintf(cov, stdout, "%e");
printf("r = \n");
pf_matrix_fprintf(r, stdout, "%e");
printf("d = \n");
pf_matrix_fprintf(d, stdout, "%e");
*/
// Compute the orientation of the error ellipse (first eigenvector)
o = atan2(r.m[1][0], r.m[0][0]);
d1 = 6 * sqrt(d.m[0][0]);
d2 = 6 * sqrt(d.m[1][1]);
if (d1 > 1e-3 && d2 > 1e-3) {
// Draw the error ellipse
rtk_fig_ellipse(fig, mean.v[0], mean.v[1], o, d1, d2, 0);
rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o, d1);
rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o + M_PI / 2, d2);
}
// Draw a direction indicator
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2], 0.50, 0.10);
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] + 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] - 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
}
}
#endif
navigation2/nav2_amcl/src/pf/pf_kdtree.c
+462
View File
@@ -0,0 +1,462 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: kd-tree functions
* Author: Andrew Howard
* Date: 18 Dec 2002
* CVS: $Id: pf_kdtree.c 7057 2008-10-02 00:44:06Z gbiggs $
*************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "nav2_amcl/pf/pf_vector.hpp"
#include "nav2_amcl/pf/pf_kdtree.hpp"
// Compare keys to see if they are equal
static int pf_kdtree_equal(pf_kdtree_t * self, int key_a[], int key_b[]);
// Insert a node into the tree
static pf_kdtree_node_t * pf_kdtree_insert_node(
pf_kdtree_t * self, pf_kdtree_node_t * parent,
pf_kdtree_node_t * node, int key[], double value);
// Recursive node search
static pf_kdtree_node_t * pf_kdtree_find_node(
pf_kdtree_t * self, pf_kdtree_node_t * node,
int key[]);
// Recursively label nodes in this cluster
static void pf_kdtree_cluster_node(pf_kdtree_t * self, pf_kdtree_node_t * node, int depth);
// Recursive node printing
// static void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node);
#ifdef INCLUDE_RTKGUI
// Recursively draw nodes
static void pf_kdtree_draw_node(pf_kdtree_t * self, pf_kdtree_node_t * node, rtk_fig_t * fig);
#endif
////////////////////////////////////////////////////////////////////////////////
// Create a tree
pf_kdtree_t * pf_kdtree_alloc(int max_size)
{
pf_kdtree_t * self;
self = calloc(1, sizeof(pf_kdtree_t));
self->size[0] = 0.50;
self->size[1] = 0.50;
self->size[2] = (10 * M_PI / 180);
self->root = NULL;
self->node_count = 0;
self->node_max_count = max_size;
self->nodes = calloc(self->node_max_count, sizeof(pf_kdtree_node_t));
self->leaf_count = 0;
return self;
}
////////////////////////////////////////////////////////////////////////////////
// Destroy a tree
void pf_kdtree_free(pf_kdtree_t * self)
{
free(self->nodes);
free(self);
}
////////////////////////////////////////////////////////////////////////////////
// Clear all entries from the tree
void pf_kdtree_clear(pf_kdtree_t * self)
{
self->root = NULL;
self->leaf_count = 0;
self->node_count = 0;
}
////////////////////////////////////////////////////////////////////////////////
// Insert a pose into the tree.
void pf_kdtree_insert(pf_kdtree_t * self, pf_vector_t pose, double value)
{
int key[3];
key[0] = floor(pose.v[0] / self->size[0]);
key[1] = floor(pose.v[1] / self->size[1]);
key[2] = floor(pose.v[2] / self->size[2]);
self->root = pf_kdtree_insert_node(self, NULL, self->root, key, value);
// Test code
/*
printf("find %d %d %d\n", key[0], key[1], key[2]);
assert(pf_kdtree_find_node(self, self->root, key) != NULL);
pf_kdtree_print_node(self, self->root);
printf("\n");
for (i = 0; i < self->node_count; i++)
{
node = self->nodes + i;
if (node->leaf)
{
printf("find %d %d %d\n", node->key[0], node->key[1], node->key[2]);
assert(pf_kdtree_find_node(self, self->root, node->key) == node);
}
}
printf("\n\n");
*/
}
////////////////////////////////////////////////////////////////////////////////
// Determine the probability estimate for the given pose. TODO: this
// should do a kernel density estimate rather than a simple histogram.
// double pf_kdtree_get_prob(pf_kdtree_t * self, pf_vector_t pose)
// {
// int key[3];
// pf_kdtree_node_t * node;
// key[0] = floor(pose.v[0] / self->size[0]);
// key[1] = floor(pose.v[1] / self->size[1]);
// key[2] = floor(pose.v[2] / self->size[2]);
// node = pf_kdtree_find_node(self, self->root, key);
// if (node == NULL) {
// return 0.0;
// }
// return node->value;
// }
////////////////////////////////////////////////////////////////////////////////
// Determine the cluster label for the given pose
int pf_kdtree_get_cluster(pf_kdtree_t * self, pf_vector_t pose)
{
int key[3];
pf_kdtree_node_t * node;
key[0] = floor(pose.v[0] / self->size[0]);
key[1] = floor(pose.v[1] / self->size[1]);
key[2] = floor(pose.v[2] / self->size[2]);
node = pf_kdtree_find_node(self, self->root, key);
if (node == NULL) {
return -1;
}
return node->cluster;
}
////////////////////////////////////////////////////////////////////////////////
// Compare keys to see if they are equal
int pf_kdtree_equal(pf_kdtree_t * self, int key_a[], int key_b[])
{
(void)self;
// double a, b;
if (key_a[0] != key_b[0]) {
return 0;
}
if (key_a[1] != key_b[1]) {
return 0;
}
if (key_a[2] != key_b[2]) {
return 0;
}
/* TODO: make this work (pivot selection needs fixing, too)
// Normalize angles
a = key_a[2] * self->size[2];
a = atan2(sin(a), cos(a)) / self->size[2];
b = key_b[2] * self->size[2];
b = atan2(sin(b), cos(b)) / self->size[2];
if ((int) a != (int) b)
return 0;
*/
return 1;
}
////////////////////////////////////////////////////////////////////////////////
// Insert a node into the tree
pf_kdtree_node_t * pf_kdtree_insert_node(
pf_kdtree_t * self, pf_kdtree_node_t * parent,
pf_kdtree_node_t * node, int key[], double value)
{
int i;
int split, max_split;
// If the node doesnt exist yet...
if (node == NULL) {
assert(self->node_count < self->node_max_count);
node = self->nodes + self->node_count++;
memset(node, 0, sizeof(pf_kdtree_node_t));
node->leaf = 1;
if (parent == NULL) {
node->depth = 0;
} else {
node->depth = parent->depth + 1;
}
for (i = 0; i < 3; i++) {
node->key[i] = key[i];
}
node->value = value;
self->leaf_count += 1;
} else if (node->leaf) { // If the node exists, and it is a leaf node...
// If the keys are equal, increment the value
if (pf_kdtree_equal(self, key, node->key)) {
node->value += value;
} else { // The keys are not equal, so split this node
// Find the dimension with the largest variance and do a mean
// split
max_split = 0;
node->pivot_dim = -1;
for (i = 0; i < 3; i++) {
split = abs(key[i] - node->key[i]);
if (split > max_split) {
max_split = split;
node->pivot_dim = i;
}
}
assert(node->pivot_dim >= 0);
node->pivot_value = (key[node->pivot_dim] + node->key[node->pivot_dim]) / 2.0;
if (key[node->pivot_dim] < node->pivot_value) {
node->children[0] = pf_kdtree_insert_node(self, node, NULL, key, value);
node->children[1] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value);
} else {
node->children[0] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value);
node->children[1] = pf_kdtree_insert_node(self, node, NULL, key, value);
}
node->leaf = 0;
self->leaf_count -= 1;
}
} else { // If the node exists, and it has children...
assert(node->children[0] != NULL);
assert(node->children[1] != NULL);
if (key[node->pivot_dim] < node->pivot_value) {
pf_kdtree_insert_node(self, node, node->children[0], key, value);
} else {
pf_kdtree_insert_node(self, node, node->children[1], key, value);
}
}
return node;
}
////////////////////////////////////////////////////////////////////////////////
// Recursive node search
pf_kdtree_node_t * pf_kdtree_find_node(pf_kdtree_t * self, pf_kdtree_node_t * node, int key[])
{
if (node->leaf) {
// printf("find : leaf %p %d %d %d\n", node, node->key[0], node->key[1], node->key[2]);
// If the keys are the same...
if (pf_kdtree_equal(self, key, node->key)) {
return node;
} else {
return NULL;
}
} else {
// printf("find : brch %p %d %f\n", node, node->pivot_dim, node->pivot_value);
assert(node->children[0] != NULL);
assert(node->children[1] != NULL);
// If the keys are different...
if (key[node->pivot_dim] < node->pivot_value) {
return pf_kdtree_find_node(self, node->children[0], key);
} else {
return pf_kdtree_find_node(self, node->children[1], key);
}
}
return NULL;
}
////////////////////////////////////////////////////////////////////////////////
// Recursive node printing
/*
void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node)
{
if (node->leaf)
{
printf("(%+02d %+02d %+02d)\n", node->key[0], node->key[1], node->key[2]);
printf("%*s", node->depth * 11, "");
}
else
{
printf("(%+02d %+02d %+02d) ", node->key[0], node->key[1], node->key[2]);
pf_kdtree_print_node(self, node->children[0]);
pf_kdtree_print_node(self, node->children[1]);
}
return;
}
*/
////////////////////////////////////////////////////////////////////////////////
// Cluster the leaves in the tree
void pf_kdtree_cluster(pf_kdtree_t * self)
{
int i;
int queue_count, cluster_count;
pf_kdtree_node_t ** queue, * node;
queue_count = 0;
queue = calloc(self->node_count, sizeof(queue[0]));
// Put all the leaves in a queue
for (i = 0; i < self->node_count; i++) {
node = self->nodes + i;
if (node->leaf) {
node->cluster = -1;
assert(queue_count < self->node_count);
queue[queue_count++] = node;
// TESTING; remove
assert(node == pf_kdtree_find_node(self, self->root, node->key));
}
}
cluster_count = 0;
// Do connected components for each node
while (queue_count > 0) {
node = queue[--queue_count];
// If this node has already been labelled, skip it
if (node->cluster >= 0) {
continue;
}
// Assign a label to this cluster
node->cluster = cluster_count++;
// Recursively label nodes in this cluster
pf_kdtree_cluster_node(self, node, 0);
}
free(queue);
}
////////////////////////////////////////////////////////////////////////////////
// Recursively label nodes in this cluster
void pf_kdtree_cluster_node(pf_kdtree_t * self, pf_kdtree_node_t * node, int depth)
{
int i;
int nkey[3];
pf_kdtree_node_t * nnode;
for (i = 0; i < 3 * 3 * 3; i++) {
nkey[0] = node->key[0] + (i / 9) - 1;
nkey[1] = node->key[1] + ((i % 9) / 3) - 1;
nkey[2] = node->key[2] + ((i % 9) % 3) - 1;
nnode = pf_kdtree_find_node(self, self->root, nkey);
if (nnode == NULL) {
continue;
}
assert(nnode->leaf);
// This node already has a label; skip it. The label should be
// consistent, however.
if (nnode->cluster >= 0) {
assert(nnode->cluster == node->cluster);
continue;
}
// Label this node and recurse
nnode->cluster = node->cluster;
pf_kdtree_cluster_node(self, nnode, depth + 1);
}
}
#ifdef INCLUDE_RTKGUI
////////////////////////////////////////////////////////////////////////////////
// Draw the tree
void pf_kdtree_draw(pf_kdtree_t * self, rtk_fig_t * fig)
{
if (self->root != NULL) {
pf_kdtree_draw_node(self, self->root, fig);
}
}
////////////////////////////////////////////////////////////////////////////////
// Recursively draw nodes
void pf_kdtree_draw_node(pf_kdtree_t * self, pf_kdtree_node_t * node, rtk_fig_t * fig)
{
double ox, oy;
char text[64];
if (node->leaf) {
ox = (node->key[0] + 0.5) * self->size[0];
oy = (node->key[1] + 0.5) * self->size[1];
rtk_fig_rectangle(fig, ox, oy, 0.0, self->size[0], self->size[1], 0);
// snprintf(text, sizeof(text), "%0.3f", node->value);
// rtk_fig_text(fig, ox, oy, 0.0, text);
snprintf(text, sizeof(text), "%d", node->cluster);
rtk_fig_text(fig, ox, oy, 0.0, text);
} else {
assert(node->children[0] != NULL);
assert(node->children[1] != NULL);
pf_kdtree_draw_node(self, node->children[0], fig);
pf_kdtree_draw_node(self, node->children[1], fig);
}
}
#endif
navigation2/nav2_amcl/src/pf/pf_pdf.c
+149
View File
@@ -0,0 +1,149 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Useful pdf functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_pdf.c 6348 2008-04-17 02:53:17Z gerkey $
*************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
// #include <gsl/gsl_rng.h>
// #include <gsl/gsl_randist.h>
#include "nav2_amcl/pf/pf_pdf.hpp"
#include "nav2_amcl/portable_utils.hpp"
// Random number generator seed value
static unsigned int pf_pdf_seed;
/**************************************************************************
* Gaussian
*************************************************************************/
// Create a gaussian pdf
pf_pdf_gaussian_t * pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
{
pf_matrix_t cd;
pf_pdf_gaussian_t * pdf;
pdf = calloc(1, sizeof(pf_pdf_gaussian_t));
pdf->x = x;
pdf->cx = cx;
// pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);
// Decompose the convariance matrix into a rotation
// matrix and a diagonal matrix.
pf_matrix_unitary(&pdf->cr, &cd, pdf->cx);
pdf->cd.v[0] = sqrt(cd.m[0][0]);
pdf->cd.v[1] = sqrt(cd.m[1][1]);
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);
srand48(++pf_pdf_seed);
return pdf;
}
// Destroy the pdf
void pf_pdf_gaussian_free(pf_pdf_gaussian_t * pdf)
{
// gsl_rng_free(pdf->rng);
free(pdf);
}
/*
// Compute the value of the pdf at some point [x].
double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x)
{
int i, j;
pf_vector_t z;
double zz, p;
z = pf_vector_sub(x, pdf->x);
zz = 0;
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j];
p = 1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2);
return p;
}
*/
// Generate a sample from the pdf.
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t * pdf)
{
int i, j;
pf_vector_t r;
pf_vector_t x;
// 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] = pf_ran_gaussian(pdf->cd.v[i]);
}
for (i = 0; i < 3; i++) {
x.v[i] = pdf->x.v[i];
for (j = 0; j < 3; j++) {
x.v[i] += pdf->cr.m[i][j] * r.v[j];
}
}
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);
}
navigation2/nav2_amcl/src/pf/pf_vector.c
+270
View File
@@ -0,0 +1,270 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Vector functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_vector.c 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#include <math.h>
// #include <gsl/gsl_matrix.h>
// #include <gsl/gsl_eigen.h>
// #include <gsl/gsl_linalg.h>
#include "nav2_amcl/pf/pf_vector.hpp"
#include "nav2_amcl/pf/eig3.hpp"
// Return a zero vector
pf_vector_t pf_vector_zero(void)
{
pf_vector_t c;
c.v[0] = 0.0;
c.v[1] = 0.0;
c.v[2] = 0.0;
return c;
}
// // Check for NAN or INF in any component
// int pf_vector_finite(pf_vector_t a)
// {
// int i;
// for (i = 0; i < 3; i++) {
// if (!isfinite(a.v[i])) {
// return 0;
// }
// }
// return 1;
// }
// Print a vector
// void pf_vector_fprintf(pf_vector_t a, FILE * file, const char * fmt)
// {
// int i;
// for (i = 0; i < 3; i++) {
// fprintf(file, fmt, a.v[i]);
// fprintf(file, " ");
// }
// fprintf(file, "\n");
// }
// // Simple vector addition
// pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b)
// {
// pf_vector_t c;
// c.v[0] = a.v[0] + b.v[0];
// c.v[1] = a.v[1] + b.v[1];
// c.v[2] = a.v[2] + b.v[2];
// return c;
// }
// Simple vector subtraction
pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b)
{
pf_vector_t c;
c.v[0] = a.v[0] - b.v[0];
c.v[1] = a.v[1] - b.v[1];
c.v[2] = a.v[2] - b.v[2];
return c;
}
// Transform from local to global coords (a + b)
pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
{
pf_vector_t c;
c.v[0] = b.v[0] + a.v[0] * cos(b.v[2]) - a.v[1] * sin(b.v[2]);
c.v[1] = b.v[1] + a.v[0] * sin(b.v[2]) + a.v[1] * cos(b.v[2]);
c.v[2] = b.v[2] + a.v[2];
c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
return c;
}
// // Transform from global to local coords (a - b)
// pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b)
// {
// pf_vector_t c;
// c.v[0] = +(a.v[0] - b.v[0]) * cos(b.v[2]) + (a.v[1] - b.v[1]) * sin(b.v[2]);
// c.v[1] = -(a.v[0] - b.v[0]) * sin(b.v[2]) + (a.v[1] - b.v[1]) * cos(b.v[2]);
// c.v[2] = a.v[2] - b.v[2];
// c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
// return c;
// }
// Return a zero matrix
pf_matrix_t pf_matrix_zero(void)
{
int i, j;
pf_matrix_t c;
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
c.m[i][j] = 0.0;
}
}
return c;
}
// // Check for NAN or INF in any component
// int pf_matrix_finite(pf_matrix_t a)
// {
// int i, j;
// for (i = 0; i < 3; i++) {
// for (j = 0; j < 3; j++) {
// if (!isfinite(a.m[i][j])) {
// return 0;
// }
// }
// }
// return 1;
// }
// Print a matrix
// void pf_matrix_fprintf(pf_matrix_t a, FILE * file, const char * fmt)
// {
// int i, j;
// for (i = 0; i < 3; i++) {
// for (j = 0; j < 3; j++) {
// fprintf(file, fmt, a.m[i][j]);
// fprintf(file, " ");
// }
// fprintf(file, "\n");
// }
// }
/*
// Compute the matrix inverse
pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det)
{
double lndet;
int signum;
gsl_permutation *p;
gsl_matrix_view A, Ai;
pf_matrix_t ai;
A = gsl_matrix_view_array((double*) a.m, 3, 3);
Ai = gsl_matrix_view_array((double*) ai.m, 3, 3);
// Do LU decomposition
p = gsl_permutation_alloc(3);
gsl_linalg_LU_decomp(&A.matrix, p, &signum);
// Check for underflow
lndet = gsl_linalg_LU_lndet(&A.matrix);
if (lndet < -1000)
{
//printf("underflow in matrix inverse lndet = %f", lndet);
gsl_matrix_set_zero(&Ai.matrix);
}
else
{
// Compute inverse
gsl_linalg_LU_invert(&A.matrix, p, &Ai.matrix);
}
gsl_permutation_free(p);
if (det)
*det = exp(lndet);
return ai;
}
*/
// 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)
{
int i, j;
/*
gsl_matrix *aa;
gsl_vector *eval;
gsl_matrix *evec;
gsl_eigen_symmv_workspace *w;
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]);
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] = eval[i];
for (j = 0; j < 3; 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);
}