add humble-navigation2

navigation2/nav2_amcl/CMakeLists.txt

@@ -0,0 +1,114 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_amcl)
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_components REQUIRED)
+find_package(message_filters REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(nav2_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+
+nav2_package()
+
+include_directories(
+  include
+)
+
+include(CheckSymbolExists)
+check_symbol_exists(drand48 stdlib.h HAVE_DRAND48)
+
+add_subdirectory(src/pf)
+add_subdirectory(src/map)
+add_subdirectory(src/motion_model)
+add_subdirectory(src/sensors)
+
+set(executable_name amcl)
+
+add_executable(${executable_name}
+  src/main.cpp
+)
+
+set(library_name ${executable_name}_core)
+
+add_library(${library_name} SHARED
+  src/amcl_node.cpp
+)
+
+target_include_directories(${library_name} PRIVATE src/include)
+
+if(HAVE_DRAND48)
+  target_compile_definitions(${library_name} PRIVATE "HAVE_DRAND48")
+endif()
+
+set(dependencies
+  rclcpp
+  rclcpp_lifecycle
+  rclcpp_components
+  message_filters
+  tf2_geometry_msgs
+  geometry_msgs
+  nav_msgs
+  sensor_msgs
+  std_srvs
+  tf2_ros
+  tf2
+  nav2_util
+  nav2_msgs
+  pluginlib
+)
+
+ament_target_dependencies(${executable_name}
+  ${dependencies}
+)
+
+target_link_libraries(${executable_name}
+  ${library_name}
+)
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+target_link_libraries(${library_name}
+  map_lib pf_lib sensors_lib
+)
+
+rclcpp_components_register_nodes(${library_name} "nav2_amcl::AmclNode")
+
+install(TARGETS ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(TARGETS ${executable_name}
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${library_name} pf_lib sensors_lib motions_lib map_lib)
+ament_export_dependencies(${dependencies})
+pluginlib_export_plugin_description_file(nav2_amcl plugins.xml)
+ament_package()

navigation2/nav2_amcl/README.md

@@ -0,0 +1,4 @@
+# AMCL
+Adaptive Monte Carlo Localization (AMCL) is a probabilistic localization module which estimates the position and orientation (i.e. Pose) of a robot in a given known map using a 2D laser scanner. This is largely a refactored port from ROS 1 without any algorithmic changes.
+
+See the [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-amcl.html) for more details about configurable settings and their meanings.

navigation2/nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -0,0 +1,398 @@
+/*
+ *  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
+ *
+ */
+
+#ifndef NAV2_AMCL__AMCL_NODE_HPP_
+#define NAV2_AMCL__AMCL_NODE_HPP_
+
+#include <atomic>
+#include <map>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "message_filters/subscriber.h"
+#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_amcl/motion_model/motion_model.hpp"
+#include "nav2_amcl/sensors/laser/laser.hpp"
+#include "nav2_msgs/msg/particle.hpp"
+#include "nav2_msgs/msg/particle_cloud.hpp"
+#include "nav2_msgs/srv/set_initial_pose.hpp"
+#include "nav_msgs/srv/set_map.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "tf2_ros/transform_broadcaster.h"
+#include "tf2_ros/transform_listener.h"
+#include "pluginlib/class_loader.hpp"
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#pragma GCC diagnostic ignored "-Wreorder"
+#include "tf2_ros/message_filter.h"
+#pragma GCC diagnostic pop
+
+#define NEW_UNIFORM_SAMPLING 1
+
+namespace nav2_amcl
+{
+/*
+ * @class AmclNode
+ * @brief ROS wrapper for AMCL
+ */
+class AmclNode : public nav2_util::LifecycleNode
+{
+public:
+  /*
+   * @brief AMCL constructor
+   * @param options Additional options to control creation of the node.
+   */
+  explicit AmclNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  /*
+   * @brief AMCL destructor
+   */
+  ~AmclNode();
+
+protected:
+  /*
+   * @brief Lifecycle configure
+   */
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  /*
+   * @brief Lifecycle activate
+   */
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  /*
+   * @brief Lifecycle deactivate
+   */
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  /*
+   * @brief Lifecycle cleanup
+   */
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  /*
+   * @brief Lifecycle shutdown
+   */
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+
+  // Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't
+  // respond until we're in the active state
+  std::atomic<bool> active_{false};
+
+  // Dedicated callback group and executor for services and subscriptions in AmclNode,
+  // in order to isolate TF timer used in message filter.
+  rclcpp::CallbackGroup::SharedPtr callback_group_;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+  std::unique_ptr<nav2_util::NodeThread> executor_thread_;
+
+  // Pose hypothesis
+  typedef struct
+  {
+    double weight;             // Total weight (weights sum to 1)
+    pf_vector_t pf_pose_mean;  // Mean of pose esimate
+    pf_matrix_t pf_pose_cov;   // Covariance of pose estimate
+  } amcl_hyp_t;
+
+  // Map-related
+  /*
+   * @brief Get new map from ROS topic to localize in
+   * @param msg Map message
+   */
+  void mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
+  /*
+   * @brief Handle a new map message
+   * @param msg Map message
+   */
+  void handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg);
+  /*
+   * @brief Creates lookup table of free cells in map
+   */
+  void createFreeSpaceVector();
+  /*
+   * @brief Frees allocated map related memory
+   */
+  void freeMapDependentMemory();
+  map_t * map_{nullptr};
+  /*
+   * @brief Convert an occupancy grid map to an AMCL map
+   * @param map_msg Map message
+   * @return pointer to map for AMCL to use
+   */
+  map_t * convertMap(const nav_msgs::msg::OccupancyGrid & map_msg);
+  bool first_map_only_{true};
+  std::atomic<bool> first_map_received_{false};
+  amcl_hyp_t * initial_pose_hyp_;
+  std::recursive_mutex mutex_;
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
+#if NEW_UNIFORM_SAMPLING
+  static std::vector<std::pair<int, int>> free_space_indices;
+#endif
+
+  // Transforms
+  /*
+   * @brief Initialize required ROS transformations
+   */
+  void initTransforms();
+  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  bool sent_first_transform_{false};
+  bool latest_tf_valid_{false};
+  tf2::Transform latest_tf_;
+
+  // Message filters
+  /*
+   * @brief Initialize incoming data message subscribers and filters
+   */
+  void initMessageFilters();
+  std::unique_ptr<message_filters::Subscriber<sensor_msgs::msg::LaserScan,
+    rclcpp_lifecycle::LifecycleNode>> laser_scan_sub_;
+  std::unique_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan>> laser_scan_filter_;
+  message_filters::Connection laser_scan_connection_;
+
+  // Publishers and subscribers
+  /*
+   * @brief Initialize pub subs of AMCL
+   */
+  void initPubSub();
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::ConstSharedPtr
+    initial_pose_sub_;
+  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr
+    pose_pub_;
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::ParticleCloud>::SharedPtr
+    particle_cloud_pub_;
+  /*
+   * @brief Handle with an initial pose estimate is received
+   */
+  void initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
+  /*
+   * @brief Handle when a laser scan is received
+   */
+  void laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan);
+
+  // Services and service callbacks
+  /*
+   * @brief Initialize state services
+   */
+  void initServices();
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr global_loc_srv_;
+  /*
+   * @brief Service callback for a global relocalization request
+   */
+  void globalLocalizationCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+    std::shared_ptr<std_srvs::srv::Empty::Response> response);
+
+  // service server for providing an initial pose guess
+  rclcpp::Service<nav2_msgs::srv::SetInitialPose>::SharedPtr initial_guess_srv_;
+  /*
+   * @brief Service callback for an initial pose guess request
+   */
+  void initialPoseReceivedSrv(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<nav2_msgs::srv::SetInitialPose::Request> request,
+    std::shared_ptr<nav2_msgs::srv::SetInitialPose::Response> response);
+
+  // Let amcl update samples without requiring motion
+  rclcpp::Service<std_srvs::srv::Empty>::SharedPtr nomotion_update_srv_;
+  /*
+   * @brief Request an AMCL update even though the robot hasn't moved
+   */
+  void nomotionUpdateCallback(
+    const std::shared_ptr<rmw_request_id_t> request_header,
+    const std::shared_ptr<std_srvs::srv::Empty::Request> request,
+    std::shared_ptr<std_srvs::srv::Empty::Response> response);
+
+  // Nomotion update control. Used to temporarily let amcl update samples even when no motion occurs
+  std::atomic<bool> force_update_{false};
+
+  // Odometry
+  /*
+   * @brief Initialize odometry
+   */
+  void initOdometry();
+  std::shared_ptr<nav2_amcl::MotionModel> motion_model_;
+  geometry_msgs::msg::PoseStamped latest_odom_pose_;
+  geometry_msgs::msg::PoseWithCovarianceStamped last_published_pose_;
+  double init_pose_[3];  // Initial robot pose
+  double init_cov_[3];
+  pluginlib::ClassLoader<nav2_amcl::MotionModel> plugin_loader_{"nav2_amcl",
+    "nav2_amcl::MotionModel"};
+  /*
+   * @brief Get robot pose in odom frame using TF
+   */
+  bool getOdomPose(
+    // Helper to get odometric pose from transform system
+    geometry_msgs::msg::PoseStamped & pose,
+    double & x, double & y, double & yaw,
+    const rclcpp::Time & sensor_timestamp, const std::string & frame_id);
+  std::atomic<bool> first_pose_sent_;
+
+  // Particle filter
+  /*
+   * @brief Initialize particle filter
+   */
+  void initParticleFilter();
+  /*
+   * @brief Pose-generating function used to uniformly distribute particles over the map
+   */
+  static pf_vector_t uniformPoseGenerator(void * arg);
+  pf_t * pf_{nullptr};
+  bool pf_init_;
+  pf_vector_t pf_odom_pose_;
+  int resample_count_{0};
+
+  // Laser scan related
+  /*
+   * @brief Initialize laser scan
+   */
+  void initLaserScan();
+  /*
+   * @brief Create a laser object
+   */
+  nav2_amcl::Laser * createLaserObject();
+  int scan_error_count_{0};
+  std::vector<nav2_amcl::Laser *> lasers_;
+  std::vector<bool> lasers_update_;
+  std::map<std::string, int> frame_to_laser_;
+  rclcpp::Time last_laser_received_ts_;
+
+  /*
+   * @brief Check if sufficient time has elapsed to get an update
+   */
+  bool checkElapsedTime(std::chrono::seconds check_interval, rclcpp::Time last_time);
+  rclcpp::Time last_time_printed_msg_;
+  /*
+   * @brief Add a new laser scanner if a new one is received in the laser scallbacks
+   */
+  bool addNewScanner(
+    int & laser_index,
+    const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
+    const std::string & laser_scan_frame_id,
+    geometry_msgs::msg::PoseStamped & laser_pose);
+  /*
+   * @brief Whether the pf needs to be updated
+   */
+  bool shouldUpdateFilter(const pf_vector_t pose, pf_vector_t & delta);
+  /*
+   * @brief Update the PF
+   */
+  bool updateFilter(
+    const int & laser_index,
+    const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
+    const pf_vector_t & pose);
+  /*
+   * @brief Publish particle cloud
+   */
+  void publishParticleCloud(const pf_sample_set_t * set);
+  /*
+   * @brief Get the current state estimat hypothesis from the particle cloud
+   */
+  bool getMaxWeightHyp(
+    std::vector<amcl_hyp_t> & hyps, amcl_hyp_t & max_weight_hyps,
+    int & max_weight_hyp);
+  /*
+   * @brief Publish robot pose in map frame from AMCL
+   */
+  void publishAmclPose(
+    const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
+    const std::vector<amcl_hyp_t> & hyps, const int & max_weight_hyp);
+  /*
+   * @brief Determine TF transformation from map to odom
+   */
+  void calculateMaptoOdomTransform(
+    const sensor_msgs::msg::LaserScan::ConstSharedPtr & laser_scan,
+    const std::vector<amcl_hyp_t> & hyps,
+    const int & max_weight_hyp);
+  /*
+   * @brief Publish TF transformation from map to odom
+   */
+  void sendMapToOdomTransform(const tf2::TimePoint & transform_expiration);
+  /*
+   * @brief Handle a new pose estimate callback
+   */
+  void handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg);
+  bool init_pose_received_on_inactive{false};
+  bool initial_pose_is_known_{false};
+  bool set_initial_pose_{false};
+  bool always_reset_initial_pose_;
+  double initial_pose_x_;
+  double initial_pose_y_;
+  double initial_pose_z_;
+  double initial_pose_yaw_;
+
+  /*
+   * @brief Get ROS parameters for node
+   */
+  void initParameters();
+  double alpha1_;
+  double alpha2_;
+  double alpha3_;
+  double alpha4_;
+  double alpha5_;
+  std::string base_frame_id_;
+  double beam_skip_distance_;
+  double beam_skip_error_threshold_;
+  double beam_skip_threshold_;
+  bool do_beamskip_;
+  std::string global_frame_id_;
+  double lambda_short_;
+  double laser_likelihood_max_dist_;
+  double laser_max_range_;
+  double laser_min_range_;
+  std::string sensor_model_type_;
+  int max_beams_;
+  int max_particles_;
+  int min_particles_;
+  std::string odom_frame_id_;
+  double pf_err_;
+  double pf_z_;
+  double alpha_fast_;
+  double alpha_slow_;
+  int resample_interval_;
+  std::string robot_model_type_;
+  tf2::Duration save_pose_period_;
+  double sigma_hit_;
+  bool tf_broadcast_;
+  tf2::Duration transform_tolerance_;
+  double a_thresh_;
+  double d_thresh_;
+  double z_hit_;
+  double z_max_;
+  double z_short_;
+  double z_rand_;
+  std::string scan_topic_{"scan"};
+  std::string map_topic_{"map"};
+};
+
+}  // namespace nav2_amcl
+
+#endif  // NAV2_AMCL__AMCL_NODE_HPP_

navigation2/nav2_amcl/include/nav2_amcl/angleutils.hpp

@@ -0,0 +1,78 @@
+/*
+ *  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
+ *
+ */
+
+#ifndef NAV2_AMCL__ANGLEUTILS_HPP_
+#define NAV2_AMCL__ANGLEUTILS_HPP_
+
+#include <math.h>
+
+namespace nav2_amcl
+{
+
+/*
+ * @class angleutils
+ * @brief Some utilities for working with angles
+ */
+class angleutils
+{
+public:
+  /*
+   * @brief Normalize angles
+   * @brief z Angle to normalize
+   * @return normalized angle
+   */
+  static double normalize(double z);
+
+  /*
+   * @brief Find minimum distance between 2 angles
+   * @brief a Angle 1
+   * @brief b Angle 2
+   * @return normalized angle difference
+   */
+  static double angle_diff(double a, double b);
+};
+
+inline double
+angleutils::normalize(double z)
+{
+  return atan2(sin(z), cos(z));
+}
+
+inline double
+angleutils::angle_diff(double a, double b)
+{
+  a = normalize(a);
+  b = normalize(b);
+  double d1 = a - b;
+  double d2 = 2 * M_PI - fabs(d1);
+  if (d1 > 0) {
+    d2 *= -1.0;
+  }
+  if (fabs(d1) < fabs(d2)) {
+    return d1;
+  } else {
+    return d2;
+  }
+}
+
+}  // namespace nav2_amcl
+
+#endif  // NAV2_AMCL__ANGLEUTILS_HPP_

navigation2/nav2_amcl/include/nav2_amcl/map/map.hpp

@@ -0,0 +1,138 @@
+/*
+ *  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.h 1713 2003-08-23 04:03:43Z inspectorg $
+ **************************************************************************/
+
+#ifndef NAV2_AMCL__MAP__MAP_HPP_
+#define NAV2_AMCL__MAP__MAP_HPP_
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Forward declarations
+struct _rtk_fig_t;
+
+
+// Limits
+#define MAP_WIFI_MAX_LEVELS 8
+
+
+// Description for a single map cell.
+typedef struct
+{
+  // Occupancy state (-1 = free, 0 = unknown, +1 = occ)
+  int occ_state;
+
+  // Distance to the nearest occupied cell
+  double occ_dist;
+
+  // Wifi levels
+  // int wifi_levels[MAP_WIFI_MAX_LEVELS];
+} map_cell_t;
+
+
+// Description for a map
+typedef struct
+{
+  // Map origin; the map is a viewport onto a conceptual larger map.
+  double origin_x, origin_y;
+
+  // Map scale (m/cell)
+  double scale;
+
+  // Map dimensions (number of cells)
+  int size_x, size_y;
+
+  // The map data, stored as a grid
+  map_cell_t * cells;
+
+  // Max distance at which we care about obstacles, for constructing
+  // likelihood field
+  double max_occ_dist;
+} map_t;
+
+
+/**************************************************************************
+ * Basic map functions
+ **************************************************************************/
+
+// Create a new (empty) map
+map_t * map_alloc(void);
+
+// Destroy a map
+void map_free(map_t * map);
+
+// Update the cspace distances
+void map_update_cspace(map_t * map, double max_occ_dist);
+
+
+/**************************************************************************
+ * Range functions
+ **************************************************************************/
+
+// Extract a single range reading from the map
+double map_calc_range(map_t * map, double ox, double oy, double oa, double max_range);
+
+
+/**************************************************************************
+ * GUI/diagnostic functions
+ **************************************************************************/
+
+// Draw the occupancy grid
+void map_draw_occ(map_t * map, struct _rtk_fig_t * fig);
+
+// Draw the cspace map
+void map_draw_cspace(map_t * map, struct _rtk_fig_t * fig);
+
+// Draw a wifi map
+void map_draw_wifi(map_t * map, struct _rtk_fig_t * fig, int index);
+
+
+/**************************************************************************
+ * Map manipulation macros
+ **************************************************************************/
+
+// Convert from map index to world coords
+#define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale)
+#define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale)
+
+// Convert from world coords to map coords
+#define MAP_GXWX(map, x) (floor((x - map->origin_x) / map->scale + 0.5) + map->size_x / 2)
+#define MAP_GYWY(map, y) (floor((y - map->origin_y) / map->scale + 0.5) + map->size_y / 2)
+
+// Test to see if the given map coords lie within the absolute map bounds.
+#define MAP_VALID(map, i, j) ((i >= 0) && (i < map->size_x) && (j >= 0) && (j < map->size_y))
+
+// Compute the cell index for the given map coords.
+#define MAP_INDEX(map, i, j) ((i) + (j) * map->size_x)
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // NAV2_AMCL__MAP__MAP_HPP_

navigation2/nav2_amcl/include/nav2_amcl/motion_model/differential_motion_model.hpp

@@ -0,0 +1,47 @@
+/*
+ *  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
+ *
+ */
+
+#ifndef NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
+#define NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_
+
+#include <sys/types.h>
+#include <math.h>
+#include <algorithm>
+#include "nav2_amcl/motion_model/motion_model.hpp"
+#include "nav2_amcl/angleutils.hpp"
+
+
+namespace nav2_amcl
+{
+
+class DifferentialMotionModel : public nav2_amcl::MotionModel
+{
+public:
+  virtual void initialize(
+    double alpha1, double alpha2, double alpha3, double alpha4,
+    double alpha5);
+  virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta);
+
+private:
+  double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
+};
+}  // namespace nav2_amcl
+#endif  // NAV2_AMCL__MOTION_MODEL__DIFFERENTIAL_MOTION_MODEL_HPP_

navigation2/nav2_amcl/include/nav2_amcl/motion_model/motion_model.hpp

@@ -0,0 +1,62 @@
+// 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
+
+#ifndef NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
+#define NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_
+
+#include <string>
+#include <memory>
+
+#include "nav2_amcl/pf/pf.hpp"
+#include "nav2_amcl/pf/pf_pdf.hpp"
+
+namespace nav2_amcl
+{
+
+/**
+ * @class nav2_amcl::MotionModel
+ * @brief An abstract motion model class
+ */
+class MotionModel
+{
+public:
+  virtual ~MotionModel() = default;
+
+  /**
+   * @brief An factory to create motion models
+   * @param type Type of motion model to create in factory
+   * @param alpha1 error parameters, see documentation
+   * @param alpha2 error parameters, see documentation
+   * @param alpha3 error parameters, see documentation
+   * @param alpha4 error parameters, see documentation
+   * @param alpha5 error parameters, see documentation
+   * @return MotionModel A pointer to the motion model it created
+   */
+  virtual void initialize(
+    double alpha1, double alpha2, double alpha3, double alpha4,
+    double alpha5) = 0;
+
+  /**
+   * @brief Update on new odometry data
+   * @param pf The particle filter to update
+   * @param pose pose of robot in odometry update
+   * @param delta change in pose in odometry update
+   */
+  virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta) = 0;
+};
+}  // namespace nav2_amcl
+
+#endif  // NAV2_AMCL__MOTION_MODEL__MOTION_MODEL_HPP_

navigation2/nav2_amcl/include/nav2_amcl/motion_model/omni_motion_model.hpp

@@ -0,0 +1,47 @@
+/*
+ *  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
+ *
+ */
+
+#ifndef NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
+#define NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_
+
+#include <sys/types.h>
+#include <math.h>
+#include <algorithm>
+#include "nav2_amcl/motion_model/motion_model.hpp"
+#include "nav2_amcl/angleutils.hpp"
+
+
+namespace nav2_amcl
+{
+
+class OmniMotionModel : public nav2_amcl::MotionModel
+{
+public:
+  virtual void initialize(
+    double alpha1, double alpha2, double alpha3, double alpha4,
+    double alpha5);
+  virtual void odometryUpdate(pf_t * pf, const pf_vector_t & pose, const pf_vector_t & delta);
+
+private:
+  double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
+};
+}  // namespace nav2_amcl
+#endif  // NAV2_AMCL__MOTION_MODEL__OMNI_MOTION_MODEL_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/eig3.hpp

@@ -0,0 +1,31 @@
+/*
+ *  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 for symmetric 3x3 real matrices.
+   Public domain, copied from the public domain Java library JAMA. */
+
+#ifndef NAV2_AMCL__PF__EIG3_HPP_
+#define NAV2_AMCL__PF__EIG3_HPP_
+
+/* Symmetric matrix A => eigenvectors in columns of V, corresponding
+   eigenvalues in d. */
+void eigen_decomposition(double A[3][3], double V[3][3], double d[3]);
+
+#endif  // NAV2_AMCL__PF__EIG3_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/pf.hpp

@@ -0,0 +1,200 @@
+/*
+ *  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.h 3293 2005-11-19 08:37:45Z gerkey $
+ *************************************************************************/
+
+#ifndef NAV2_AMCL__PF__PF_HPP_
+#define NAV2_AMCL__PF__PF_HPP_
+
+#include "nav2_amcl/pf/pf_vector.hpp"
+#include "nav2_amcl/pf/pf_kdtree.hpp"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Forward declarations
+struct _pf_t;
+struct _rtk_fig_t;
+struct _pf_sample_set_t;
+
+// Function prototype for the initialization model; generates a sample pose from
+// an appropriate distribution.
+typedef pf_vector_t (* pf_init_model_fn_t) (void * init_data);
+
+// Function prototype for the action model; generates a sample pose from
+// an appropriate distribution
+typedef void (* pf_action_model_fn_t) (
+  void * action_data,
+  struct _pf_sample_set_t * set);
+
+// Function prototype for the sensor model; determines the probability
+// for the given set of sample poses.
+typedef double (* pf_sensor_model_fn_t) (
+  void * sensor_data,
+  struct _pf_sample_set_t * set);
+
+
+// Information for a single sample
+typedef struct
+{
+  // Pose represented by this sample
+  pf_vector_t pose;
+
+  // Weight for this pose
+  double weight;
+} pf_sample_t;
+
+
+// Information for a cluster of samples
+typedef struct
+{
+  // Number of samples
+  int count;
+
+  // Total weight of samples in this cluster
+  double weight;
+
+  // Cluster statistics
+  pf_vector_t mean;
+  pf_matrix_t cov;
+
+  // Workspace
+  double m[4], c[2][2];
+} pf_cluster_t;
+
+
+// Information for a set of samples
+typedef struct _pf_sample_set_t
+{
+  // The samples
+  int sample_count;
+  pf_sample_t * samples;
+
+  // A kdtree encoding the histogram
+  pf_kdtree_t * kdtree;
+
+  // Clusters
+  int cluster_count, cluster_max_count;
+  pf_cluster_t * clusters;
+
+  // Filter statistics
+  pf_vector_t mean;
+  pf_matrix_t cov;
+  int converged;
+} pf_sample_set_t;
+
+
+// Information for an entire filter
+typedef struct _pf_t
+{
+  // This min and max number of samples
+  int min_samples, max_samples;
+
+  // Population size parameters
+  double pop_err, pop_z;
+
+  // The sample sets.  We keep two sets and use [current_set]
+  // to identify the active set.
+  int current_set;
+  pf_sample_set_t sets[2];
+
+  // Running averages, slow and fast, of likelihood
+  double w_slow, w_fast;
+
+  // Decay rates for running averages
+  double alpha_slow, alpha_fast;
+
+  // Function used to draw random pose samples
+  pf_init_model_fn_t random_pose_fn;
+
+  double dist_threshold;  // distance threshold in each axis over which the pf is considered to not
+                          // be converged
+  int converged;
+} pf_t;
+
+
+// Create a new 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);
+
+// Free an existing filter
+void pf_free(pf_t * pf);
+
+// Initialize the filter using a guassian
+void pf_init(pf_t * pf, pf_vector_t mean, pf_matrix_t cov);
+
+// Initialize the filter using some model
+void pf_init_model(pf_t * pf, pf_init_model_fn_t init_fn, void * init_data);
+
+// Update the filter with some new action
+// void pf_update_action(pf_t * pf, pf_action_model_fn_t action_fn, void * action_data);
+
+// 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);
+
+// Resample the distribution
+void pf_update_resample(pf_t * pf, void * random_pose_data);
+
+// Compute the CEP statistics (mean and variance).
+// void pf_get_cep_stats(pf_t * pf, pf_vector_t * mean, double * var);
+
+// Compute the statistics for a particular cluster.  Returns 0 if
+// there is no such cluster.
+int pf_get_cluster_stats(
+  pf_t * pf, int cluster, double * weight,
+  pf_vector_t * mean, pf_matrix_t * cov);
+
+// Re-compute the cluster statistics for a sample set
+void pf_cluster_stats(pf_t * pf, pf_sample_set_t * set);
+
+
+// Display the sample set
+void pf_draw_samples(pf_t * pf, struct _rtk_fig_t * fig, int max_samples);
+
+// Draw the histogram (kdtree)
+void pf_draw_hist(pf_t * pf, struct _rtk_fig_t * fig);
+
+// Draw the CEP statistics
+// void pf_draw_cep_stats(pf_t * pf, struct _rtk_fig_t * fig);
+
+// Draw the cluster statistics
+void pf_draw_cluster_stats(pf_t * pf, struct _rtk_fig_t * fig);
+
+// calculate if the particle filter has converged -
+// and sets the converged flag in the current set and the pf
+int pf_update_converged(pf_t * pf);
+
+// sets the current set and pf converged values to zero
+void pf_init_converged(pf_t * pf);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif  // NAV2_AMCL__PF__PF_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/pf_kdtree.hpp

@@ -0,0 +1,107 @@
+/*
+ *  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.h 6532 2008-06-11 02:45:56Z gbiggs $
+ *************************************************************************/
+
+#ifndef NAV2_AMCL__PF__PF_KDTREE_HPP_
+#define NAV2_AMCL__PF__PF_KDTREE_HPP_
+
+#ifdef INCLUDE_RTKGUI
+#include <rtk.h>
+#endif
+
+
+// Info for a node in the tree
+typedef struct pf_kdtree_node
+{
+  // Depth in the tree
+  int leaf, depth;
+
+  // Pivot dimension and value
+  int pivot_dim;
+  double pivot_value;
+
+  // The key for this node
+  int key[3];
+
+  // The value for this node
+  double value;
+
+  // The cluster label (leaf nodes)
+  int cluster;
+
+  // Child nodes
+  struct pf_kdtree_node * children[2];
+} pf_kdtree_node_t;
+
+
+// A kd tree
+typedef struct
+{
+  // Cell size
+  double size[3];
+
+  // The root node of the tree
+  pf_kdtree_node_t * root;
+
+  // The number of nodes in the tree
+  int node_count, node_max_count;
+  pf_kdtree_node_t * nodes;
+
+  // The number of leaf nodes in the tree
+  int leaf_count;
+} pf_kdtree_t;
+
+
+// Create a tree
+extern pf_kdtree_t * pf_kdtree_alloc(int max_size);
+
+// Destroy a tree
+extern void pf_kdtree_free(pf_kdtree_t * self);
+
+// Clear all entries from the tree
+extern void pf_kdtree_clear(pf_kdtree_t * self);
+
+// Insert a pose into the tree
+extern void pf_kdtree_insert(pf_kdtree_t * self, pf_vector_t pose, double value);
+
+// Cluster the leaves in the tree
+extern void pf_kdtree_cluster(pf_kdtree_t * self);
+
+// Determine the probability estimate for the given pose
+// extern double pf_kdtree_get_prob(pf_kdtree_t * self, pf_vector_t pose);
+
+// Determine the cluster label for the given pose
+extern int pf_kdtree_get_cluster(pf_kdtree_t * self, pf_vector_t pose);
+
+
+#ifdef INCLUDE_RTKGUI
+
+// Draw the tree
+extern void pf_kdtree_draw(pf_kdtree_t * self, rtk_fig_t * fig);
+
+#endif
+
+#endif  // NAV2_AMCL__PF__PF_KDTREE_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/pf_pdf.hpp

@@ -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: Useful pdf functions
+ * Author: Andrew Howard
+ * Date: 10 Dec 2002
+ * CVS: $Id: pf_pdf.h 6345 2008-04-17 01:36:39Z gerkey $
+ *************************************************************************/
+
+#ifndef NAV2_AMCL__PF__PF_PDF_HPP_
+#define NAV2_AMCL__PF__PF_PDF_HPP_
+
+#include "nav2_amcl/pf/pf_vector.hpp"
+
+// #include <gsl/gsl_rng.h>
+// #include <gsl/gsl_randist.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**************************************************************************
+ * Gaussian
+ *************************************************************************/
+
+// Gaussian PDF info
+typedef struct
+{
+  // Mean, covariance and inverse covariance
+  pf_vector_t x;
+  pf_matrix_t cx;
+  // pf_matrix_t cxi;
+  double cxdet;
+
+  // Decomposed covariance matrix (rotation * diagonal)
+  pf_matrix_t cr;
+  pf_vector_t cd;
+
+  // A random number generator
+  // gsl_rng *rng;
+} pf_pdf_gaussian_t;
+
+
+// Create a gaussian pdf
+pf_pdf_gaussian_t * pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx);
+
+// Destroy the pdf
+void pf_pdf_gaussian_free(pf_pdf_gaussian_t * pdf);
+
+// Compute the value of the pdf at some point [z].
+// double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z);
+
+// 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);
+
+// Generate a sample from the pdf.
+pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t * pdf);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // NAV2_AMCL__PF__PF_PDF_HPP_

navigation2/nav2_amcl/include/nav2_amcl/pf/pf_vector.hpp

@@ -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
+ *
+ */
+/**************************************************************************
+ * Desc: Vector functions
+ * Author: Andrew Howard
+ * Date: 10 Dec 2002
+ * CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $
+ *************************************************************************/
+
+#ifndef NAV2_AMCL__PF__PF_VECTOR_HPP_
+#define NAV2_AMCL__PF__PF_VECTOR_HPP_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdio.h>
+
+// The basic vector
+typedef struct
+{
+  double v[3];
+} pf_vector_t;
+
+
+// The basic matrix
+typedef struct
+{
+  double m[3][3];
+} pf_matrix_t;
+
+
+// Return a zero vector
+pf_vector_t pf_vector_zero(void);
+
+// Check for NAN or INF in any component
+// int pf_vector_finite(pf_vector_t a);
+
+// Print a vector
+// void pf_vector_fprintf(pf_vector_t s, FILE * file, const char * fmt);
+
+// Simple vector addition
+// pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b);
+
+// Simple vector subtraction
+pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b);
+
+// Transform from local to global coords (a + b)
+pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);
+
+// Transform from global to local coords (a - b)
+// pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b);
+
+
+// Return a zero matrix
+pf_matrix_t pf_matrix_zero(void);
+
+// Check for NAN or INF in any component
+// int pf_matrix_finite(pf_matrix_t a);
+
+// Print a matrix
+// void pf_matrix_fprintf(pf_matrix_t s, FILE * file, const char * fmt);
+
+// Compute the matrix inverse.  Will also return the determinant,
+// which should be checked for underflow (indicated singular matrix).
+// pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det);
+
+// Decompose a covariance matrix [a] into a rotation matrix [r] and a
+// diagonal matrix [d] such that a = r * d * r^T.
+void pf_matrix_unitary(pf_matrix_t * r, pf_matrix_t * d, pf_matrix_t a);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // NAV2_AMCL__PF__PF_VECTOR_HPP_

navigation2/nav2_amcl/include/nav2_amcl/portable_utils.hpp

@@ -0,0 +1,41 @@
+// Copyright (c) Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef NAV2_AMCL__PORTABLE_UTILS_HPP_
+#define NAV2_AMCL__PORTABLE_UTILS_HPP_
+
+#include <stdlib.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef HAVE_DRAND48
+// Some system (e.g., Windows) doesn't come with drand48(), srand48().
+// Use rand, and srand for such system.
+static double drand48(void)
+{
+  return ((double)rand()) / RAND_MAX;// NOLINT
+}
+
+static void srand48(long int seedval)// NOLINT
+{
+  srand(seedval);
+}
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // NAV2_AMCL__PORTABLE_UTILS_HPP_

navigation2/nav2_amcl/include/nav2_amcl/sensors/laser/laser.hpp

@@ -0,0 +1,210 @@
+// 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
+
+
+#ifndef NAV2_AMCL__SENSORS__LASER__LASER_HPP_
+#define NAV2_AMCL__SENSORS__LASER__LASER_HPP_
+
+#include <string>
+#include "nav2_amcl/pf/pf.hpp"
+#include "nav2_amcl/pf/pf_pdf.hpp"
+#include "nav2_amcl/map/map.hpp"
+
+namespace nav2_amcl
+{
+
+// Forward declarations
+class LaserData;
+
+/*
+ * @class Laser
+ * @brief Base class for laser sensor models
+ */
+class Laser
+{
+public:
+  /**
+   * @brief A Laser constructor
+   * @param max_beams number of beams to use
+   * @param map Map pointer to use
+   */
+  Laser(size_t max_beams, map_t * map);
+
+  /*
+   * @brief Laser destructor
+   */
+  virtual ~Laser();
+
+  /*
+   * @brief Run a sensor update on laser
+   * @param pf Particle filter to use
+   * @param data Laser data to use
+   * @return if it was succesful
+   */
+  virtual bool sensorUpdate(pf_t * pf, LaserData * data) = 0;
+
+  /*
+   * @brief Set the laser pose from an update
+   * @param laser_pose Pose of the laser
+   */
+  void SetLaserPose(pf_vector_t & laser_pose);
+
+protected:
+  double z_hit_;
+  double z_rand_;
+  double sigma_hit_;
+
+  /*
+   * @brief Reallocate weights
+   * @param max_samples Max number of samples
+   * @param max_obs number of observations
+   */
+  void reallocTempData(int max_samples, int max_obs);
+  map_t * map_;
+  pf_vector_t laser_pose_;
+  int max_beams_;
+  int max_samples_;
+  int max_obs_;
+  double ** temp_obs_;
+};
+
+/*
+ * @class LaserData
+ * @brief Class of laser data to process
+ */
+class LaserData
+{
+public:
+  Laser * laser;
+
+  /*
+   * @brief LaserData constructor
+   */
+  LaserData() {ranges = NULL;}
+  /*
+   * @brief LaserData destructor
+   */
+  virtual ~LaserData() {delete[] ranges;}
+
+public:
+  int range_count;
+  double range_max;
+  double(*ranges)[2];
+};
+
+/*
+ * @class BeamModel
+ * @brief Beam model laser sensor
+ */
+class BeamModel : public Laser
+{
+public:
+  /*
+   * @brief BeamModel constructor
+   */
+  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);
+
+  /*
+   * @brief Run a sensor update on laser
+   * @param pf Particle filter to use
+   * @param data Laser data to use
+   * @return if it was succesful
+   */
+  bool sensorUpdate(pf_t * pf, LaserData * data);
+
+private:
+  static double sensorFunction(LaserData * data, pf_sample_set_t * set);
+  double z_short_;
+  double z_max_;
+  double lambda_short_;
+  double chi_outlier_;
+};
+
+/*
+ * @class LikelihoodFieldModel
+ * @brief likelihood field model laser sensor
+ */
+class LikelihoodFieldModel : public Laser
+{
+public:
+  /*
+   * @brief BeamModel constructor
+   */
+  LikelihoodFieldModel(
+    double z_hit, double z_rand, double sigma_hit, double max_occ_dist,
+    size_t max_beams, map_t * map);
+
+  /*
+   * @brief Run a sensor update on laser
+   * @param pf Particle filter to use
+   * @param data Laser data to use
+   * @return if it was succesful
+   */
+  bool sensorUpdate(pf_t * pf, LaserData * data);
+
+private:
+  /*
+   * @brief Perform the update function
+   * @param data Laser data to use
+   * @param pf Particle filter to use
+   * @return if it was succesful
+   */
+  static double sensorFunction(LaserData * data, pf_sample_set_t * set);
+};
+
+/*
+ * @class LikelihoodFieldModelProb
+ * @brief likelihood prob model laser sensor
+ */
+class LikelihoodFieldModelProb : public Laser
+{
+public:
+  /*
+   * @brief BeamModel constructor
+   */
+  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);
+
+  /*
+   * @brief Run a sensor update on laser
+   * @param pf Particle filter to use
+   * @param data Laser data to use
+   * @return if it was succesful
+   */
+  bool sensorUpdate(pf_t * pf, LaserData * data);
+
+private:
+  /*
+   * @brief Perform the update function
+   * @param data Laser data to use
+   * @param pf Particle filter to use
+   * @return if it was succesful
+   */
+  static double sensorFunction(LaserData * data, pf_sample_set_t * set);
+  bool do_beamskip_;
+  double beam_skip_distance_;
+  double beam_skip_threshold_;
+  double beam_skip_error_threshold_;
+};
+
+}  // namespace nav2_amcl
+
+#endif  // NAV2_AMCL__SENSORS__LASER__LASER_HPP_

navigation2/nav2_amcl/package.xml

@@ -0,0 +1,45 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nav2_amcl</name>
+  <version>1.1.18</version>
+  <description>
+    <p>
+      amcl is a probabilistic localization system for a robot moving in
+      2D. It implements the adaptive (or KLD-sampling) Monte Carlo
+      localization approach (as described by Dieter Fox), which uses a
+      particle filter to track the pose of a robot against a known map.
+    </p>
+    <p>
+      This node is derived, with thanks, from Andrew Howard's excellent
+      'amcl' Player driver.
+    </p>
+  </description>
+  <!-- <author>Brian P. Gerkey</author>
+  <author>contradict@gmail.com</author> -->
+  <maintainer email="mohammad.haghighipanah@intel.com">Mohammad Haghighipanah</maintainer>
+  <license>LGPL-2.1-or-later</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>nav2_common</build_depend>
+  <depend>rclcpp</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>message_filters</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_msgs</depend>
+  <depend>launch_ros</depend>
+  <depend>launch_testing</depend>
+  <depend>pluginlib</depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

navigation2/nav2_amcl/plugins.xml

@@ -0,0 +1,8 @@
+<library path="motions_lib">
+  <class type="nav2_amcl::DifferentialMotionModel" base_class_type="nav2_amcl::MotionModel">
+    <description>This is a diff plugin.</description>
+  </class>
+  <class type="nav2_amcl::OmniMotionModel" base_class_type="nav2_amcl::MotionModel">
+    <description>This is a omni plugin.</description>
+  </class>
+</library>

navigation2/nav2_amcl/src/main.cpp

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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

@@ -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