Defines | Functions | Variables
2d_ndt_mcl_offline_test.cpp File Reference
#include <ndt_visualisation/ndt_viz.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 <tf/transform_broadcaster.h>
#include <ndt_map/ndt_map.h>
#include "ndt_mcl/tfMessageReader.h"
#include "ndt_mcl/ndt_mcl.h"
Include dependency graph for 2d_ndt_mcl_offline_test.cpp:

Go to the source code of this file.


 Enable / Disable visualization.


void callback (const sensor_msgs::LaserScan &scan, tf::Transform odo_pose, tf::Transform state_pose)
Eigen::Affine3d getAsAffine (float x, float y, float yaw)
double getDoubleTime ()
int main (int argc, char **argv)


FILE * flog = fopen("ndtmcl_result.txt","wt")
static bool has_sensor_offset_set = false
static bool isFirstLoad = true
NDTViz ndt_viz
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"
double time_end
Eigen::Affine3d Todo
Eigen::Affine3d Told

Define Documentation

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

Jari Saarinen (

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

Definition at line 97 of file 2d_ndt_mcl_offline_test.cpp.

Eigen::Affine3d getAsAffine ( float  x,
float  y,
float  yaw 

Convert x,y,yaw to Eigen::Affine3d

Definition at line 47 of file 2d_ndt_mcl_offline_test.cpp.

double getDoubleTime ( )

Definition at line 57 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 210 of file 2d_ndt_mcl_offline_test.cpp.

Variable Documentation

FILE* flog = fopen("ndtmcl_result.txt","wt")

Definition at line 83 of file 2d_ndt_mcl_offline_test.cpp.

bool has_sensor_offset_set = false [static]

Definition at line 80 of file 2d_ndt_mcl_offline_test.cpp.

bool isFirstLoad = true [static]

Definition at line 81 of file 2d_ndt_mcl_offline_test.cpp.

Definition at line 24 of file 2d_ndt_mcl_offline_test.cpp.

Update measurement


Definition at line 73 of file 2d_ndt_mcl_offline_test.cpp.

float offa = 0

Definition at line 78 of file 2d_ndt_mcl_offline_test.cpp.

float offx = 0

Laser sensor offset.

Definition at line 76 of file 2d_ndt_mcl_offline_test.cpp.

float offy = 0

Definition at line 77 of file 2d_ndt_mcl_offline_test.cpp.

std::string tf_laser_link = "base_laser_link"

Definition at line 92 of file 2d_ndt_mcl_offline_test.cpp.

std::string tf_odo_topic = "odom_base_link"

Definition at line 90 of file 2d_ndt_mcl_offline_test.cpp.

std::string tf_state_topic = "base_link"

Definition at line 91 of file 2d_ndt_mcl_offline_test.cpp.

double time_end

Definition at line 89 of file 2d_ndt_mcl_offline_test.cpp.

Eigen::Affine3d Todo

Definition at line 82 of file 2d_ndt_mcl_offline_test.cpp.

Eigen::Affine3d Told

Definition at line 82 of file 2d_ndt_mcl_offline_test.cpp.

Author(s): Jari Saarinen
autogenerated on Wed Aug 26 2015 15:25:02