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/amcl_node.cpp
+1629
View File
File diff suppressed because it is too large Load Diff
navigation2/nav2_amcl/src/main.cpp
+30
View File
@@ -0,0 +1,30 @@
// Copyright (c) 2018 Intel Corporation
//
// 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 <memory>
#include "nav2_amcl/amcl_node.hpp"
#include "rclcpp/rclcpp.hpp"
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<nav2_amcl::AmclNode>();
rclcpp::spin(node->get_node_base_interface());
rclcpp::shutdown();
return 0;
}
navigation2/nav2_amcl/src/map/CMakeLists.txt
+13
View File
@@ -0,0 +1,13 @@
add_library(map_lib SHARED
map.c
map_range.c
map_draw.c
map_cspace.cpp
)
install(TARGETS
map_lib
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
navigation2/nav2_amcl/src/map/map.c
+65
View File
@@ -0,0 +1,65 @@
/*
* 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 "nav2_amcl/map/map.hpp"
// 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);
}
navigation2/nav2_amcl/src/map/map_cspace.cpp
+213
View File
@@ -0,0 +1,213 @@
/*
* 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 <math.h>
#include <stdlib.h>
#include <string.h>
#include <queue>
#include "nav2_amcl/map/map.hpp"
/*
* @class CellData
* @brief Data about map cells
*/
class CellData
{
public:
map_t * map_;
unsigned int i_, j_;
unsigned int src_i_, src_j_;
};
/*
* @class CachedDistanceMap
* @brief Cached map with distances
*/
class CachedDistanceMap
{
public:
/*
* @brief CachedDistanceMap constructor
*/
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);
}
}
}
/*
* @brief CachedDistanceMap destructor
*/
~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_;
};
/*
* @brief operator<
*/
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;
}
/*
* @brief get_distance_map
* @param scale of cost information wrt distance
* @param max_dist Maximum distance to cache from occupied information
* @return Pointer to cached distance map
*/
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;
}
/*
* @brief enqueue cell data for caching
*/
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;
}
/*
* @brief Update the cspace distance values
* @param map Map to update
* @param max_occ_distance Maximum distance for occpuancy interest
*/
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 (static_cast<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 (static_cast<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;
}
navigation2/nav2_amcl/src/map/map_draw.c
+147
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: Local map GUI functions
* Author: Andrew Howard
* Date: 18 Jan 2003
* CVS: $Id: map_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
**************************************************************************/
#pragma GCC diagnostic ignored "-Wpedantic"
#ifdef INCLUDE_RTKGUI
#include <errno.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <rtk.h>
#include "nav2_amcl/map/map.hpp"
////////////////////////////////////////////////////////////////////////////
// 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);
}
////////////////////////////////////////////////////////////////////////////
// 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);
}
////////////////////////////////////////////////////////////////////////////
// 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);
}
#endif
navigation2/nav2_amcl/src/map/map_range.c
+118
View File
@@ -0,0 +1,118 @@
/*
* 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 "nav2_amcl/map/map.hpp"
// 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;
}
navigation2/nav2_amcl/src/motion_model/CMakeLists.txt
+16
View File
@@ -0,0 +1,16 @@
add_library(motions_lib SHARED
omni_motion_model.cpp
differential_motion_model.cpp
)
target_link_libraries(motions_lib pf_lib)
ament_target_dependencies(motions_lib
pluginlib
nav2_util
)
install(TARGETS
motions_lib
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
navigation2/nav2_amcl/src/motion_model/differential_motion_model.cpp
+117
View File
@@ -0,0 +1,117 @@
/*
* 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 "nav2_amcl/motion_model/differential_motion_model.hpp"
namespace nav2_amcl
{
void
DifferentialMotionModel::initialize(
double alpha1, double alpha2, double alpha3, double alpha4,
double alpha5)
{
alpha1_ = alpha1;
alpha2_ = alpha2;
alpha3_ = alpha3;
alpha4_ = alpha4;
alpha5_ = alpha5;
}
void
DifferentialMotionModel::odometryUpdate(
pf_t * pf, const pf_vector_t & pose,
const pf_vector_t & delta)
{
// Compute the new sample poses
pf_sample_set_t * set;
set = pf->sets + pf->current_set;
pf_vector_t old_pose = pf_vector_sub(pose, delta);
// 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(
delta.v[1] * delta.v[1] +
delta.v[0] * delta.v[0]) < 0.01)
{
delta_rot1 = 0.0;
} else {
delta_rot1 = angleutils::angle_diff(
atan2(delta.v[1], delta.v[0]),
old_pose.v[2]);
}
delta_trans = sqrt(
delta.v[0] * delta.v[0] +
delta.v[1] * delta.v[1]);
delta_rot2 = angleutils::angle_diff(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(angleutils::angle_diff(delta_rot1, 0.0)),
fabs(angleutils::angle_diff(delta_rot1, M_PI)));
delta_rot2_noise = std::min(
fabs(angleutils::angle_diff(delta_rot2, 0.0)),
fabs(angleutils::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 = angleutils::angle_diff(
delta_rot1,
pf_ran_gaussian(
sqrt(
alpha1_ * delta_rot1_noise * delta_rot1_noise +
alpha2_ * delta_trans * delta_trans)));
delta_trans_hat = delta_trans -
pf_ran_gaussian(
sqrt(
alpha3_ * delta_trans * delta_trans +
alpha4_ * delta_rot1_noise * delta_rot1_noise +
alpha4_ * delta_rot2_noise * delta_rot2_noise));
delta_rot2_hat = angleutils::angle_diff(
delta_rot2,
pf_ran_gaussian(
sqrt(
alpha1_ * delta_rot2_noise * delta_rot2_noise +
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;
}
}
} // namespace nav2_amcl
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(nav2_amcl::DifferentialMotionModel, nav2_amcl::MotionModel)
navigation2/nav2_amcl/src/motion_model/omni_motion_model.cpp
+94
View File
@@ -0,0 +1,94 @@
/*
* 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 "nav2_amcl/motion_model/omni_motion_model.hpp"
namespace nav2_amcl
{
void
OmniMotionModel::initialize(
double alpha1, double alpha2, double alpha3, double alpha4,
double alpha5)
{
alpha1_ = alpha1;
alpha2_ = alpha2;
alpha3_ = alpha3;
alpha4_ = alpha4;
alpha5_ = alpha5;
}
void
OmniMotionModel::odometryUpdate(
pf_t * pf, const pf_vector_t & pose,
const pf_vector_t & delta)
{
// Compute the new sample poses
pf_sample_set_t * set;
set = pf->sets + pf->current_set;
pf_vector_t old_pose = pf_vector_sub(pose, delta);
double delta_trans, delta_rot, delta_bearing;
double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
delta_trans = sqrt(
delta.v[0] * delta.v[0] +
delta.v[1] * delta.v[1]);
delta_rot = 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 = angleutils::angle_diff(
atan2(delta.v[1], 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;
}
}
} // namespace nav2_amcl
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(nav2_amcl::OmniMotionModel, nav2_amcl::MotionModel)
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);
}
navigation2/nav2_amcl/src/sensors/CMakeLists.txt
+15
View File
@@ -0,0 +1,15 @@
add_library(sensors_lib SHARED
laser/laser.cpp
laser/beam_model.cpp
laser/likelihood_field_model.cpp
laser/likelihood_field_model_prob.cpp
)
# map_update_cspace
target_link_libraries(sensors_lib pf_lib map_lib)
install(TARGETS
sensors_lib
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
navigation2/nav2_amcl/src/sensors/laser/beam_model.cpp
+136
View File
@@ -0,0 +1,136 @@
/*
* 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 <math.h>
#include <assert.h>
#include "nav2_amcl/sensors/laser/laser.hpp"
namespace nav2_amcl
{
BeamModel::BeamModel(
double z_hit, double z_short, double z_max, double z_rand, double sigma_hit,
double lambda_short, double chi_outlier, size_t max_beams, map_t * map)
: Laser(max_beams, map)
{
z_hit_ = z_hit;
z_rand_ = z_rand;
sigma_hit_ = sigma_hit;
z_short_ = z_short;
z_max_ = z_max;
lambda_short_ = lambda_short;
chi_outlier_ = chi_outlier;
}
// Determine the probability for the given pose
double
BeamModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
{
BeamModel * 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 = reinterpret_cast<BeamModel *>(data->laser);
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];
// Check for NaN
if (isnan(obs_range)) {
continue;
}
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;
}
bool
BeamModel::sensorUpdate(pf_t * pf, LaserData * data)
{
if (max_beams_ < 2) {
return false;
}
pf_update_sensor(pf, (pf_sensor_model_fn_t) sensorFunction, data);
return true;
}
} // namespace nav2_amcl
navigation2/nav2_amcl/src/sensors/laser/laser.cpp
+73
View File
@@ -0,0 +1,73 @@
/*
* 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 <sys/types.h>
#include <math.h>
#include <stdlib.h>
#include <assert.h>
#include "nav2_amcl/sensors/laser/laser.hpp"
namespace nav2_amcl
{
Laser::Laser(size_t max_beams, map_t * map)
: max_samples_(0), max_obs_(0), temp_obs_(NULL)
{
max_beams_ = max_beams;
map_ = map;
}
Laser::~Laser()
{
if (temp_obs_) {
for (int k = 0; k < max_samples_; k++) {
delete[] temp_obs_[k];
}
delete[] temp_obs_;
}
}
void
Laser::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_]();
}
}
void
Laser::SetLaserPose(pf_vector_t & laser_pose)
{
laser_pose_ = laser_pose;
}
} // namespace nav2_amcl
navigation2/nav2_amcl/src/sensors/laser/likelihood_field_model.cpp
+146
View File
@@ -0,0 +1,146 @@
/*
* 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 <math.h>
#include <assert.h>
#include "nav2_amcl/sensors/laser/laser.hpp"
namespace nav2_amcl
{
LikelihoodFieldModel::LikelihoodFieldModel(
double z_hit, double z_rand, double sigma_hit,
double max_occ_dist, size_t max_beams, map_t * map)
: Laser(max_beams, map)
{
z_hit_ = z_hit;
z_rand_ = z_rand;
sigma_hit_ = sigma_hit;
map_update_cspace(map, max_occ_dist);
}
double
LikelihoodFieldModel::sensorFunction(LaserData * data, pf_sample_set_t * set)
{
LikelihoodFieldModel * 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 = reinterpret_cast<LikelihoodFieldModel *>(data->laser);
// 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;
}
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;
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;
}
bool
LikelihoodFieldModel::sensorUpdate(pf_t * pf, LaserData * data)
{
if (max_beams_ < 2) {
return false;
}
pf_update_sensor(pf, (pf_sensor_model_fn_t) sensorFunction, data);
return true;
}
} // namespace nav2_amcl
navigation2/nav2_amcl/src/sensors/laser/likelihood_field_model_prob.cpp
+254
View File
@@ -0,0 +1,254 @@
/*
* 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 <math.h>
#include <assert.h>
#include "nav2_amcl/sensors/laser/laser.hpp"
namespace nav2_amcl
{
LikelihoodFieldModelProb::LikelihoodFieldModelProb(
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,
size_t max_beams, map_t * map)
: Laser(max_beams, map)
{
z_hit_ = z_hit;
z_rand_ = z_rand;
sigma_hit_ = sigma_hit;
do_beamskip_ = do_beamskip;
beam_skip_distance_ = beam_skip_distance;
beam_skip_threshold_ = beam_skip_threshold;
beam_skip_error_threshold_ = beam_skip_error_threshold;
map_update_cspace(map, max_occ_dist);
}
// Determine the probability for the given pose
double
LikelihoodFieldModelProb::sensorFunction(LaserData * data, pf_sample_set_t * set)
{
LikelihoodFieldModelProb * 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 = reinterpret_cast<LikelihoodFieldModelProb *>(data->laser);
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 majoirty 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;
}
bool
LikelihoodFieldModelProb::sensorUpdate(pf_t * pf, LaserData * data)
{
if (max_beams_ < 2) {
return false;
}
pf_update_sensor(pf, (pf_sensor_model_fn_t) sensorFunction, data);
return true;
}
} // namespace nav2_amcl