HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1409
Localizations/Libraries/Ros/amcl/src/amcl.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

84
Localizations/Libraries/Ros/amcl/src/amcl/map/map.c Normal file
View File

@@ -0,0 +1,84 @@
/*
* 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: Global map (grid-based)
* Author: Andrew Howard
* Date: 6 Feb 2003
* CVS: $Id: map.c 1713 2003-08-23 04:03:43Z inspectorg $
**************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include "amcl/map/map.h"
// Create a new map
map_t *map_alloc(void)
{
map_t *map;
map = (map_t*) malloc(sizeof(map_t));
// Assume we start at (0, 0)
map->origin_x = 0;
map->origin_y = 0;
// Make the size odd
map->size_x = 0;
map->size_y = 0;
map->scale = 0;
// Allocate storage for main map
map->cells = (map_cell_t*) NULL;
return map;
}
// Destroy a map
void map_free(map_t *map)
{
free(map->cells);
free(map);
return;
}
// Get the cell at the given point
map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa)
{
int i, j;
map_cell_t *cell;
i = MAP_GXWX(map, ox);
j = MAP_GYWY(map, oy);
if (!MAP_VALID(map, i, j))
return NULL;
cell = map->cells + MAP_INDEX(map, i, j);
return cell;
}

177
Localizations/Libraries/Ros/amcl/src/amcl/map/map_cspace.cpp Normal file
View File

@@ -0,0 +1,177 @@
/*
* 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
*
*/
#include <queue>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "amcl/map/map.h"
#include <ros/ros.h>
class CellData
{
public:
map_t* map_;
unsigned int i_, j_;
unsigned int src_i_, src_j_;
};
class CachedDistanceMap
{
public:
CachedDistanceMap(double scale, double max_dist) :
distances_(NULL), scale_(scale), max_dist_(max_dist)
{
cell_radius_ = max_dist / scale;
distances_ = new double *[cell_radius_+2];
for(int i=0; i<=cell_radius_+1; i++)
{
distances_[i] = new double[cell_radius_+2];
for(int j=0; j<=cell_radius_+1; j++)
{
distances_[i][j] = sqrt(i*i + j*j);
}
}
}
~CachedDistanceMap()
{
if(distances_)
{
for(int i=0; i<=cell_radius_+1; i++)
delete[] distances_[i];
delete[] distances_;
}
}
double** distances_;
double scale_;
double max_dist_;
int cell_radius_;
};
bool operator<(const CellData& a, const CellData& b)
{
return a.map_->cells[MAP_INDEX(a.map_, a.i_, a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
}
CachedDistanceMap*
get_distance_map(double scale, double max_dist)
{
static CachedDistanceMap* cdm = NULL;
if(!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist))
{
if(cdm)
delete cdm;
cdm = new CachedDistanceMap(scale, max_dist);
}
return cdm;
}
void enqueue(map_t* map, int i, int j,
int src_i, int src_j,
std::priority_queue<CellData>& Q,
CachedDistanceMap* cdm,
unsigned char* marked)
{
if(marked[MAP_INDEX(map, i, j)])
return;
int di = abs(i - src_i);
int dj = abs(j - src_j);
double distance = cdm->distances_[di][dj];
if(distance > cdm->cell_radius_)
return;
map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
CellData cell;
cell.map_ = map;
cell.i_ = i;
cell.j_ = j;
cell.src_i_ = src_i;
cell.src_j_ = src_j;
Q.push(cell);
marked[MAP_INDEX(map, i, j)] = 1;
}
// Update the cspace distance values
void map_update_cspace(map_t *map, double max_occ_dist)
{
unsigned char* marked;
std::priority_queue<CellData> Q;
marked = new unsigned char[map->size_x*map->size_y];
memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y);
map->max_occ_dist = max_occ_dist;
CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist);
// Enqueue all the obstacle cells
CellData cell;
cell.map_ = map;
for(int i=0; i<map->size_x; i++)
{
cell.src_i_ = cell.i_ = i;
for(int j=0; j<map->size_y; j++)
{
if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1)
{
map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
cell.src_j_ = cell.j_ = j;
marked[MAP_INDEX(map, i, j)] = 1;
Q.push(cell);
}
else
map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
}
}
while(!Q.empty())
{
CellData current_cell = Q.top();
if(current_cell.i_ > 0)
enqueue(map, current_cell.i_-1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
if(current_cell.j_ > 0)
enqueue(map, current_cell.i_, current_cell.j_-1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
if((int)current_cell.i_ < map->size_x - 1)
enqueue(map, current_cell.i_+1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
if((int)current_cell.j_ < map->size_y - 1)
enqueue(map, current_cell.i_, current_cell.j_+1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
Q.pop();
}
delete[] marked;
}

158
Localizations/Libraries/Ros/amcl/src/amcl/map/map_draw.c Normal file
View File

@@ -0,0 +1,158 @@
/*
* 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: Local map GUI functions
* Author: Andrew Howard
* Date: 18 Jan 2003
* CVS: $Id: map_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
**************************************************************************/
#ifdef INCLUDE_RTKGUI
#include <errno.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <rtk.h>
#include "amcl/map/map.h"
////////////////////////////////////////////////////////////////////////////
// Draw the occupancy map
void map_draw_occ(map_t *map, rtk_fig_t *fig)
{
int i, j;
int col;
map_cell_t *cell;
uint16_t *image;
uint16_t *pixel;
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
// Draw occupancy
for (j = 0; j < map->size_y; j++)
{
for (i = 0; i < map->size_x; i++)
{
cell = map->cells + MAP_INDEX(map, i, j);
pixel = image + (j * map->size_x + i);
col = 127 - 127 * cell->occ_state;
*pixel = RTK_RGB16(col, col, col);
}
}
// Draw the entire occupancy map as an image
rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
map->scale, map->size_x, map->size_y, 16, image, NULL);
free(image);
return;
}
////////////////////////////////////////////////////////////////////////////
// Draw the cspace map
void map_draw_cspace(map_t *map, rtk_fig_t *fig)
{
int i, j;
int col;
map_cell_t *cell;
uint16_t *image;
uint16_t *pixel;
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
// Draw occupancy
for (j = 0; j < map->size_y; j++)
{
for (i = 0; i < map->size_x; i++)
{
cell = map->cells + MAP_INDEX(map, i, j);
pixel = image + (j * map->size_x + i);
col = 255 * cell->occ_dist / map->max_occ_dist;
*pixel = RTK_RGB16(col, col, col);
}
}
// Draw the entire occupancy map as an image
rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
map->scale, map->size_x, map->size_y, 16, image, NULL);
free(image);
return;
}
////////////////////////////////////////////////////////////////////////////
// Draw a wifi map
void map_draw_wifi(map_t *map, rtk_fig_t *fig, int index)
{
int i, j;
int level, col;
map_cell_t *cell;
uint16_t *image, *mask;
uint16_t *ipix, *mpix;
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
mask = malloc(map->size_x * map->size_y * sizeof(mask[0]));
// Draw wifi levels
for (j = 0; j < map->size_y; j++)
{
for (i = 0; i < map->size_x; i++)
{
cell = map->cells + MAP_INDEX(map, i, j);
ipix = image + (j * map->size_x + i);
mpix = mask + (j * map->size_x + i);
level = cell->wifi_levels[index];
if (cell->occ_state == -1 && level != 0)
{
col = 255 * (100 + level) / 100;
*ipix = RTK_RGB16(col, col, col);
*mpix = 1;
}
else
{
*mpix = 0;
}
}
}
// Draw the entire occupancy map as an image
rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
map->scale, map->size_x, map->size_y, 16, image, mask);
free(mask);
free(image);
return;
}
#endif

120
Localizations/Libraries/Ros/amcl/src/amcl/map/map_range.c Normal file
View File

@@ -0,0 +1,120 @@
/*
* 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: Range routines
* Author: Andrew Howard
* Date: 18 Jan 2003
* CVS: $Id: map_range.c 1347 2003-05-05 06:24:33Z inspectorg $
**************************************************************************/
#include <assert.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include "amcl/map/map.h"
// Extract a single range reading from the map. Unknown cells and/or
// out-of-bound cells are treated as occupied, which makes it easy to
// use Stage bitmap files.
double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range)
{
// Bresenham raytracing
int x0,x1,y0,y1;
int x,y;
int xstep, ystep;
char steep;
int tmp;
int deltax, deltay, error, deltaerr;
x0 = MAP_GXWX(map,ox);
y0 = MAP_GYWY(map,oy);
x1 = MAP_GXWX(map,ox + max_range * cos(oa));
y1 = MAP_GYWY(map,oy + max_range * sin(oa));
if(abs(y1-y0) > abs(x1-x0))
steep = 1;
else
steep = 0;
if(steep)
{
tmp = x0;
x0 = y0;
y0 = tmp;
tmp = x1;
x1 = y1;
y1 = tmp;
}
deltax = abs(x1-x0);
deltay = abs(y1-y0);
error = 0;
deltaerr = deltay;
x = x0;
y = y0;
if(x0 < x1)
xstep = 1;
else
xstep = -1;
if(y0 < y1)
ystep = 1;
else
ystep = -1;
if(steep)
{
if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
}
else
{
if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
}
while(x != (x1 + xstep * 1))
{
x += xstep;
error += deltaerr;
if(2*error >= deltax)
{
y += ystep;
error -= deltax;
}
if(steep)
{
if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
}
else
{
if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
}
}
return max_range;
}

215
Localizations/Libraries/Ros/amcl/src/amcl/map/map_store.c Normal file
View File

@@ -0,0 +1,215 @@
/*
* 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: Global map storage functions
* Author: Andrew Howard
* Date: 6 Feb 2003
* CVS: $Id: map_store.c 2951 2005-08-19 00:48:20Z gerkey $
**************************************************************************/
#include <errno.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "amcl/map/map.h"
////////////////////////////////////////////////////////////////////////////
// Load an occupancy grid
int map_load_occ(map_t *map, const char *filename, double scale, int negate)
{
FILE *file;
char magic[3];
int i, j;
int ch, occ;
int width, height, depth;
map_cell_t *cell;
// Open file
file = fopen(filename, "r");
if (file == NULL)
{
fprintf(stderr, "%s: %s\n", strerror(errno), filename);
return -1;
}
// Read ppm header
if ((fscanf(file, "%2s \n", magic) != 1) || (strcmp(magic, "P5") != 0))
{
fprintf(stderr, "incorrect image format; must be PGM/binary");
fclose(file);
return -1;
}
// Ignore comments
while ((ch = fgetc(file)) == '#')
while (fgetc(file) != '\n');
ungetc(ch, file);
// Read image dimensions
if(fscanf(file, " %d %d \n %d \n", &width, &height, &depth) != 3)
{
fprintf(stderr, "Failed ot read image dimensions");
return -1;
}
// Allocate space in the map
if (map->cells == NULL)
{
map->scale = scale;
map->size_x = width;
map->size_y = height;
map->cells = calloc(width * height, sizeof(map->cells[0]));
}
else
{
if (width != map->size_x || height != map->size_y)
{
//PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
return -1;
}
}
// Read in the image
for (j = height - 1; j >= 0; j--)
{
for (i = 0; i < width; i++)
{
ch = fgetc(file);
// Black-on-white images
if (!negate)
{
if (ch < depth / 4)
occ = +1;
else if (ch > 3 * depth / 4)
occ = -1;
else
occ = 0;
}
// White-on-black images
else
{
if (ch < depth / 4)
occ = -1;
else if (ch > 3 * depth / 4)
occ = +1;
else
occ = 0;
}
if (!MAP_VALID(map, i, j))
continue;
cell = map->cells + MAP_INDEX(map, i, j);
cell->occ_state = occ;
}
}
fclose(file);
return 0;
}
////////////////////////////////////////////////////////////////////////////
// Load a wifi signal strength map
/*
int map_load_wifi(map_t *map, const char *filename, int index)
{
FILE *file;
char magic[3];
int i, j;
int ch, level;
int width, height, depth;
map_cell_t *cell;
// Open file
file = fopen(filename, "r");
if (file == NULL)
{
fprintf(stderr, "%s: %s\n", strerror(errno), filename);
return -1;
}
// Read ppm header
fscanf(file, "%10s \n", magic);
if (strcmp(magic, "P5") != 0)
{
fprintf(stderr, "incorrect image format; must be PGM/binary");
return -1;
}
// Ignore comments
while ((ch = fgetc(file)) == '#')
while (fgetc(file) != '\n');
ungetc(ch, file);
// Read image dimensions
fscanf(file, " %d %d \n %d \n", &width, &height, &depth);
// Allocate space in the map
if (map->cells == NULL)
{
map->size_x = width;
map->size_y = height;
map->cells = calloc(width * height, sizeof(map->cells[0]));
}
else
{
if (width != map->size_x || height != map->size_y)
{
//PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
return -1;
}
}
// Read in the image
for (j = height - 1; j >= 0; j--)
{
for (i = 0; i < width; i++)
{
ch = fgetc(file);
if (!MAP_VALID(map, i, j))
continue;
if (ch == 0)
level = 0;
else
level = ch * 100 / 255 - 100;
cell = map->cells + MAP_INDEX(map, i, j);
cell->wifi_levels[index] = level;
}
}
fclose(file);
return 0;
}
*/

274
Localizations/Libraries/Ros/amcl/src/amcl/pf/eig3.c Normal file
View File

@@ -0,0 +1,274 @@
/* 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
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);
}

763
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf.c Normal file
View File

@@ -0,0 +1,763 @@
/*
* 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 <assert.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include "amcl/pf/pf.h"
#include "amcl/pf/pf_pdf.h"
#include "amcl/pf/pf_kdtree.h"
#include "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, void *random_pose_data)
{
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->random_pose_data = random_pose_data;
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;
// Number of leaf nodes is never higher than the max number of samples
pf->limit_cache = calloc(max_samples, sizeof(int));
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;
free(pf->limit_cache);
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);
return;
}
// Initialize the filter using a gaussian
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);
return;
}
// 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);
return;
}
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;
double total;
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);
return;
}
#include <float.h>
// 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);
set->n_effective = 0;
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;
set->n_effective += sample->weight*sample->weight;
}
// 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);
//printf("w_avg: %e slow: %e fast: %e\n",
//w_avg, pf->w_slow, 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;
}
}
set->n_effective = 1.0/set->n_effective;
return;
}
// copy set a to set b
void copy_set(pf_sample_set_t* set_a, pf_sample_set_t* set_b)
{
int i;
double total;
pf_sample_t *sample_a, *sample_b;
// Clean set b's kdtree
pf_kdtree_clear(set_b->kdtree);
// Copy samples from set a to create set b
total = 0;
set_b->sample_count = 0;
for(i = 0; i < set_a->sample_count; i++)
{
sample_b = set_b->samples + set_b->sample_count++;
sample_a = set_a->samples + i;
assert(sample_a->weight > 0);
// Copy sample a to sample b
sample_b->pose = sample_a->pose;
sample_b->weight = sample_a->weight;
total += sample_b->weight;
// Add sample to histogram
pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
}
// Normalize weights
for (i = 0; i < set_b->sample_count; i++)
{
sample_b = set_b->samples + i;
sample_b->weight /= total;
}
set_b->converged = set_a->converged;
}
// Resample the distribution
void pf_update_resample(pf_t *pf)
{
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;
if (pf->selective_resampling != 0)
{
if (set_a->n_effective > 0.5*(set_a->sample_count))
{
// copy set a to b
copy_set(set_a,set_b);
// Re-compute cluster statistics
pf_cluster_stats(pf, set_b);
// Use the newly created sample set
pf->current_set = (pf->current_set + 1) % 2;
return;
}
}
// 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)(pf->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);
return;
}
// 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;
// Return max_samples in case k is outside expected range, this shouldn't
// happen, but is added to prevent any runtime errors
if (k < 1 || k > pf->max_samples)
return pf->max_samples;
// Return value if cache is valid, which means value is non-zero positive
if (pf->limit_cache[k-1] > 0)
return pf->limit_cache[k-1];
if (k == 1)
{
pf->limit_cache[k-1] = pf->max_samples;
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)
{
pf->limit_cache[k-1] = pf->min_samples;
return pf->min_samples;
}
if (n > pf->max_samples)
{
pf->limit_cache[k-1] = pf->max_samples;
return pf->max_samples;
}
pf->limit_cache[k-1] = n;
return n;
}
// Re-compute the cluster statistics for a sample set
void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)
{
int i, j, k, cidx;
pf_sample_t *sample;
pf_cluster_t *cluster;
// Workspace
double m[4], c[2][2];
size_t count;
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->count = 0;
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
count = 0;
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->count += 1;
cluster->weight += sample->weight;
count += 1;
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]) / cluster->weight);
//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");
}
assert(fabs(weight) >= DBL_EPSILON);
if (fabs(weight) < DBL_EPSILON)
{
printf("ERROR : divide-by-zero exception : weight is zero\n");
return;
}
// 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]));
return;
}
void pf_set_selective_resampling(pf_t *pf, int selective_resampling)
{
pf->selective_resampling = selective_resampling;
}
// 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];
}
assert(fabs(mn) >= DBL_EPSILON);
if (fabs(mn) < DBL_EPSILON)
{
printf("ERROR : divide-by-zero exception : mn is zero\n");
return;
}
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));
return;
}
// 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;
}

163
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_draw.c Normal file
View File

@@ -0,0 +1,163 @@
/*
* 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 $
*************************************************************************/
#ifdef INCLUDE_RTKGUI
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include "rtk.h"
#include "pf.h"
#include "pf_pdf.h"
#include "pf_kdtree.h"
// 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);
}
return;
}
// 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);
return;
}
// 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);
return;
}
// 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);
}
return;
}
#endif

486
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_kdtree.c Normal file
View File

@@ -0,0 +1,486 @@
/*
* 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 "amcl/pf/pf_vector.h"
#include "amcl/pf/pf_kdtree.h"
// 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);
return;
}
////////////////////////////////////////////////////////////////////////////////
// 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;
return;
}
////////////////////////////////////////////////////////////////////////////////
// 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");
*/
return;
}
////////////////////////////////////////////////////////////////////////////////
// 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[])
{
//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;
}
// If the node exists, and it is a leaf node...
else if (node->leaf)
{
// If the keys are equal, increment the value
if (pf_kdtree_equal(self, key, node->key))
{
node->value += value;
}
// The keys are not equal, so split this node
else
{
// 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;
}
}
// If the node exists, and it has children...
else
{
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);
return;
}
////////////////////////////////////////////////////////////////////////////////
// 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);
}
return;
}
#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);
return;
}
////////////////////////////////////////////////////////////////////////////////
// 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);
}
return;
}
#endif

147
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_pdf.c Normal file
View File

@@ -0,0 +1,147 @@
/*
* 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 "amcl/pf/pf_pdf.h"
#include "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);
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)
{
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));
}

276
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_vector.c Normal file
View File

@@ -0,0 +1,276 @@
/*
* 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 "amcl/pf/pf_vector.h"
#include "amcl/pf/eig3.h"
// Return a zero vector
pf_vector_t pf_vector_zero()
{
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");
return;
}
// 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()
{
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");
}
return;
}
/*
// 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);
return;
}

510
Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_laser.cpp Normal file
View File

@@ -0,0 +1,510 @@
/*
* 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: AMCL laser routines
// Author: Andrew Howard
// Date: 6 Feb 2003
// CVS: $Id: amcl_laser.cc 7057 2008-10-02 00:44:06Z gbiggs $
//
///////////////////////////////////////////////////////////////////////////
#include <sys/types.h> // required by Darwin
#include <math.h>
#include <stdlib.h>
#include <assert.h>
#ifdef HAVE_UNISTD_H
#include <unistd.h>
#endif
#include "amcl/sensors/amcl_laser.h"
using namespace amcl;
////////////////////////////////////////////////////////////////////////////////
// Default constructor
AMCLLaser::AMCLLaser(size_t max_beams, map_t* map) : AMCLSensor(),
max_samples(0), max_obs(0),
temp_obs(NULL)
{
this->time = 0.0;
this->max_beams = max_beams;
this->map = map;
return;
}
AMCLLaser::~AMCLLaser()
{
if(temp_obs){
for(int k=0; k < max_samples; k++){
delete [] temp_obs[k];
}
delete []temp_obs;
}
}
void
AMCLLaser::SetModelBeam(double z_hit,
double z_short,
double z_max,
double z_rand,
double sigma_hit,
double lambda_short,
double chi_outlier)
{
this->model_type = LASER_MODEL_BEAM;
this->z_hit = z_hit;
this->z_short = z_short;
this->z_max = z_max;
this->z_rand = z_rand;
this->sigma_hit = sigma_hit;
this->lambda_short = lambda_short;
this->chi_outlier = chi_outlier;
}
void
AMCLLaser::SetModelLikelihoodField(double z_hit,
double z_rand,
double sigma_hit,
double max_occ_dist)
{
this->model_type = LASER_MODEL_LIKELIHOOD_FIELD;
this->z_hit = z_hit;
this->z_rand = z_rand;
this->sigma_hit = sigma_hit;
map_update_cspace(this->map, max_occ_dist);
}
void
AMCLLaser::SetModelLikelihoodFieldProb(double z_hit,
double z_rand,
double sigma_hit,
double max_occ_dist,
bool do_beamskip,
double beam_skip_distance,
double beam_skip_threshold,
double beam_skip_error_threshold)
{
this->model_type = LASER_MODEL_LIKELIHOOD_FIELD_PROB;
this->z_hit = z_hit;
this->z_rand = z_rand;
this->sigma_hit = sigma_hit;
this->do_beamskip = do_beamskip;
this->beam_skip_distance = beam_skip_distance;
this->beam_skip_threshold = beam_skip_threshold;
this->beam_skip_error_threshold = beam_skip_error_threshold;
map_update_cspace(this->map, max_occ_dist);
}
////////////////////////////////////////////////////////////////////////////////
// Apply the laser sensor model
bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
{
if (this->max_beams < 2)
return false;
// Apply the laser sensor model
if(this->model_type == LASER_MODEL_BEAM)
pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD)
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);
else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD_PROB)
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModelProb, data);
else
pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
return true;
}
////////////////////////////////////////////////////////////////////////////////
// Determine the probability for the given pose
double AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t* set)
{
AMCLLaser *self;
int i, j, step;
double z, pz;
double p;
double map_range;
double obs_range, obs_bearing;
double total_weight;
pf_sample_t *sample;
pf_vector_t pose;
self = (AMCLLaser*) data->sensor;
total_weight = 0.0;
// Compute the sample weights
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose;
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add(self->laser_pose, pose);
p = 1.0;
step = (data->range_count - 1) / (self->max_beams - 1);
for (i = 0; i < data->range_count; i += step)
{
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// Compute the range according to the map
map_range = map_calc_range(self->map, pose.v[0], pose.v[1],
pose.v[2] + obs_bearing, data->range_max);
pz = 0.0;
// Part 1: good, but noisy, hit
z = obs_range - map_range;
pz += self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit));
// Part 2: short reading from unexpected obstacle (e.g., a person)
if(z < 0)
pz += self->z_short * self->lambda_short * exp(-self->lambda_short*obs_range);
// Part 3: Failure to detect obstacle, reported as max-range
if(obs_range == data->range_max)
pz += self->z_max * 1.0;
// Part 4: Random measurements
if(obs_range < data->range_max)
pz += self->z_rand * 1.0/data->range_max;
// TODO: outlier rejection for short readings
assert(pz <= 1.0);
assert(pz >= 0.0);
// p *= pz;
// here we have an ad-hoc weighting scheme for combining beam probs
// works well, though...
p += pz*pz*pz;
}
sample->weight *= p;
total_weight += sample->weight;
}
return(total_weight);
}
double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
{
AMCLLaser *self;
int i, j, step;
double z, pz;
double p;
double obs_range, obs_bearing;
double total_weight;
pf_sample_t *sample;
pf_vector_t pose;
pf_vector_t hit;
self = (AMCLLaser*) data->sensor;
total_weight = 0.0;
// Compute the sample weights
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose;
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add(self->laser_pose, pose);
p = 1.0;
// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
double z_rand_mult = 1.0/data->range_max;
step = (data->range_count - 1) / (self->max_beams - 1);
// Step size must be at least 1
if(step < 1)
step = 1;
for (i = 0; i < data->range_count; i += step)
{
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// This model ignores max range readings
if(obs_range >= data->range_max)
continue;
// Check for NaN
if(obs_range != obs_range)
continue;
pz = 0.0;
// Compute the endpoint of the beam
hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
// Convert to map grid coords.
int mi, mj;
mi = MAP_GXWX(self->map, hit.v[0]);
mj = MAP_GYWY(self->map, hit.v[1]);
// Part 1: Get distance from the hit to closest obstacle.
// Off-map penalized as max distance
if(!MAP_VALID(self->map, mi, mj))
z = self->map->max_occ_dist;
else
z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
// Gaussian model
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
pz += self->z_hit * exp(-(z * z) / z_hit_denom);
// Part 2: random measurements
pz += self->z_rand * z_rand_mult;
// TODO: outlier rejection for short readings
assert(pz <= 1.0);
assert(pz >= 0.0);
// p *= pz;
// here we have an ad-hoc weighting scheme for combining beam probs
// works well, though...
p += pz*pz*pz;
}
sample->weight *= p;
total_weight += sample->weight;
}
return(total_weight);
}
double AMCLLaser::LikelihoodFieldModelProb(AMCLLaserData *data, pf_sample_set_t* set)
{
AMCLLaser *self;
int i, j, step;
double z, pz;
double log_p;
double obs_range, obs_bearing;
double total_weight;
pf_sample_t *sample;
pf_vector_t pose;
pf_vector_t hit;
self = (AMCLLaser*) data->sensor;
total_weight = 0.0;
step = ceil((data->range_count) / static_cast<double>(self->max_beams));
// Step size must be at least 1
if(step < 1)
step = 1;
// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
double z_rand_mult = 1.0/data->range_max;
double max_dist_prob = exp(-(self->map->max_occ_dist * self->map->max_occ_dist) / z_hit_denom);
//Beam skipping - ignores beams for which a majority of particles do not agree with the map
//prevents correct particles from getting down weighted because of unexpected obstacles
//such as humans
bool do_beamskip = self->do_beamskip;
double beam_skip_distance = self->beam_skip_distance;
double beam_skip_threshold = self->beam_skip_threshold;
//we only do beam skipping if the filter has converged
if(do_beamskip && !set->converged){
do_beamskip = false;
}
//we need a count the no of particles for which the beam agreed with the map
int *obs_count = new int[self->max_beams]();
//we also need a mask of which observations to integrate (to decide which beams to integrate to all particles)
bool *obs_mask = new bool[self->max_beams]();
int beam_ind = 0;
//realloc indicates if we need to reallocate the temp data structure needed to do beamskipping
bool realloc = false;
if(do_beamskip){
if(self->max_obs < self->max_beams){
realloc = true;
}
if(self->max_samples < set->sample_count){
realloc = true;
}
if(realloc){
self->reallocTempData(set->sample_count, self->max_beams);
fprintf(stderr, "Reallocing temp weights %d - %d\n", self->max_samples, self->max_obs);
}
}
// Compute the sample weights
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose;
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add(self->laser_pose, pose);
log_p = 0;
beam_ind = 0;
for (i = 0; i < data->range_count; i += step, beam_ind++)
{
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// This model ignores max range readings
if(obs_range >= data->range_max){
continue;
}
// Check for NaN
if(obs_range != obs_range){
continue;
}
pz = 0.0;
// Compute the endpoint of the beam
hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
// Convert to map grid coords.
int mi, mj;
mi = MAP_GXWX(self->map, hit.v[0]);
mj = MAP_GYWY(self->map, hit.v[1]);
// Part 1: Get distance from the hit to closest obstacle.
// Off-map penalized as max distance
if(!MAP_VALID(self->map, mi, mj)){
pz += self->z_hit * max_dist_prob;
}
else{
z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
if(z < beam_skip_distance){
obs_count[beam_ind] += 1;
}
pz += self->z_hit * exp(-(z * z) / z_hit_denom);
}
// Gaussian model
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
// Part 2: random measurements
pz += self->z_rand * z_rand_mult;
assert(pz <= 1.0);
assert(pz >= 0.0);
// TODO: outlier rejection for short readings
if(!do_beamskip){
log_p += log(pz);
}
else{
self->temp_obs[j][beam_ind] = pz;
}
}
if(!do_beamskip){
sample->weight *= exp(log_p);
total_weight += sample->weight;
}
}
if(do_beamskip){
int skipped_beam_count = 0;
for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){
if((obs_count[beam_ind] / static_cast<double>(set->sample_count)) > beam_skip_threshold){
obs_mask[beam_ind] = true;
}
else{
obs_mask[beam_ind] = false;
skipped_beam_count++;
}
}
//we check if there is at least a critical number of beams that agreed with the map
//otherwise it probably indicates that the filter converged to a wrong solution
//if that's the case we integrate all the beams and hope the filter might converge to
//the right solution
bool error = false;
if(skipped_beam_count >= (beam_ind * self->beam_skip_error_threshold)){
fprintf(stderr, "Over %f%% of the observations were not in the map - pf may have converged to wrong pose - integrating all observations\n", (100 * self->beam_skip_error_threshold));
error = true;
}
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose;
log_p = 0;
for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){
if(error || obs_mask[beam_ind]){
log_p += log(self->temp_obs[j][beam_ind]);
}
}
sample->weight *= exp(log_p);
total_weight += sample->weight;
}
}
delete [] obs_count;
delete [] obs_mask;
return(total_weight);
}
void AMCLLaser::reallocTempData(int new_max_samples, int new_max_obs){
if(temp_obs){
for(int k=0; k < max_samples; k++){
delete [] temp_obs[k];
}
delete []temp_obs;
}
max_obs = new_max_obs;
max_samples = fmax(max_samples, new_max_samples);
temp_obs = new double*[max_samples]();
for(int k=0; k < max_samples; k++){
temp_obs[k] = new double[max_obs]();
}
}

310
Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_odom.cpp Normal file
View File

@@ -0,0 +1,310 @@
/*
* 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: AMCL odometry routines
// Author: Andrew Howard
// Date: 6 Feb 2003
// CVS: $Id: amcl_odom.cc 7057 2008-10-02 00:44:06Z gbiggs $
//
///////////////////////////////////////////////////////////////////////////
#include <algorithm>
#include <sys/types.h> // required by Darwin
#include <math.h>
#include "amcl/sensors/amcl_odom.h"
using namespace amcl;
static double
normalize(double z)
{
return atan2(sin(z),cos(z));
}
static double
angle_diff(double a, double b)
{
double d1, d2;
a = normalize(a);
b = normalize(b);
d1 = a-b;
d2 = 2*M_PI - fabs(d1);
if(d1 > 0)
d2 *= -1.0;
if(fabs(d1) < fabs(d2))
return(d1);
else
return(d2);
}
////////////////////////////////////////////////////////////////////////////////
// Default constructor
AMCLOdom::AMCLOdom() : AMCLSensor()
{
this->time = 0.0;
}
void
AMCLOdom::SetModelDiff(double alpha1,
double alpha2,
double alpha3,
double alpha4)
{
this->model_type = ODOM_MODEL_DIFF;
this->alpha1 = alpha1;
this->alpha2 = alpha2;
this->alpha3 = alpha3;
this->alpha4 = alpha4;
}
void
AMCLOdom::SetModelOmni(double alpha1,
double alpha2,
double alpha3,
double alpha4,
double alpha5)
{
this->model_type = ODOM_MODEL_OMNI;
this->alpha1 = alpha1;
this->alpha2 = alpha2;
this->alpha3 = alpha3;
this->alpha4 = alpha4;
this->alpha5 = alpha5;
}
void
AMCLOdom::SetModel( odom_model_t type,
double alpha1,
double alpha2,
double alpha3,
double alpha4,
double alpha5 )
{
this->model_type = type;
this->alpha1 = alpha1;
this->alpha2 = alpha2;
this->alpha3 = alpha3;
this->alpha4 = alpha4;
this->alpha5 = alpha5;
}
////////////////////////////////////////////////////////////////////////////////
// Apply the action model
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
{
AMCLOdomData *ndata;
ndata = (AMCLOdomData*) data;
// Compute the new sample poses
pf_sample_set_t *set;
set = pf->sets + pf->current_set;
pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
switch( this->model_type )
{
case ODOM_MODEL_OMNI:
{
double delta_trans, delta_rot, delta_bearing;
double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot = ndata->delta.v[2];
// Precompute a couple of things
double trans_hat_stddev = (alpha3 * (delta_trans*delta_trans) +
alpha1 * (delta_rot*delta_rot));
double rot_hat_stddev = (alpha4 * (delta_rot*delta_rot) +
alpha2 * (delta_trans*delta_trans));
double strafe_hat_stddev = (alpha1 * (delta_rot*delta_rot) +
alpha5 * (delta_trans*delta_trans));
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]) + sample->pose.v[2];
double cs_bearing = cos(delta_bearing);
double sn_bearing = sin(delta_bearing);
// Sample pose differences
delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
// Apply sampled update to particle pose
sample->pose.v[0] += (delta_trans_hat * cs_bearing +
delta_strafe_hat * sn_bearing);
sample->pose.v[1] += (delta_trans_hat * sn_bearing -
delta_strafe_hat * cs_bearing);
sample->pose.v[2] += delta_rot_hat ;
}
}
break;
case ODOM_MODEL_DIFF:
{
// Implement sample_motion_odometry (Prob Rob p 136)
double delta_rot1, delta_trans, delta_rot2;
double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
double delta_rot1_noise, delta_rot2_noise;
// Avoid computing a bearing from two poses that are extremely near each
// other (happens on in-place rotation).
if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] +
ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
delta_rot1 = 0.0;
else
delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]);
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
// We want to treat backward and forward motion symmetrically for the
// noise model to be applied below. The standard model seems to assume
// forward motion.
delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
fabs(angle_diff(delta_rot1,M_PI)));
delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
fabs(angle_diff(delta_rot2,M_PI)));
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
// Sample pose differences
delta_rot1_hat = angle_diff(delta_rot1,
pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
this->alpha2*delta_trans*delta_trans));
delta_trans_hat = delta_trans -
pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
this->alpha4*delta_rot1_noise*delta_rot1_noise +
this->alpha4*delta_rot2_noise*delta_rot2_noise);
delta_rot2_hat = angle_diff(delta_rot2,
pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
this->alpha2*delta_trans*delta_trans));
// Apply sampled update to particle pose
sample->pose.v[0] += delta_trans_hat *
cos(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[1] += delta_trans_hat *
sin(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
}
}
break;
case ODOM_MODEL_OMNI_CORRECTED:
{
double delta_trans, delta_rot, delta_bearing;
double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot = ndata->delta.v[2];
// Precompute a couple of things
double trans_hat_stddev = sqrt( alpha3 * (delta_trans*delta_trans) +
alpha4 * (delta_rot*delta_rot) );
double rot_hat_stddev = sqrt( alpha1 * (delta_rot*delta_rot) +
alpha2 * (delta_trans*delta_trans) );
double strafe_hat_stddev = sqrt( alpha4 * (delta_rot*delta_rot) +
alpha5 * (delta_trans*delta_trans) );
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]) + sample->pose.v[2];
double cs_bearing = cos(delta_bearing);
double sn_bearing = sin(delta_bearing);
// Sample pose differences
delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
// Apply sampled update to particle pose
sample->pose.v[0] += (delta_trans_hat * cs_bearing +
delta_strafe_hat * sn_bearing);
sample->pose.v[1] += (delta_trans_hat * sn_bearing -
delta_strafe_hat * cs_bearing);
sample->pose.v[2] += delta_rot_hat ;
}
}
break;
case ODOM_MODEL_DIFF_CORRECTED:
{
// Implement sample_motion_odometry (Prob Rob p 136)
double delta_rot1, delta_trans, delta_rot2;
double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
double delta_rot1_noise, delta_rot2_noise;
// Avoid computing a bearing from two poses that are extremely near each
// other (happens on in-place rotation).
if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] +
ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
delta_rot1 = 0.0;
else
delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]);
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
// We want to treat backward and forward motion symmetrically for the
// noise model to be applied below. The standard model seems to assume
// forward motion.
delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
fabs(angle_diff(delta_rot1,M_PI)));
delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
fabs(angle_diff(delta_rot2,M_PI)));
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
// Sample pose differences
delta_rot1_hat = angle_diff(delta_rot1,
pf_ran_gaussian(sqrt(this->alpha1*delta_rot1_noise*delta_rot1_noise +
this->alpha2*delta_trans*delta_trans)));
delta_trans_hat = delta_trans -
pf_ran_gaussian(sqrt(this->alpha3*delta_trans*delta_trans +
this->alpha4*delta_rot1_noise*delta_rot1_noise +
this->alpha4*delta_rot2_noise*delta_rot2_noise));
delta_rot2_hat = angle_diff(delta_rot2,
pf_ran_gaussian(sqrt(this->alpha1*delta_rot2_noise*delta_rot2_noise +
this->alpha2*delta_trans*delta_trans)));
// Apply sampled update to particle pose
sample->pose.v[0] += delta_trans_hat *
cos(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[1] += delta_trans_hat *
sin(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
}
}
break;
}
return true;
}

95
Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_sensor.cpp Normal file
View File

@@ -0,0 +1,95 @@
/*
* 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: AMCL sensor
// Author: Andrew Howard
// Date: 6 Feb 2003
// CVS: $Id: amcl_sensor.cc 7057 2008-10-02 00:44:06Z gbiggs $
//
///////////////////////////////////////////////////////////////////////////
#include "amcl/sensors/amcl_sensor.h"
using namespace amcl;
////////////////////////////////////////////////////////////////////////////////
// Default constructor
AMCLSensor::AMCLSensor()
{
return;
}
AMCLSensor::~AMCLSensor()
{
}
////////////////////////////////////////////////////////////////////////////////
// Apply the action model
bool AMCLSensor::UpdateAction(pf_t *pf, AMCLSensorData *data)
{
return false;
}
////////////////////////////////////////////////////////////////////////////////
// Initialize the filter
bool AMCLSensor::InitSensor(pf_t *pf, AMCLSensorData *data)
{
return false;
}
////////////////////////////////////////////////////////////////////////////////
// Apply the sensor model
bool AMCLSensor::UpdateSensor(pf_t *pf, AMCLSensorData *data)
{
return false;
}
#ifdef INCLUDE_RTKGUI
////////////////////////////////////////////////////////////////////////////////
// Setup the GUI
void AMCLSensor::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig)
{
return;
}
////////////////////////////////////////////////////////////////////////////////
// Shutdown the GUI
void AMCLSensor::ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig)
{
return;
}
////////////////////////////////////////////////////////////////////////////////
// Draw sensor data
void AMCLSensor::UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data)
{
return;
}
#endif

72
Localizations/Libraries/Ros/amcl/src/amcl_node.cpp Normal file
View File

@@ -0,0 +1,72 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* 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
*
*/
/* Author: Brian Gerkey */
// Signal handling
#include <signal.h>
#include <amcl/amcl.h>
boost::shared_ptr<amcl::Amcl> amcl_node_ptr;
void sigintHandler(int sig)
{
// Save latest pose as we're shutting down.
geometry_msgs::PoseWithCovarianceStamped amcl_pose;
amcl_node_ptr->savePoseToServer(amcl_pose);
ros::shutdown();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "amcl");
ros::NodeHandle nh;
// Override default sigint handler
signal(SIGINT, sigintHandler);
// Make our node available to sigintHandler
amcl_node_ptr.reset(new amcl::Amcl());
if (argc == 1)
{
// run using ROS input
ros::spin();
}
else if ((argc >= 3) && (std::string(argv[1]) == "--run-from-bag"))
{
if (argc == 3)
{
amcl_node_ptr->runFromBag(argv[2]);
}
else if ((argc == 4) && (std::string(argv[3]) == "--global-localization"))
{
amcl_node_ptr->runFromBag(argv[2], true);
}
}
// Without this, our boost locks are not shut down nicely
amcl_node_ptr.reset();
// To quote Morgan, Hooray!
return(0);
}