diff --git a/config/VoxelGridTracking.rviz b/config/VoxelGridTracking.rviz
index 04395fc..ccaff20 100644
--- a/config/VoxelGridTracking.rviz
+++ b/config/VoxelGridTracking.rviz
@@ -27,29 +27,30 @@ Panels:
         - /voxel_grid_tracking1/SpeedText1
         - /voxel_grid_tracking1/fakeParticles1
         - /voxel_grid_tracking1/fakePointCloud1
-        - /StereoAndOdom1
+        - /voxel_grid_tracking1/TrackingDbg1
+        - /voxel_grid_tracking1/TrackingDbg1/Namespaces1
         - /StereoAndOdom1/InputCloudStereo1
         - /StereoAndOdom1/LeftImg1
         - /StereoAndOdom1/RightImg1
         - /StereoAndOdom1/Velodyne1
         - /dynamic_obstacles1/FreeCells1
         - /dynamic_obstacles1/Octomap1
+        - /Evaluation1
         - /Evaluation1/ImageGT1
         - /Evaluation1/ImageRT1
         - /Evaluation1/dbgROIs1
+        - /Evaluation1/PointCloud21
         - /DemoLZ1/Undistorted1/Right1
         - /DemoLZ1/Undistorted1/Left1
         - /DemoLZ1/Rectified1/Left_rectified1
-        - /Navigation1
         - /Navigation1/Footprint1
         - /Navigation1/global_costmap1
         - /Navigation1/Temptativetrajs1
         - /Navigation1/global_plan1
         - /Navigation1/local_costmap1
         - /Navigation1/VerdinoModel1
-        - /WinnerPath1
       Splitter Ratio: 0.298137
-    Tree Height: 341
+    Tree Height: 363
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -69,7 +70,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: InputCloudStereo
+    SyncSource: ObstaclesPointCloud
 Visualization Manager:
   Class: ""
   Displays:
@@ -272,7 +273,7 @@ Visualization Manager:
             {}
           Queue Size: 100
           Value: false
-        - Alpha: 1
+        - Alpha: 0.25
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
             Max Value: 10
@@ -427,10 +428,10 @@ Visualization Manager:
         - Arrow Length: 0.3
           Class: rviz/PoseArray
           Color: 255; 0; 0
-          Enabled: true
+          Enabled: false
           Name: fakeParticles
           Topic: /voxel_grid_tracking/voxel_grid_tracking/fakeParticles
-          Value: true
+          Value: false
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
@@ -460,6 +461,14 @@ Visualization Manager:
           Use Fixed Frame: true
           Use rainbow: true
           Value: false
+        - Class: rviz/MarkerArray
+          Enabled: true
+          Marker Topic: /voxel_grid_tracking/voxel_grid_tracking/tracking
+          Name: TrackingDbg
+          Namespaces:
+            {}
+          Queue Size: 100
+          Value: true
       Enabled: true
       Name: voxel_grid_tracking
     - Class: rviz/Group
@@ -494,7 +503,7 @@ Visualization Manager:
           Use rainbow: true
           Value: true
         - Class: rviz/Image
-          Enabled: false
+          Enabled: true
           Image Topic: /stereo/left/image_rect_color
           Max Value: 1
           Median window: 5
@@ -503,9 +512,9 @@ Visualization Manager:
           Normalize Range: true
           Queue Size: 2
           Transport Hint: compressed
-          Value: false
+          Value: true
         - Class: rviz/Image
-          Enabled: false
+          Enabled: true
           Image Topic: /stereo/right/image_rect_color
           Max Value: 1
           Median window: 5
@@ -514,7 +523,7 @@ Visualization Manager:
           Normalize Range: true
           Queue Size: 2
           Transport Hint: raw
-          Value: false
+          Value: true
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
@@ -628,7 +637,13 @@ Visualization Manager:
           Class: rviz/Path
           Color: 25; 255; 0
           Enabled: true
+          Line Style: Lines
+          Line Width: 0.03
           Name: globalPlan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
           Topic: /verdino/move_base/TrajectoryPlannerROS/global_plan
           Value: true
         - Alpha: 1
@@ -810,7 +825,7 @@ Visualization Manager:
           Channel Name: intensity
           Class: rviz/PointCloud2
           Color: 255; 0; 0
-          Color Transformer: FlatColor
+          Color Transformer: RGB8
           Decay Time: 0
           Enabled: true
           Invert Rainbow: false
@@ -825,7 +840,7 @@ Visualization Manager:
           Size (Pixels): 3
           Size (m): 0.5
           Style: Spheres
-          Topic: /voxel_grid_tracking/dbgROIs
+          Topic: /voxel_grid_tracking/voxel_grid_tracking/debug_roi
           Use Fixed Frame: true
           Use rainbow: true
           Value: true
@@ -837,7 +852,36 @@ Visualization Manager:
             {}
           Queue Size: 100
           Value: false
-      Enabled: false
+        - Alpha: 1
+          Autocompute Intensity Bounds: true
+          Autocompute Value Bounds:
+            Max Value: 10
+            Min Value: -10
+            Value: true
+          Axis: Z
+          Channel Name: intensity
+          Class: rviz/PointCloud2
+          Color: 255; 255; 255
+          Color Transformer: RGB8
+          Decay Time: 0
+          Enabled: true
+          Invert Rainbow: false
+          Max Color: 255; 255; 255
+          Max Intensity: 4096
+          Min Color: 0; 0; 0
+          Min Intensity: 0
+          Name: PointCloud2
+          Position Transformer: XYZ
+          Queue Size: 10
+          Selectable: true
+          Size (Pixels): 3
+          Size (m): 0.5
+          Style: Squares
+          Topic: /voxel_grid_tracking/voxel_grid_tracking/debug_roi_transformed
+          Use Fixed Frame: true
+          Use rainbow: true
+          Value: true
+      Enabled: true
       Name: Evaluation
     - Class: rviz/Group
       Displays:
@@ -1031,7 +1075,13 @@ Visualization Manager:
           Class: rviz/Path
           Color: 25; 255; 0
           Enabled: true
+          Line Style: Lines
+          Line Width: 0.03
           Name: global_plan
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
           Topic: /verdino/move_base/GrullNavfnROS/plan
           Value: true
         - Alpha: 0.7
@@ -1113,22 +1163,22 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 8.06214
+      Distance: 11.2351
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 2.24123
-        Y: 1.10835
-        Z: -0.403067
+        X: 6.7955
+        Y: -0.637684
+        Z: -0.403069
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.265204
+      Pitch: 0.605204
       Target Frame: left_cam
       Value: VideoBahnhoffAbove
-      Yaw: 3.14173
+      Yaw: 3.22674
     Saved:
       - Class: rviz/XYOrbit
         Distance: 20.0678
@@ -1251,9 +1301,9 @@ Visualization Manager:
         Yaw: 3.14173
 Window Geometry:
   Displays:
-    collapsed: true
-  Height: 748
-  Hide Left Dock: true
+    collapsed: false
+  Height: 713
+  Hide Left Dock: false
   Hide Right Dock: true
   ImageGT:
     collapsed: false
@@ -1267,7 +1317,7 @@ Window Geometry:
     collapsed: false
   Left_rectified:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Right:
     collapsed: false
   RightImg:
@@ -1281,7 +1331,7 @@ Window Geometry:
   Time:
     collapsed: false
   Tool Properties:
-    collapsed: true
+    collapsed: false
   Views:
     collapsed: true
   Width: 1366
diff --git a/config/VoxelGridTrackingVerdino.rviz b/config/VoxelGridTrackingVerdino.rviz
index 894a22a..ea0eea9 100644
--- a/config/VoxelGridTrackingVerdino.rviz
+++ b/config/VoxelGridTrackingVerdino.rviz
@@ -32,6 +32,7 @@ Panels:
         - /StereoAndOdom1/LeftImg1
         - /StereoAndOdom1/RightImg1
         - /StereoAndOdom1/Velodyne1
+        - /StereoAndOdom1/Odometry1
         - /dynamic_obstacles1/FreeCells1
         - /dynamic_obstacles1/Octomap1
         - /Evaluation1/ImageGT1
@@ -49,7 +50,7 @@ Panels:
         - /Navigation1/VerdinoModel1
         - /WinnerPath1
       Splitter Ratio: 0.298137
-    Tree Height: 341
+    Tree Height: 465
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -68,7 +69,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: InputCloudStereo
+    SyncSource: LeftImg
 Visualization Manager:
   Class: ""
   Displays:
@@ -76,7 +77,7 @@ Visualization Manager:
       Cell Size: 1
       Class: rviz/Grid
       Color: 160; 160; 164
-      Enabled: false
+      Enabled: true
       Line Style:
         Line Width: 0.03
         Value: Lines
@@ -89,7 +90,7 @@ Visualization Manager:
       Plane: XY
       Plane Cell Count: 60
       Reference Frame: map
-      Value: false
+      Value: true
     - Class: rviz/TF
       Enabled: true
       Frame Timeout: 15
@@ -285,12 +286,12 @@ Visualization Manager:
           Channel Name: intensity
           Class: rviz/PointCloud2
           Color: 255; 255; 255
-          Color Transformer: Intensity
+          Color Transformer: RGB8
           Decay Time: 0
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 0.68
+          Max Intensity: 0.65
           Min Color: 0; 0; 0
           Min Intensity: 0
           Name: ObstaclesPointCloud
@@ -300,7 +301,7 @@ Visualization Manager:
           Size (Pixels): 3
           Size (m): 0.01
           Style: Points
-          Topic: /verdino/obstaclesPointCloud
+          Topic: /verdino/elas_ros/point_cloud
           Use Fixed Frame: true
           Use rainbow: true
           Value: true
@@ -470,8 +471,8 @@ Visualization Manager:
         - Alpha: 0.1
           Autocompute Intensity Bounds: false
           Autocompute Value Bounds:
-            Max Value: 4.319
-            Min Value: -8.926
+            Max Value: 4.331
+            Min Value: -10.104
             Value: true
           Axis: Z
           Channel Name: intensity
@@ -552,7 +553,7 @@ Visualization Manager:
           Axes Radius: 0.1
           Class: rviz/Pose
           Color: 255; 25; 0
-          Enabled: false
+          Enabled: true
           Head Length: 0.3
           Head Radius: 0.1
           Name: Pose
@@ -560,17 +561,17 @@ Visualization Manager:
           Shaft Radius: 0.05
           Shape: Arrow
           Topic: /stereo_odometer/pose
-          Value: false
+          Value: true
         - Angle Tolerance: 0.1
           Class: rviz/Odometry
           Color: 255; 25; 0
-          Enabled: false
+          Enabled: true
           Keep: 100
           Length: 1
           Name: Odometry
           Position Tolerance: 0.1
-          Topic: /stereo_odometer/odometry
-          Value: false
+          Topic: /verdino/stereo_odometer/odometry
+          Value: true
         - Alpha: 1
           Autocompute Intensity Bounds: true
           Autocompute Value Bounds:
@@ -1128,22 +1129,22 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/XYOrbit
-      Distance: 8.06278
+      Distance: 5.80711
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.06
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -3.1571
-        Y: -0.806058
-        Z: 1.04693
+        X: 0.508564
+        Y: -0.912588
+        Z: 1.11883
       Name: Current View
       Near Clip Distance: 0.01
-      Pitch: 0.419797
-      Target Frame: base_footprint
+      Pitch: 0.160204
+      Target Frame: map
       Value: VideoBahnhoffAbove
-      Yaw: 3.14992
+      Yaw: 3.30174
     Saved:
       - Class: rviz/XYOrbit
         Distance: 20.0678
@@ -1333,7 +1334,7 @@ Window Geometry:
     collapsed: false
   Left_rectified:
     collapsed: false
-  QMainWindow State: 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
+  QMainWindow State: 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
   Right:
     collapsed: false
   RightImg:
diff --git a/launch/VoxelGridTrackingResults.launch b/launch/VoxelGridTrackingResults.launch
index e59c2c1..bedb9e4 100644
--- a/launch/VoxelGridTrackingResults.launch
+++ b/launch/VoxelGridTrackingResults.launch
@@ -1,9 +1,9 @@
 <launch>
     <arg name="namespace" default="voxel_grid_tracking"/>
     <arg name="test_name" default="eth"/>
-    <arg name="use_elas" default="false"/>
-    <arg name="use_rsgm" default="true"/>
-    <arg name="do_evaluation" default="false"/>
+    <arg name="use_elas" default="true"/>
+    <arg name="use_rsgm" default="false"/>
+    <arg name="do_evaluation" default="true"/>
     <arg name="evaluation_path" default="/tmp/test"/>
     <arg name="evaluation_header" default="This is a header"/>
     <arg name="voxel_tracking_params_file" 
@@ -24,7 +24,7 @@
     <node pkg="tf" type="static_transform_publisher" name="baselink2baseleftcam" args="0.27 0 1.45 0 0 -1.57079632679489661923 base_link base_left_cam 100" />
     <node pkg="tf" type="static_transform_publisher" name="leftcam2baseleftcam" args="0 0 0 0 0 0 base_left_cam left_cam 100" />
     <!-- <node pkg="tf" type="static_transform_publisher" name="baselink2baseleftcam" args="0.27 0 1.65 0 0 -1.57 base_link base_left_cam 100" /> -->
-    <!-- <node pkg="tf" type="static_transform_publisher" name="baseleftcam2leftcam" args="0 -0.4 0 0 0 1.57 base_left_cam left_cam 100" /> -->
+<!--     <node pkg="tf" type="static_transform_publisher" name="baseleftcam2leftcam" args="0 -0.4 0 0 0 1.57 base_left_cam left_cam 100" /> -->
     
 <!--    <node pkg="octomap_server" type="octomap_server_node" name="octomap_server" output="screen" required="true">
         <remap from="cloud_in" to="/$(arg namespace)/pointCloudFilteredAndTransformed" />
diff --git a/launch/launch.launch b/launch/launch.launch
index 68b5550..1c51f2e 100644
--- a/launch/launch.launch
+++ b/launch/launch.launch
@@ -85,7 +85,7 @@
             <param name="filter_limit_max" value="1000.0" />
             <param name="filter_limit_negative" value="false" />
             <param name="input_frame" value="base_footprint" />
-            <param name="output_frame" value="odom" />
+            <param name="output_frame" value="map" />
         </node>
     </group>
     
diff --git a/launch/voxel_tracking_sequence_reader.launch b/launch/voxel_tracking_sequence_reader.launch
index 4f724b5..efaae27 100644
--- a/launch/voxel_tracking_sequence_reader.launch
+++ b/launch/voxel_tracking_sequence_reader.launch
@@ -12,17 +12,17 @@
     
     <include file="$(find sequence_reader)/launch/launch.launch">
         <arg name="namespace" value="$(arg namespace)"/>
-        <arg name="wait_time_sequence_reader" default="200"/>
+        <arg name="wait_time_sequence_reader" default="0"/>
         <arg name="use_rviz" value="false"/>
-        <arg name="use_elas" value="false"/>
+        <arg name="use_elas" value="true"/>
         <arg name="use_rsgm" value="false"/>
         
         <arg name="static_transforms" value="true" />
         <arg name="static_odom2basefootprint" value="false" />
-        <arg name="visual_odom" value="false" />
+        <arg name="visual_odom" value="true" />
         
         <arg name="sequence_file" 
-             value="$(find sequence_reader)/params/kitti_sequence_campus.yaml" />
+             value="$(find sequence_reader)/params/bahnhoffstrasse.yaml" />
     </include>
     
     <include file="$(find voxel_grid_tracking)/launch/launch.launch">
diff --git a/msg/roi_and_speed_2d.msg b/msg/roi_and_speed_2d.msg
index 71c2473..f4efb10 100644
--- a/msg/roi_and_speed_2d.msg
+++ b/msg/roi_and_speed_2d.msg
@@ -16,4 +16,6 @@ point_2d H
 
 point_2d center
 
+point_2d[] track
+
 Vector2 speed
\ No newline at end of file
diff --git a/msg/roi_and_speed_3d.msg b/msg/roi_and_speed_3d.msg
index b759a8c..4a5a60a 100644
--- a/msg/roi_and_speed_3d.msg
+++ b/msg/roi_and_speed_3d.msg
@@ -18,4 +18,6 @@ geometry_msgs/Point32 center
 
 geometry_msgs/Vector3 speed
 
+geometry_msgs/Point32[] track
+
 float32 speed_magnitude
\ No newline at end of file
diff --git a/msg/voxel_tracker_time_stats.msg b/msg/voxel_tracker_time_stats.msg
index 3e13730..17b6abc 100644
--- a/msg/voxel_tracker_time_stats.msg
+++ b/msg/voxel_tracker_time_stats.msg
@@ -9,6 +9,7 @@ float64 prediction
 float64 measurementBasedUpdate
 float64 segment
 float64 updateSpeedFromObstacles
+float64 updateTracks
 float64 initialization
 float64 totalCompute
 float64 totalVisualization
\ No newline at end of file
diff --git a/params/voxel_tracking_default_params.yaml b/params/voxel_tracking_default_params.yaml
index f3a32d6..57be7b6 100644
--- a/params/voxel_tracking_default_params.yaml
+++ b/params/voxel_tracking_default_params.yaml
@@ -44,6 +44,9 @@ l1_distance_for_neighbor_thresh_x: 1
 l1_distance_for_neighbor_thresh_y: 1
 l1_distance_for_neighbor_thresh_z: 1
 
+# Publish debug visualization
+publish_intermediate_info: false
+
 # Method used to compute the speed of obstacles
 #speed_method_mean, speed_method_circ_hist
 obstacle_speed_method: "speed_method_circ_hist"
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b13a853..b8a5c5f 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,48 +1,48 @@
-# 
-# #################################################################
-# # stereo_and_odom
-# #################################################################
-# add_executable(stereo_and_odom
-# #     obstacle.cpp 
-# #     polarcell.cpp 
-# #     polargridtrackingros.cpp
-#     ObstaclesFromStereo.cpp
-#     sequence_reader/calibrationmanager.cpp
-#     sequence_reader/groundtruthmanager.cpp
-# #     polargridtracking.cpp
-# #     particle.cpp
-# #     cell.cpp
-# #     utils.cpp
-# #     libvisohelper.cpp
-# #     ${VISO_FILES}
-# #     ${POLAR_CALIBRATION_CPP_FILES}
-#     ${LIBELAS_SRC_FILES}
-#     mainStereoAndOdom.cpp
-# )
-# 
-# include_directories(stereo_and_odom
-# #     ${POLAR_CALIBRATION_INCLUDE_DIRS}
-#     ${PCL_INCLUDE_DIRS}/pcl-1.7
-#     ${OpenCV_INCLUDE_DIR}
-#     ${LIBELAS_INCLUDE_DIRS}
-#     ${Boost_INCLUDE_DIR}
-#     /opt/ros/indigo/include
-# )
-# 
-# link_directories(stereo_and_odom
-#     /usr/local/lib
-#     /usr/lib/x86_64-linux-gnu/
-# )
-# 
-# target_link_libraries(stereo_and_odom
-#   ${EIGEN3_LIBRARIES}
-#   ${PCL_LIBRARIES}
-#   ${OpenCV_LIBS}
-#   ${Boost_LIBRARIES}
-#   ${catkin_LIBRARIES}
-# )
-# 
-# add_dependencies(stereo_and_odom ${PROJECT_NAME}_generate_messages_cpp)
+
+#################################################################
+# stereo_and_odom
+#################################################################
+add_executable(stereo_and_odom
+#     obstacle.cpp 
+#     polarcell.cpp 
+#     polargridtrackingros.cpp
+    ObstaclesFromStereo.cpp
+    sequence_reader/calibrationmanager.cpp
+    sequence_reader/groundtruthmanager.cpp
+#     polargridtracking.cpp
+#     particle.cpp
+#     cell.cpp
+#     utils.cpp
+#     libvisohelper.cpp
+#     ${VISO_FILES}
+#     ${POLAR_CALIBRATION_CPP_FILES}
+    ${LIBELAS_SRC_FILES}
+    mainStereoAndOdom.cpp
+)
+
+include_directories(stereo_and_odom
+#     ${POLAR_CALIBRATION_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}/pcl-1.7
+    ${OpenCV_INCLUDE_DIR}
+    ${LIBELAS_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIR}
+    /opt/ros/indigo/include
+)
+
+link_directories(stereo_and_odom
+    /usr/local/lib
+    /usr/lib/x86_64-linux-gnu/
+)
+
+target_link_libraries(stereo_and_odom
+  ${EIGEN3_LIBRARIES}
+  ${PCL_LIBRARIES}
+  ${OpenCV_LIBS}
+  ${Boost_LIBRARIES}
+  ${catkin_LIBRARIES}
+)
+
+add_dependencies(stereo_and_odom ${PROJECT_NAME}_generate_messages_cpp)
 
 set(VISO_FILES 
     libviso2/filter.cpp
diff --git a/src/mainStereoAndOdom.cpp b/src/mainStereoAndOdom.cpp
index c5823b9..6b05e7d 100644
--- a/src/mainStereoAndOdom.cpp
+++ b/src/mainStereoAndOdom.cpp
@@ -220,8 +220,8 @@ void testStereoTracking() {
     
     GroundTruthManager gtManager;
     
-    const ObstaclesFromStereo::t_CalibrationFileType calibrationType = ObstaclesFromStereo::KARLSRUHE_V2;
-//     const ObstaclesFromStereo::t_CalibrationFileType calibrationTZype = ObstaclesFromStereo::BAHNHOFSTRASSE;
+//     const ObstaclesFromStereo::t_CalibrationFileType calibrationType = ObstaclesFromStereo::KARLSRUHE_V2;
+    const ObstaclesFromStereo::t_CalibrationFileType calibrationType = ObstaclesFromStereo::BAHNHOFSTRASSE;
 //     const ObstaclesFromStereo::t_CalibrationFileType calibrationType = ObstaclesFromStereo::FISHEYE;
     
     ros::NodeHandle nh("~");
@@ -321,10 +321,16 @@ void testStereoTracking() {
         {
             initialIdx = 1; //60; //295; //1; 295; //55;
             lastIdx = 1000;
+            
             correspondencesPath = boost::filesystem::path("/local/imaged/stixels");
             seqName = boost::filesystem::path("bahnhof");
             leftImagePattern = "seq03-img-left/image_%08d_0.png";
             rightImagePattern = "seq03-img-right/image_%08d_1.png";
+
+//             correspondencesPath = boost::filesystem::path("/media/nestor/data/research_data/sequences");
+//             seqName = boost::filesystem::path("jelmoli");
+//             leftImagePattern = "seq04-img-left/image_%08d_0.png";
+//             rightImagePattern = "seq04-img-right/image_%08d_1.png";
             
             ObstaclesFromStereo::getParams("/local/imaged/stixels/bahnhof", cameraParams, ObstaclesFromStereo::BAHNHOFSTRASSE);
             
@@ -625,10 +631,11 @@ void testStereoTracking() {
         // Publish point cloud
         uint8_t keycode;
 //         if (i < 15)
-//             keycode = cv::waitKey(0);
+            keycode = cv::waitKey(0);
 //         else
         cout << "deltaTime " << deltaTime << endl;
 //             keycode = cv::waitKey((uint32_t)(deltaTime * 2000));
+//         waitTime=0;
         keycode = cv::waitKey(waitTime);
         if (keycode == 27) {
             break;
diff --git a/src/utilspolargridtracking.cpp b/src/utilspolargridtracking.cpp
index c7c6499..d49fb9b 100644
--- a/src/utilspolargridtracking.cpp
+++ b/src/utilspolargridtracking.cpp
@@ -92,10 +92,10 @@ void project3dTo2d(const pcl::PointXYZRGB& point3d, pcl::PointXYZRGB& point2d,
     cv::Mat R(stereoCameraModel.right().rotationMatrix());
     
     tf::Vector3 point = map2CamTransform * tf::Vector3(point3d.x, point3d.y, point3d.z);
-    
+
     cv::Mat pointMat = (cv::Mat_<double>(3,1) << point[0], point[1], point[2]);
     pointMat = R.inv() * pointMat;
-    
+
     const double d = focalX * baseline / pointMat.at<double>(2, 0);
     const double u = centerX + ((pointMat.at<double>(0, 0) * d) / baseline);
     const double v = centerY + ((pointMat.at<double>(1, 0) * focalY * d) / (focalX * baseline));
diff --git a/src/voxelgridtracking.cpp b/src/voxelgridtracking.cpp
index 3c85581..9ffb473 100644
--- a/src/voxelgridtracking.cpp
+++ b/src/voxelgridtracking.cpp
@@ -306,6 +306,9 @@ VoxelGridTracking::VoxelGridTracking()
     m_segmentedPointCloudPub = nh.advertise<sensor_msgs::PointCloud2> ("segmentedPointCloud", 1);
     m_debugSegmentPub = nh.advertise<sensor_msgs::PointCloud2> ("debugSegment", 1);
     m_debugProbPub = nh.advertise<sensor_msgs::PointCloud2> ("debugProbPub", 1);
+    m_debugUpdateTracks = nh.advertise<visualization_msgs::MarkerArray>("tracking", 1);
+    m_debugROIs = nh.advertise<sensor_msgs::PointCloud2>("debug_roi", 1);
+    m_debugROIs2 = nh.advertise<sensor_msgs::PointCloud2>("debug_roi_transformed", 1);
 //     ros::spin();
 }
 
@@ -350,7 +353,7 @@ void VoxelGridTracking::pointCloudCallback(const sensor_msgs::PointCloud2::Const
                                            const sensor_msgs::CameraInfoConstPtr& leftCameraInfo, 
                                            const sensor_msgs::CameraInfoConstPtr& rightCameraInfo) 
 {
-    m_cameraFrame = msgPointCloud->header.frame_id;
+    m_cameraFrame = leftCameraInfo->header.frame_id;
     try {
         m_tfListener.lookupTransform(m_mapFrame, m_poseFrame, ros::Time(0), m_pose2MapTransform);
         m_tfListener.lookupTransform(m_cameraFrame, m_mapFrame, ros::Time(0), m_map2CamTransform);
@@ -379,7 +382,7 @@ void VoxelGridTracking::pointCloudCallback(const sensor_msgs::PointCloud2::Const
                                            const sensor_msgs::CameraInfoConstPtr& leftCameraInfo, 
                                            const sensor_msgs::CameraInfoConstPtr& rightCameraInfo) 
 {
-    m_cameraFrame = msgPointCloud->header.frame_id;
+    m_cameraFrame = leftCameraInfo->header.frame_id;
     try {
         m_tfListener.lookupTransform(m_mapFrame, m_poseFrame, ros::Time(0), m_pose2MapTransform);
     } catch (tf::TransformException ex){
@@ -482,13 +485,19 @@ void VoxelGridTracking::compute(const PointCloudPtr& pointCloud)
         END_CLOCK(totalCompute8, startCompute8)
         ROS_INFO("[%s] %d, updateSpeedFromObstacles: %f seconds", __FUNCTION__, __LINE__, totalCompute8);
         timeStatsMsg.updateSpeedFromObstacles = totalCompute8;
-        INIT_CLOCK(startCompute2)
+        
+        INIT_CLOCK(startCompute9)
+        updateTracks();
+        END_CLOCK(totalCompute9, startCompute9)
+        ROS_INFO("[%s] %d, updateTracks: %f seconds", __FUNCTION__, __LINE__, totalCompute9);
+        timeStatsMsg.updateTracks = totalCompute9;
+        
     }
-    INIT_CLOCK(startCompute9)
+    INIT_CLOCK(startCompute10)
     initialization();
-    END_CLOCK(totalCompute9, startCompute9)
-    ROS_INFO("[%s] %d, initialization: %f seconds", __FUNCTION__, __LINE__, totalCompute9);
-    timeStatsMsg.initialization = totalCompute9;
+    END_CLOCK(totalCompute10, startCompute10)
+    ROS_INFO("[%s] %d, initialization: %f seconds", __FUNCTION__, __LINE__, totalCompute10);
+    timeStatsMsg.initialization = totalCompute10;
 // END OF COMMENT
     
     
@@ -518,11 +527,12 @@ void VoxelGridTracking::compute(const PointCloudPtr& pointCloud)
 //         cout << __FILE__ << ":" << __LINE__ << endl;
         publishObstacleCubes();
 //         cout << __FILE__ << ":" << __LINE__ << endl;
-        
-        publishROI();
-//         visualizeROI2d();
+        publishTracks();
     }
     END_CLOCK(totalVis, startVis)
+//     visualizeROI2d();
+//     visualizeROI3d();
+    publishROI();
     
     publishFakePointCloud();
 //     cout << __FILE__ << ":" << __LINE__ << endl;
@@ -1364,6 +1374,29 @@ void VoxelGridTracking::filterObstacles()
     }
 }
 
+void VoxelGridTracking::updateTracks()
+{
+    if (m_prevObstacles.size() != 0) {
+        
+        BOOST_FOREACH(const VoxelObstaclePtr & obstacle, m_obstacles) {
+            pcl::PointXYZ o;
+            o.x = obstacle->centerX() - obstacle->vx() * obstacle->magnitude() * m_deltaTime;
+            o.y = obstacle->centerY() - obstacle->vy() * obstacle->magnitude() * m_deltaTime;
+            o.z = obstacle->centerZ() - obstacle->vz() * obstacle->magnitude() * m_deltaTime;
+            
+            BOOST_FOREACH(const VoxelObstaclePtr & lastObstacle, m_prevObstacles) {
+                // TODO: Select that closer to the center
+                if (lastObstacle->isInObstacle(o)) {
+                    obstacle->addTrackFromObstacle(lastObstacle);
+                } else {
+                    obstacle->startTrack();
+                }
+            }
+        }        
+    }
+    m_prevObstacles = m_obstacles;
+}
+
 void VoxelGridTracking::publishVoxels()
 {
     visualization_msgs::MarkerArray voxelMarkers;
@@ -2073,6 +2106,15 @@ void VoxelGridTracking::publishROI()
 
             obstacle->getROI(m_stereoCameraModel, m_map2CamTransform,
                              roi2D, roi3D);
+            
+            BOOST_FOREACH(const pcl::PointXYZ & p, *(obstacle->getTrack())) {
+                geometry_msgs::Point32 center;
+                center.x = p.x;
+                center.y = p.y;
+                center.z = p.z;
+                
+                roi3D.track.push_back(center);
+            }
 
             roiMsg.rois3d[i] = roi3D;
             roiMsg.rois2d[i] = roi2D;
@@ -2118,21 +2160,22 @@ void VoxelGridTracking::visualizeROI2d()
         pointBR.x = max(roi2D.D.u, roi2D.H.u);
         pointBR.y = max(roi2D.D.v, roi2D.H.v);
 
-        const float MAX_SPEED = 20.0f;
-        float speedX = roi2D.speed.x;
-        float speedY = roi2D.speed.y;
-        cv::Scalar color;
-        if ((speedX == 0.0f) && (speedY == 0.0f)) {
-            color = cv::Scalar::all(0);
-        } else {
-            if (speedX > MAX_SPEED) speedX = MAX_SPEED;
-            if (speedX < -MAX_SPEED) speedX = -MAX_SPEED;
-            if (speedY > MAX_SPEED) speedY = MAX_SPEED;
-            if (speedY < -MAX_SPEED) speedY = -MAX_SPEED;
-            color = cv::Scalar(speedX / MAX_SPEED * 128 + 128, 
-                                speedY / MAX_SPEED * 128 + 128, 
-                                128);
-        }
+//         const float MAX_SPEED = 20.0f;
+//         float speedX = roi2D.speed.x;
+//         float speedY = roi2D.speed.y;
+//         cv::Scalar color;
+//         if ((speedX == 0.0f) && (speedY == 0.0f)) {
+//             color = cv::Scalar::all(0);
+//         } else {
+//             if (speedX > MAX_SPEED) speedX = MAX_SPEED;
+//             if (speedX < -MAX_SPEED) speedX = -MAX_SPEED;
+//             if (speedY > MAX_SPEED) speedY = MAX_SPEED;
+//             if (speedY < -MAX_SPEED) speedY = -MAX_SPEED;
+//             color = cv::Scalar(speedX / MAX_SPEED * 128 + 128, 
+//                                 speedY / MAX_SPEED * 128 + 128, 
+//                                 128);
+//         }
+        cv::Scalar color = cv::Scalar::all(255);
         cv::rectangle(img, pointUL, pointBR, color, 2);
     }
     
@@ -2141,6 +2184,163 @@ void VoxelGridTracking::visualizeROI2d()
     cv::waitKey(200);
 }
 
+void VoxelGridTracking::visualizeROI3d()
+{
+    PointCloudPtr pointCloud(new PointCloud);
+    PointCloudPtr pointCloudTransformed(new PointCloud);
+    
+    const double & tx = m_stereoCameraModel.right().projectionMatrix()(0, 3);
+    const double & ty = m_stereoCameraModel.right().projectionMatrix()(1, 3);
+    const double & tz = m_stereoCameraModel.right().projectionMatrix()(2, 3);
+    cv::Mat R(m_stereoCameraModel.right().rotationMatrix());
+    tf::Vector3 point;
+    
+    cv::Mat pointMat;
+    
+    for (uint32_t i = 0; i < m_obstacles.size(); i++) {
+        const VoxelObstaclePtr & obstacle = m_obstacles[i];
+                
+        voxel_grid_tracking::roi_and_speed_2d roi2D;
+        voxel_grid_tracking::roi_and_speed_3d roi3D;
+        
+        obstacle->getROI(m_stereoCameraModel, m_map2CamTransform,
+                         roi2D, roi3D);
+        
+        PointType p;
+        p.r = m_obstacleColors[i % MAX_OBSTACLES_VISUALIZATION][0] * 255.0;
+        p.g = m_obstacleColors[i % MAX_OBSTACLES_VISUALIZATION][1] * 255.0;
+        p.b = m_obstacleColors[i % MAX_OBSTACLES_VISUALIZATION][2] * 255.0;
+        
+        p.x = roi3D.A.x;
+        p.y = roi3D.A.y;
+        p.z = roi3D.A.z;
+        pointCloud->push_back(p);
+        point = m_map2CamTransform * tf::Vector3(p.x, p.y, p.z);
+        pointMat = (cv::Mat_<double>(3,1) << point[0], point[1], point[2]);
+        pointMat = R.inv() * pointMat;
+        p.x = pointMat.at<double>(0, 0);
+        p.y = pointMat.at<double>(1, 0);
+        p.z = pointMat.at<double>(2, 0);
+        pointCloudTransformed->push_back(p);
+        PointType p2d;
+        project3dTo2d(p, p2d, m_stereoCameraModel, m_map2CamTransform);
+        cout << cv::Point3d(p.x, p.y, p.z) << " -> " << cv::Point2d(p2d.x, p2d.y) << endl;
+        
+        p.x = roi3D.B.x;
+        p.y = roi3D.B.y;
+        p.z = roi3D.B.z;
+        pointCloud->push_back(p);
+        point = m_map2CamTransform * tf::Vector3(p.x, p.y, p.z);
+        pointMat = (cv::Mat_<double>(3,1) << point[0], point[1], point[2]);
+        pointMat = R.inv() * pointMat;
+        p.x = pointMat.at<double>(0, 0);
+        p.y = pointMat.at<double>(1, 0);
+        p.z = pointMat.at<double>(2, 0);
+        pointCloudTransformed->push_back(p);
+        
+        p.x = roi3D.C.x;
+        p.y = roi3D.C.y;
+        p.z = roi3D.C.z;
+        pointCloud->push_back(p);
+        point = m_map2CamTransform * tf::Vector3(p.x, p.y, p.z);
+        pointMat = (cv::Mat_<double>(3,1) << point[0], point[1], point[2]);
+        pointMat = R.inv() * pointMat;
+        p.x = pointMat.at<double>(0, 0);
+        p.y = pointMat.at<double>(1, 0);
+        p.z = pointMat.at<double>(2, 0);
+        pointCloudTransformed->push_back(p);
+        
+        p.x = roi3D.D.x;
+        p.y = roi3D.D.y;
+        p.z = roi3D.D.z;
+        pointCloud->push_back(p);
+        point = m_map2CamTransform * tf::Vector3(p.x, p.y, p.z);
+        pointMat = (cv::Mat_<double>(3,1) << point[0], point[1], point[2]);
+        pointMat = R.inv() * pointMat;
+        p.x = pointMat.at<double>(0, 0);
+        p.y = pointMat.at<double>(1, 0);
+        p.z = pointMat.at<double>(2, 0);
+        pointCloudTransformed->push_back(p);
+        
+        p.x = roi3D.E.x;
+        p.y = roi3D.E.y;
+        p.z = roi3D.E.z;
+        pointCloud->push_back(p);
+        point = m_map2CamTransform * tf::Vector3(p.x, p.y, p.z);
+        pointMat = (cv::Mat_<double>(3,1) << point[0], point[1], point[2]);
+        pointMat = R.inv() * pointMat;
+        p.x = pointMat.at<double>(0, 0);
+        p.y = pointMat.at<double>(1, 0);
+        p.z = pointMat.at<double>(2, 0);
+        pointCloudTransformed->push_back(p);
+        
+        p.x = roi3D.F.x;
+        p.y = roi3D.F.y;
+        p.z = roi3D.F.z;
+        pointCloud->push_back(p);
+        point = m_map2CamTransform * tf::Vector3(p.x, p.y, p.z);
+        pointMat = (cv::Mat_<double>(3,1) << point[0], point[1], point[2]);
+        pointMat = R.inv() * pointMat;
+        p.x = pointMat.at<double>(0, 0);
+        p.y = pointMat.at<double>(1, 0);
+        p.z = pointMat.at<double>(2, 0);
+        pointCloudTransformed->push_back(p);
+        
+        p.x = roi3D.G.x;
+        p.y = roi3D.G.y;
+        p.z = roi3D.G.z;
+        pointCloud->push_back(p);
+        point = m_map2CamTransform * tf::Vector3(p.x, p.y, p.z);
+        pointMat = (cv::Mat_<double>(3,1) << point[0], point[1], point[2]);
+        pointMat = R.inv() * pointMat;
+        p.x = pointMat.at<double>(0, 0);
+        p.y = pointMat.at<double>(1, 0);
+        p.z = pointMat.at<double>(2, 0);
+        pointCloudTransformed->push_back(p);
+        
+        p.x = roi3D.H.x;
+        p.y = roi3D.H.y;
+        p.z = roi3D.H.z;
+        pointCloud->push_back(p);
+        point = m_map2CamTransform * tf::Vector3(p.x, p.y, p.z);
+        pointMat = (cv::Mat_<double>(3,1) << point[0], point[1], point[2]);
+        pointMat = R.inv() * pointMat;
+        p.x = pointMat.at<double>(0, 0);
+        p.y = pointMat.at<double>(1, 0);
+        p.z = pointMat.at<double>(2, 0);
+        pointCloudTransformed->push_back(p);
+        
+        p.x = roi3D.A.x;
+        p.y = roi3D.A.y;
+        p.z = roi3D.A.z;
+        pointCloud->push_back(p);
+        point = m_map2CamTransform * tf::Vector3(p.x, p.y, p.z);
+        pointMat = (cv::Mat_<double>(3,1) << point[0], point[1], point[2]);
+        pointMat = R.inv() * pointMat;
+        PointType p2;
+        p2.r = p.r;
+        p2.g = p.g;
+        p2.b = p.b;
+        p2.x = pointMat.at<double>(0, 0);
+        p2.y = pointMat.at<double>(1, 0);
+        p2.z = pointMat.at<double>(2, 0);
+        pointCloudTransformed->push_back(p2);
+    }           
+    
+    sensor_msgs::PointCloud2 cloudMsg;
+    pcl::toROSMsg (*pointCloud, cloudMsg);
+    cloudMsg.header.frame_id = m_mapFrame;
+    cloudMsg.header.stamp = ros::Time::now();
+    
+    m_debugROIs.publish(cloudMsg);
+
+    sensor_msgs::PointCloud2 cloudMsgTransformed;
+    pcl::toROSMsg (*pointCloudTransformed, cloudMsgTransformed);
+    cloudMsgTransformed.header.frame_id = m_cameraFrame;
+    cloudMsgTransformed.header.stamp = ros::Time::now();
+    
+    m_debugROIs2.publish(cloudMsgTransformed);
+}
 
 tf::Quaternion getQuaternion(const double &vx, const double &vy, const double &vz)
 {
@@ -2251,4 +2451,212 @@ void VoxelGridTracking::publishFakePointCloud()
     m_fakeParticlesPub.publish(fakeParticles);
 }
 
+void VoxelGridTracking::publishTracks()
+{
+    visualization_msgs::MarkerArray tracksCleaner;
+    
+    for (uint32_t i = 0; i < MAX_OBSTACLES_VISUALIZATION * 3; i++) {
+        visualization_msgs::Marker voxelMarker;
+        voxelMarker.header.frame_id = m_mapFrame;
+        voxelMarker.header.stamp = ros::Time();
+        voxelMarker.id = i;
+        voxelMarker.ns = "tracks";
+        voxelMarker.type = visualization_msgs::Marker::LINE_STRIP;
+        voxelMarker.action = visualization_msgs::Marker::DELETE;
+        
+        tracksCleaner.markers.push_back(voxelMarker);
+    }
+    
+    m_debugUpdateTracks.publish(tracksCleaner);
+    
+    visualization_msgs::MarkerArray trackMarkers;
+    
+    uint32_t idCount = 0;
+    uint32_t obstId = 0;
+    
+//     BOOST_FOREACH(const VoxelObstaclePtr & obstacle, m_obstacles) {
+//         if (obstacle->numVoxels() < 2) {
+//             continue;
+//         }
+//         
+//         visualization_msgs::Marker obstacleMarker;
+//         obstacleMarker.header.frame_id = m_mapFrame;
+//         obstacleMarker.header.stamp = ros::Time();
+//         obstacleMarker.id = idCount++;
+//         obstacleMarker.ns = "obstacleCubes";
+//         obstacleMarker.type = visualization_msgs::Marker::CUBE;
+//         obstacleMarker.action = visualization_msgs::Marker::ADD;
+//         
+//         obstacleMarker.pose.position.x = obstacle->centerX();
+//         obstacleMarker.pose.position.y = obstacle->centerY();
+//         obstacleMarker.pose.position.z = obstacle->centerZ();
+//         
+//         obstacleMarker.pose.orientation.x = 0.0;
+//         obstacleMarker.pose.orientation.y = 0.0;
+//         obstacleMarker.pose.orientation.z = 0.0;
+//         obstacleMarker.pose.orientation.w = 1.0;
+//         obstacleMarker.scale.x = 2 * max(fabs(obstacle->maxX() - obstacle->centerX()),
+//                                              fabs(obstacle->minX() - obstacle->centerX()));
+//         obstacleMarker.scale.y = 2 * max(fabs(obstacle->maxY() - obstacle->centerY()),
+//                                              fabs(obstacle->minY() - obstacle->centerY()));
+//         obstacleMarker.scale.z = 2 * max(fabs(obstacle->maxZ() - obstacle->centerZ()),
+//                                              fabs(obstacle->minZ() - obstacle->centerZ()));
+// //         obstacleMarker.color.r = m_obstacleColors[obstId % MAX_OBSTACLES_VISUALIZATION][0];
+// //         obstacleMarker.color.g = m_obstacleColors[obstId % MAX_OBSTACLES_VISUALIZATION][1];
+// //         obstacleMarker.color.b = m_obstacleColors[obstId % MAX_OBSTACLES_VISUALIZATION][2];
+//         obstacleMarker.color.r = 1.0f;
+//         obstacleMarker.color.g = 0.0f;
+//         obstacleMarker.color.b = 0.0f;
+//         obstacleMarker.color.a = 0.5f;
+//         
+//         trackMarkers.markers.push_back(obstacleMarker);
+//         
+//         visualization_msgs::Marker speedVector;
+//         speedVector.header.frame_id = m_mapFrame;
+//         speedVector.header.stamp = ros::Time();
+//         speedVector.id = idCount;
+//         speedVector.ns = "speedVector";
+//         speedVector.type = visualization_msgs::Marker::ARROW;
+//         speedVector.action = visualization_msgs::Marker::ADD;
+//         
+//         speedVector.pose.orientation.x = 0.0;
+//         speedVector.pose.orientation.y = 0.0;
+//         speedVector.pose.orientation.z = 0.0;
+//         speedVector.pose.orientation.w = 1.0;
+//         speedVector.scale.x = 0.01;
+//         speedVector.scale.y = 0.03;
+//         speedVector.scale.z = 0.1;
+//         speedVector.color.a = 1.0;
+//         speedVector.color.r = 255.0;
+//         speedVector.color.g = m_obstacleColors[obstId % MAX_OBSTACLES_VISUALIZATION][1];
+//         speedVector.color.b = m_obstacleColors[obstId % MAX_OBSTACLES_VISUALIZATION][2];
+//         
+//         geometry_msgs::Point origin, dest;
+//         origin.x = obstacle->centerX();
+//         origin.y = obstacle->centerY();
+//         origin.z = obstacle->centerZ();        
+//         
+//         dest.x = obstacle->centerX() + obstacle->vx() * obstacle->magnitude() * 5.0;
+//         dest.y = obstacle->centerY() + obstacle->vy() * obstacle->magnitude() * 5.0;
+//         dest.z = obstacle->centerZ() + obstacle->vz() * obstacle->magnitude() * 5.0;
+//         
+//         speedVector.points.push_back(origin);
+//         speedVector.points.push_back(dest);
+//         
+//         trackMarkers.markers.push_back(speedVector);
+//         
+//         visualization_msgs::Marker speedVectorPrev;
+//         speedVectorPrev.header.frame_id = m_mapFrame;
+//         speedVectorPrev.header.stamp = ros::Time();
+//         speedVectorPrev.id = idCount;
+//         speedVectorPrev.ns = "speedVectorPrev";
+//         speedVectorPrev.type = visualization_msgs::Marker::ARROW;
+//         speedVectorPrev.action = visualization_msgs::Marker::ADD;
+//         
+//         speedVectorPrev.pose.orientation.x = 0.0;
+//         speedVectorPrev.pose.orientation.y = 0.0;
+//         speedVectorPrev.pose.orientation.z = 0.0;
+//         speedVectorPrev.pose.orientation.w = 1.0;
+//         speedVectorPrev.scale.x = 0.01;
+//         speedVectorPrev.scale.y = 0.03;
+//         speedVectorPrev.scale.z = 0.1;
+//         speedVectorPrev.color.a = 0.75;
+//         speedVectorPrev.color.r = 255.0;
+//         speedVectorPrev.color.g = m_obstacleColors[obstId % MAX_OBSTACLES_VISUALIZATION][1];
+//         speedVectorPrev.color.b = m_obstacleColors[obstId % MAX_OBSTACLES_VISUALIZATION][2];
+//         
+//         geometry_msgs::Point origin2, dest2;
+//         origin2.x = obstacle->centerX() - obstacle->vx() * obstacle->magnitude() * 1.0;
+//         origin2.y = obstacle->centerY() - obstacle->vy() * obstacle->magnitude() * 1.0;
+//         origin2.z = obstacle->centerZ() - obstacle->vz() * obstacle->magnitude() * 1.0;     
+//         
+//         dest2.x = origin2.x - obstacle->vx() * obstacle->magnitude() * 5.0;
+//         dest2.y = origin2.y - obstacle->vy() * obstacle->magnitude() * 5.0;
+//         dest2.z = origin2.z - obstacle->vz() * obstacle->magnitude() * 5.0;
+//         
+//         speedVectorPrev.points.push_back(origin2);
+//         speedVectorPrev.points.push_back(dest2);
+//         
+//         trackMarkers.markers.push_back(speedVectorPrev);
+//         
+//         obstId++;
+//     }
+//     
+//     obstId = 0;
+//     BOOST_FOREACH(const VoxelObstaclePtr & obstacle, m_prevObstacles) {
+//         if (obstacle->numVoxels() < 2) {
+//             continue;
+//         }
+//         
+//         visualization_msgs::Marker obstacleMarker;
+//         obstacleMarker.header.frame_id = m_mapFrame;
+//         obstacleMarker.header.stamp = ros::Time();
+//         obstacleMarker.id = idCount++;
+//         obstacleMarker.ns = "obstacleCubes";
+//         obstacleMarker.type = visualization_msgs::Marker::CUBE;
+//         obstacleMarker.action = visualization_msgs::Marker::ADD;
+//         
+//         obstacleMarker.pose.position.x = obstacle->centerX();
+//         obstacleMarker.pose.position.y = obstacle->centerY();
+//         obstacleMarker.pose.position.z = obstacle->centerZ();
+//         
+//         obstacleMarker.pose.orientation.x = 0.0;
+//         obstacleMarker.pose.orientation.y = 0.0;
+//         obstacleMarker.pose.orientation.z = 0.0;
+//         obstacleMarker.pose.orientation.w = 1.0;
+//         obstacleMarker.scale.x = 2 * max(fabs(obstacle->maxX() - obstacle->centerX()),
+//                                          fabs(obstacle->minX() - obstacle->centerX()));
+//         obstacleMarker.scale.y = 2 * max(fabs(obstacle->maxY() - obstacle->centerY()),
+//                                          fabs(obstacle->minY() - obstacle->centerY()));
+//         obstacleMarker.scale.z = 2 * max(fabs(obstacle->maxZ() - obstacle->centerZ()),
+//                                          fabs(obstacle->minZ() - obstacle->centerZ()));
+// //         obstacleMarker.color.r = m_obstacleColors[obstId % MAX_OBSTACLES_VISUALIZATION][0];
+// //         obstacleMarker.color.g = m_obstacleColors[obstId % MAX_OBSTACLES_VISUALIZATION][1];
+// //         obstacleMarker.color.b = m_obstacleColors[obstId % MAX_OBSTACLES_VISUALIZATION][2];
+//         obstacleMarker.color.r = 0.0f;
+//         obstacleMarker.color.g = 0.0f;
+//         obstacleMarker.color.b = 1.0f;
+//         obstacleMarker.color.a = 0.5f;
+//         
+//         trackMarkers.markers.push_back(obstacleMarker);
+//         obstId++;
+//         
+//     }
+    
+    obstId = 0;
+    BOOST_FOREACH(const VoxelObstaclePtr & obstacle, m_obstacles) {
+        if (obstacle->numVoxels() < 2) {
+            continue;
+        }
+        
+        visualization_msgs::Marker obstacleMarker;
+        obstacleMarker.header.frame_id = m_mapFrame;
+        obstacleMarker.header.stamp = ros::Time();
+        obstacleMarker.id = idCount++;
+        obstacleMarker.ns = "obstacleTracks";
+        obstacleMarker.type = visualization_msgs::Marker::LINE_STRIP;
+        obstacleMarker.action = visualization_msgs::Marker::ADD;
+        
+        BOOST_FOREACH(const pcl::PointXYZ & p, *(obstacle->getTrack())) {
+            geometry_msgs::Point pGeom;
+            pGeom.x = p.x;
+            pGeom.y = p.y;
+            pGeom.z = obstacle->centerZ();
+            obstacleMarker.points.push_back(pGeom);
+        }
+        obstacleMarker.scale.x = 0.5f;
+        
+        obstacleMarker.color.r = m_obstacleColors[obstId % MAX_OBSTACLES_VISUALIZATION][0];
+        obstacleMarker.color.g = m_obstacleColors[obstId % MAX_OBSTACLES_VISUALIZATION][1];
+        obstacleMarker.color.b = m_obstacleColors[obstId % MAX_OBSTACLES_VISUALIZATION][2];
+        obstacleMarker.color.a = 1.0f;
+        
+        trackMarkers.markers.push_back(obstacleMarker);
+        obstId++;
+    }
+    
+    m_debugUpdateTracks.publish(trackMarkers);    
+}
+
+
 }
\ No newline at end of file
diff --git a/src/voxelgridtracking.h b/src/voxelgridtracking.h
index fbbbbf1..de5087a 100644
--- a/src/voxelgridtracking.h
+++ b/src/voxelgridtracking.h
@@ -113,6 +113,7 @@ class VoxelGridTracking
     void filterObstacles();
     void joinCommonVolumes();
     void updateSpeedFromObstacles();
+    void updateTracks();
     
     void updateFromOFlow();
     
@@ -126,6 +127,8 @@ class VoxelGridTracking
     void publishROI();
     void publishFakePointCloud();
     void visualizeROI2d();
+    void visualizeROI3d();
+    void publishTracks();
     
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr m_pointCloud;
     pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr m_oFlowCloud;
@@ -156,6 +159,7 @@ class VoxelGridTracking
     tf::TransformListener m_tfListener;
     
     VoxelObstacleList m_obstacles;
+    VoxelObstacleList m_prevObstacles;
     
     uint32_t m_currentId;
     
@@ -241,6 +245,9 @@ class VoxelGridTracking
     image_transport::Subscriber m_debugImgSub;
     ros::Publisher m_fakePointCloudPub;
     ros::Publisher m_fakeParticlesPub;
+    ros::Publisher m_debugUpdateTracks;
+    ros::Publisher m_debugROIs;
+    ros::Publisher m_debugROIs2;
 };
 
 }
diff --git a/src/voxelobstacle.cpp b/src/voxelobstacle.cpp
index bff6fea..9558e0a 100644
--- a/src/voxelobstacle.cpp
+++ b/src/voxelobstacle.cpp
@@ -36,6 +36,8 @@ namespace voxel_grid_tracking {
     m_minX = m_minY = m_minZ = std::numeric_limits<double>::max();
     m_maxX = m_maxY = m_maxZ = std::numeric_limits<double>::min();
     
+    m_track.reset(new pcl::PointCloud<pcl::PointXYZ>);
+    
     addVoxelToObstacle(voxel);
     
 }
@@ -51,6 +53,8 @@ VoxelObstacle::VoxelObstacle(const uint32_t& obstIdx, const double& threshYaw,
 {
     m_minX = m_minY = m_minZ = std::numeric_limits<double>::max();
     m_maxX = m_maxY = m_maxZ = std::numeric_limits<double>::min();
+    
+    m_track.reset(new pcl::PointCloud<pcl::PointXYZ>);
 }
 
 bool VoxelObstacle::addVoxelToObstacle(VoxelPtr& voxel)
@@ -334,6 +338,46 @@ void VoxelObstacle::update(const double & m_voxelSizeX, const double & m_voxelSi
     m_sizeZ = m_maxZ - m_minZ + m_voxelSizeZ;
 }
 
+bool VoxelObstacle::isInObstacle(const pcl::PointXYZ& p)
+{
+//     cout << "isInObstacle? [" << p.x << "," << p.y << "," << p.z << "]" << endl;
+//     cout << "X = " << "[" << m_minX << " - " << m_maxX << "]" << endl;
+//     cout << "Y = " << "[" << m_minY << " - " << m_maxY << "]" << endl;
+//     cout << "Z = " << "[" << m_minZ << " - " << m_maxZ << "]" << endl;
+    
+    if (p.x < m_minX) return false;
+    if (p.x > m_maxX) return false;
+
+    if (p.y < m_minY) return false;
+    if (p.y > m_maxY) return false;
+
+    if (p.z < m_minZ) return false;
+    if (p.z > m_maxZ) return false;
+    
+    return true;
+}
+
+void VoxelObstacle::addTrackFromObstacle(const VoxelObstaclePtr& lastObstacle)
+{
+    pcl::copyPointCloud(*lastObstacle->getTrack(), *m_track);
+
+    pcl::PointXYZ p;
+    p.x = m_centerX;
+    p.y = m_centerY;
+    p.z = m_centerZ;
+    m_track->push_back(p);
+}
+
+void VoxelObstacle::startTrack()
+{
+    pcl::PointXYZ p;
+    p.x = m_centerX;
+    p.y = m_centerY;
+    p.z = m_centerZ;
+    m_track->push_back(p);
+}
+
+
 double VoxelObstacle::commonVolume(const VoxelObstacle& obst1, const VoxelObstacle& obst2)
 {
     const double minX = max(obst1.minX(), obst2.minX());
diff --git a/src/voxelobstacle.h b/src/voxelobstacle.h
index 87bcbf9..6f32944 100644
--- a/src/voxelobstacle.h
+++ b/src/voxelobstacle.h
@@ -62,6 +62,12 @@ class VoxelObstacle
     double vy() const { return m_vy; }
     double vz() const { return m_vz; }
     
+    pcl::PointCloud<pcl::PointXYZ>::Ptr getTrack() { return m_track; }
+    void addTrackFromObstacle(const VoxelObstaclePtr & lastObstacle);
+    void startTrack();
+    
+    bool isInObstacle(const pcl::PointXYZ & p);
+    
     void getROI(const image_geometry::StereoCameraModel & stereoCameraModel,
                 const tf::StampedTransform & map2CamTransform,
                 voxel_grid_tracking::roi_and_speed_2d & roi2D, 
@@ -110,6 +116,8 @@ class VoxelObstacle
     
     VoxelList m_voxels;
     
+    pcl::PointCloud<pcl::PointXYZ>::Ptr m_track;
+    
     uint32_t m_idx;
     double m_magnitude;
     double m_yaw;