Defines | Functions | Variables
2d_ndt_mcl_offline_test.cpp File Reference
#include <mrpt/utils/CTicTac.h>
#include <mrpt/gui.h>
#include <mrpt/base.h>
#include <mrpt/opengl.h>
#include <GL/gl.h>
#include "ndt_mcl/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 <tf/transform_broadcaster.h>
#include <ndt_map/ndt_map.h>
#include "ndt_mcl/tfMessageReader.h"
#include "ndt_mcl/impl/ndt_mcl.hpp"
#include "ndt_mcl/impl/mcl_visualization.hpp"
Include dependency graph for 2d_ndt_mcl_offline_test.cpp:

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

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

Author:
Jari Saarinen (jari.p.saarinen@gmail.com) here is a punch of visualization code based on the MRPT's GUI components

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 100 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 55 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 227 of file 2d_ndt_mcl_offline_test.cpp.


Variable Documentation

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

Definition at line 85 of file 2d_ndt_mcl_offline_test.cpp.

bool has_sensor_offset_set = false [static]

Definition at line 82 of file 2d_ndt_mcl_offline_test.cpp.

bool isFirstLoad = true [static]

Definition at line 83 of file 2d_ndt_mcl_offline_test.cpp.

Update measurement

Globals

Definition at line 75 of file 2d_ndt_mcl_offline_test.cpp.

float offa = 0

Definition at line 80 of file 2d_ndt_mcl_offline_test.cpp.

float offx = 0

Laser sensor offset.

Definition at line 78 of file 2d_ndt_mcl_offline_test.cpp.

float offy = 0

Definition at line 79 of file 2d_ndt_mcl_offline_test.cpp.

std::string tf_laser_link = "base_laser_link"

Definition at line 95 of file 2d_ndt_mcl_offline_test.cpp.

std::string tf_odo_topic = "odom_base_link"

Definition at line 93 of file 2d_ndt_mcl_offline_test.cpp.

std::string tf_state_topic = "base_link"

Definition at line 94 of file 2d_ndt_mcl_offline_test.cpp.

Eigen::Affine3d Todo

Definition at line 84 of file 2d_ndt_mcl_offline_test.cpp.

Eigen::Affine3d Told

Definition at line 84 of file 2d_ndt_mcl_offline_test.cpp.

mrpt::utils::CTicTac TT

Definition at line 91 of file 2d_ndt_mcl_offline_test.cpp.



ndt_mcl
Author(s): Jari Saarinen
autogenerated on Mon Oct 6 2014 03:20:03