#include <mrpt/utils/CTicTac.h>#include <mrpt/gui.h>#include <mrpt/base.h>#include <mrpt/opengl.h>#include <GL/gl.h>#include "CMyEllipsoid.h"#include <ros/ros.h>#include <rosbag/bag.h>#include <rosbag/view.h>#include <tf/transform_listener.h>#include <boost/foreach.hpp>#include <sensor_msgs/LaserScan.h>#include <message_filters/subscriber.h>#include <message_filters/sync_policies/approximate_time.h>#include <nav_msgs/Odometry.h>#include <nav_msgs/OccupancyGrid.h>#include <ndt_map.h>#include <tf/transform_broadcaster.h>#include "tfMessageReader.h"#include "ndt_mcl.hpp"#include "mcl_visualization.hpp"
Go to the source code of this file.
Defines | |
| #define | USE_VISUALIZATION_DEBUG |
| Enable / Disable visualization. | |
Functions | |
| void | callback (const sensor_msgs::LaserScan &scan, tf::Transform odo_pose, tf::Transform state_pose) |
| Eigen::Affine3d | getAsAffine (float x, float y, float yaw) |
| < here is a punch of visualization code based on the MRPT's GUI components | |
| int | main (int argc, char **argv) |
Variables | |
| FILE * | flog = fopen("ndtmcl_result.txt","wt") |
| static bool | has_sensor_offset_set = false |
| static bool | isFirstLoad = true |
| NDTMCL< pcl::PointXYZ > * | ndtmcl |
| float | offa = 0 |
| float | offx = 0 |
| Laser sensor offset. | |
| float | offy = 0 |
| std::string | tf_laser_link = "base_laser_link" |
| std::string | tf_odo_topic = "odom_base_link" |
| std::string | tf_state_topic = "base_link" |
| Eigen::Affine3d | Todo |
| Eigen::Affine3d | Told |
| mrpt::utils::CTicTac | TT |
Define Documentation
| #define USE_VISUALIZATION_DEBUG |
Enable / Disable visualization.
2D NDT-MCL for offline processing (reads directly the bag files). This application runs the ndt-mcl localization based on a map and laser scanner and odometry.
This is based now on our sample file (see data folder). The sample file also has ground truth pose information, which we use now for the initialization (replace with manual input if you will).
The visualization depends on mrpt-gui
More details about the algorithm: Jari Saarinen, Henrik Andreasson, Todor Stoyanov and Achim J. Lilienthal, Normal Distributions Transform Monte-Carlo Localization (NDT-MCL) IEEE/RSJ International Conference on Intelligent Robots and Systems November 3-8, 2013, Tokyo Big Sight, Japan
Known issues: in launch file (or hard coded parameters) you have to set the same resolution for NDT map as is saved -- otherwise it wont work
Definition at line 20 of file 2d_ndt_mcl_offline_test.cpp.
Function Documentation
| void callback | ( | const sensor_msgs::LaserScan & | scan, |
| tf::Transform | odo_pose, | ||
| tf::Transform | state_pose | ||
| ) |
This is where the localization is done... not really callback in the offline version
Get the ground truth
Get the odometry
< number of scan lines
Pose conversions
We are now using the information from the ground truth to initialize the filter
Initialize the filter with 1m^2 variance in position and 20deg in heading
Compute the differential motion between two time steps
Get the laser pose with respect to the base
Laser pose in base frame
Laser scan to PointCloud transformed with respect to the base
Now we have the differential motion and pointcloud -- Lets do MCL
< does the prediction, update and resampling steps (see ndt_mcl.hpp)
<Maximum aposteriori estimate of the pose
distribution variances ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
The sensor pose in the global frame for visualization
Definition at line 99 of file 2d_ndt_mcl_offline_test.cpp.
| Eigen::Affine3d getAsAffine | ( | float | x, |
| float | y, | ||
| float | yaw | ||
| ) |
< here is a punch of visualization code based on the MRPT's GUI components
Convert x,y,yaw to Eigen::Affine3d
Definition at line 54 of file 2d_ndt_mcl_offline_test.cpp.
| int main | ( | int | argc, |
| char ** | argv | ||
| ) |
Parameters for the mapper
< flag to indicate that we want to load a map
<name and the path to the map
< indicates if we should make the map over confident (stationary that is)
< indicates if we want to save the map in a regular intervals
Set the values from a config file
Prepare the map
Open the bag file for reading
Set sensor offsets
Loop while we have data in the bag
If we have the data then lets run the localization
Definition at line 226 of file 2d_ndt_mcl_offline_test.cpp.
Variable Documentation
| FILE* flog = fopen("ndtmcl_result.txt","wt") |
Definition at line 84 of file 2d_ndt_mcl_offline_test.cpp.
bool has_sensor_offset_set = false [static] |
Definition at line 81 of file 2d_ndt_mcl_offline_test.cpp.
bool isFirstLoad = true [static] |
Definition at line 82 of file 2d_ndt_mcl_offline_test.cpp.
| float offa = 0 |
Definition at line 79 of file 2d_ndt_mcl_offline_test.cpp.
| float offx = 0 |
Laser sensor offset.
Definition at line 77 of file 2d_ndt_mcl_offline_test.cpp.
| float offy = 0 |
Definition at line 78 of file 2d_ndt_mcl_offline_test.cpp.
| std::string tf_laser_link = "base_laser_link" |
Definition at line 94 of file 2d_ndt_mcl_offline_test.cpp.
| std::string tf_odo_topic = "odom_base_link" |
Definition at line 92 of file 2d_ndt_mcl_offline_test.cpp.
| std::string tf_state_topic = "base_link" |
Definition at line 93 of file 2d_ndt_mcl_offline_test.cpp.
| Eigen::Affine3d Todo |
Definition at line 83 of file 2d_ndt_mcl_offline_test.cpp.
| Eigen::Affine3d Told |
Definition at line 83 of file 2d_ndt_mcl_offline_test.cpp.
| mrpt::utils::CTicTac TT |
Definition at line 90 of file 2d_ndt_mcl_offline_test.cpp.