added launch/jfr2018 directory with scripts used to generate results for MIT Stata Center dataset of the jfr2018 paper

launch/jfr2018/README.md

@@ -0,0 +1,7 @@
+
+**TODO: currently commited here for backup, though a comprehensive step-by-step procedure would be required so that anyone can reproduce the results**
+
+This is the raw files used to generate results for MIT Stata Center dataset of this paper (for KITTI, EuRoC and TUM datasets, see this [page](https://github.com/introlab/rtabmap/blob/docker/jfr2018)):
+ * M. Labbé and F. Michaud, “RTAB-Map as an Open-Source Lidar and Visual SLAM Library for Large-Scale and Long-Term Online Operation,” in Journal of Field Robotics, accepted, 2018. ([pdf](https://introlab.3it.usherbrooke.ca/mediawiki-introlab/images/7/7a/Labbe18JFR_preprint.pdf)) ([Wiley](https://doi.org/10.1002/rob.21831))
+
+Manual instructions are in [launch_lidar](https://github.com/introlab/rtabmap_ros/blob/master/launch/jfr2018/launch_lidar), [launch_stereo](https://github.com/introlab/rtabmap_ros/blob/master/launch/jfr2018/launch_stereo) and [launch_rgbd](https://github.com/introlab/rtabmap_ros/blob/master/launch/jfr2018/launch_rgbd) files depending on the sensor configuration. Refer also to explanations in the paper.

launch/jfr2018/associate.py

@@ -0,0 +1,128 @@
+#!/usr/bin/python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2013, Juergen Sturm, TUM
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of TUM nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Requirements: 
+# sudo apt-get install python-argparse
+
+"""
+The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images.
+
+For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches.
+"""
+
+import argparse
+import sys
+import os
+import numpy
+
+
+def read_file_list(filename):
+    """
+    Reads a trajectory from a text file. 
+    
+    File format:
+    The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched)
+    and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. 
+    
+    Input:
+    filename -- File name
+    
+    Output:
+    dict -- dictionary of (stamp,data) tuples
+    
+    """
+    file = open(filename)
+    data = file.read()
+    lines = data.replace(","," ").replace("\t"," ").split("\n") 
+    list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
+    list = [(float(l[0]),l[1:]) for l in list if len(l)>1]
+    return dict(list)
+
+def associate(first_list, second_list,offset,max_difference):
+    """
+    Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim 
+    to find the closest match for every input tuple.
+    
+    Input:
+    first_list -- first dictionary of (stamp,data) tuples
+    second_list -- second dictionary of (stamp,data) tuples
+    offset -- time offset between both dictionaries (e.g., to model the delay between the sensors)
+    max_difference -- search radius for candidate generation
+
+    Output:
+    matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))
+    
+    """
+    first_keys = first_list.keys()
+    second_keys = second_list.keys()
+    potential_matches = [(abs(a - (b + offset)), a, b) 
+                         for a in first_keys 
+                         for b in second_keys 
+                         if abs(a - (b + offset)) < max_difference]
+    potential_matches.sort()
+    matches = []
+    for diff, a, b in potential_matches:
+        if a in first_keys and b in second_keys:
+            first_keys.remove(a)
+            second_keys.remove(b)
+            matches.append((a, b))
+    
+    matches.sort()
+    return matches
+
+if __name__ == '__main__':
+    
+    # parse command line
+    parser = argparse.ArgumentParser(description='''
+    This script takes two data files with timestamps and associates them   
+    ''')
+    parser.add_argument('first_file', help='first text file (format: timestamp data)')
+    parser.add_argument('second_file', help='second text file (format: timestamp data)')
+    parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true')
+    parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
+    parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
+    args = parser.parse_args()
+
+    first_list = read_file_list(args.first_file)
+    second_list = read_file_list(args.second_file)
+
+    matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))    
+
+    if args.first_only:
+        for a,b in matches:
+            print("%f %s"%(a," ".join(first_list[a])))
+    else:
+        for a,b in matches:
+            print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
+            
+        

launch/jfr2018/cartographer.launch

@@ -0,0 +1,49 @@
+<!--
+  Copyright 2016 The Cartographer Authors
+
+  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.
+-->
+
+<launch>
+  <param name="/use_sim_time" value="true" />
+
+  <!-- copy provided pr2.lua in $(find cartographer_ros)/configuration_files to use odometry as guess -->
+  <node name="cartographer_node" pkg="cartographer_ros"
+      type="cartographer_node" args="
+          -configuration_directory
+              $(find cartographer_ros)/configuration_files
+          -configuration_basename pr2.lua" 
+      output="screen">
+    <remap from="scan" to="/base_scan_t_filtered" /> <!-- /base_scan_t /base_scan_t_filtered /camera_scan -->
+    <remap from="odom" to="/odom_combined" />
+  </node>
+
+  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
+      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
+
+  <node name="tf_remove_frames" pkg="cartographer_ros"
+      type="tf_remove_frames.py">
+    <remap from="tf_out" to="/tf" />
+    <rosparam param="remove_frames">
+      - map
+      - odom_combined
+    </rosparam>
+  </node>
+
+  <node name="rviz" pkg="rviz" type="rviz" required="true"
+      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
+  <node name="playbag" pkg="rosbag" type="play"
+      args="--clock $(arg bag_filename)">
+    <remap from="tf" to="tf_in" />
+  </node>
+</launch>

launch/jfr2018/cartographer_sync_markers_gt.py

@@ -0,0 +1,77 @@
+#!/usr/bin/env python  
+import roslib
+import rospy
+import os
+import tf
+import numpy
+import evaluate_ate
+from visualization_msgs.msg import MarkerArray
+from geometry_msgs.msg import Point
+
+def callback(data):
+    global slamPoses
+    global gtPoses
+    global stamps
+    global listener
+    global lastSize
+    global slamPosesInd
+    global rmse
+    if len(data.markers) > 0 and lastSize != len(data.markers[0].points):
+        t = rospy.Time(data.markers[0].header.stamp.secs, data.markers[0].header.stamp.nsecs)
+        try:
+            listener.waitForTransform(fixedFrame, baseFrame, t, rospy.Duration(0.2))
+            (trans,rot) = listener.lookupTransform(fixedFrame, baseFrame, t)
+            gtPoses.append(Point(trans[0], trans[1], 0))
+            stamps.append(t.to_sec())
+            slamPoses.append(data.markers[0].points[len(data.markers[0].points)-1])
+            slamPosesInd.append(len(data.markers[0].points)-1)
+            first_xyz = numpy.empty([0,3])
+            second_xyz = numpy.empty([0,3])
+            for g in gtPoses:
+                newrow = [g.x,g.y,g.z]
+                first_xyz = numpy.vstack([first_xyz, newrow])
+            for p in slamPoses:
+                newrow = [p.x,p.y,p.z]
+                second_xyz = numpy.vstack([second_xyz, newrow])
+            first_xyz = numpy.matrix(first_xyz).transpose()
+            second_xyz = numpy.matrix(second_xyz).transpose()
+            rot,trans,trans_error = evaluate_ate.align(second_xyz, first_xyz)
+            rmse_v = numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
+            rmse.append(rmse_v)
+            print "points= " + str(len(data.markers[0].points)) + " added=" + str(len(slamPoses)) + " rmse=" + str(rmse_v)
+        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
+            print str(e)
+    if len(data.markers) > 0 and len(slamPosesInd) > 0:
+        j=0
+        for i in slamPosesInd:
+           slamPoses[j] = data.markers[0].points[i]
+           j+=1
+    if len(data.markers) > 0:
+        lastSize = len(data.markers[0].points)
+
+if __name__ == '__main__':
+    rospy.init_node('sync_markers_gt', anonymous=True)
+    listener = tf.TransformListener()
+    fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
+    baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
+    rospy.Subscriber("trajectory_node_list", MarkerArray, callback, queue_size=1)
+    slamPoses = []
+    gtPoses = []
+    stamps = []
+    slamPosesInd = []
+    rmse = []
+    lastSize = 0
+    rospy.spin()
+    fileSlam = open('slam_poses.txt','w')
+    fileGt = open('gt_poses.txt','w')
+    fileRMSE = open('rmse.txt','w')
+    print "slam= " + str(len(slamPoses))
+    print "gt= " + str(len(gtPoses))
+    print "stamps= " + str(len(stamps))
+    for c, g, t, r in zip(slamPoses, gtPoses, stamps, rmse):
+        fileSlam.write('%f %f %f 0 0 0 0 1\n' % (t, c.x, c.y))
+        fileGt.write('%f %f %f 0 0 0 0 1\n' % (t, g.x, g.y))
+        fileRMSE.write('%f %f\n' % (t, r))
+    fileSlam.close()
+    fileGt.close()
+    fileRMSE.close()

launch/jfr2018/ethzasl_icp_mapper_sync_gt.py

@@ -0,0 +1,71 @@
+#!/usr/bin/env python  
+import roslib
+import rospy
+import os
+import tf
+import numpy
+import evaluate_ate
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import Point
+
+def callback(data):
+    global slamPoses
+    global gtPoses
+    global stamps
+    global listener
+    global rmse
+    global lastTime
+  
+    if rospy.get_time() - lastTime < 1:
+        return
+    lastTime = rospy.get_time()
+
+    t = rospy.Time(data.header.stamp.secs, data.header.stamp.nsecs)
+    try:
+        listener.waitForTransform(fixedFrame, baseFrame, t, rospy.Duration(0.2))
+        (trans,rot) = listener.lookupTransform(fixedFrame, baseFrame, t)
+        gtPoses.append(Point(trans[0], trans[1], 0))
+        stamps.append(t.to_sec())
+        slamPoses.append(data.pose.pose.position)
+        first_xyz = numpy.empty([0,3])
+        second_xyz = numpy.empty([0,3])
+        for g in gtPoses:
+            newrow = [g.x,g.y,g.z]
+            first_xyz = numpy.vstack([first_xyz, newrow])
+        for p in slamPoses:
+            newrow = [p.x,p.y,p.z]
+            second_xyz = numpy.vstack([second_xyz, newrow])
+        first_xyz = numpy.matrix(first_xyz).transpose()
+        second_xyz = numpy.matrix(second_xyz).transpose()
+        rot,trans,trans_error = evaluate_ate.align(second_xyz, first_xyz)
+        rmse_v = numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
+        rmse.append(rmse_v)
+        print " added=" + str(len(slamPoses)) + " rmse=" + str(rmse_v)
+    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
+        print str(e)
+
+if __name__ == '__main__':
+    rospy.init_node('sync_odom_gt', anonymous=True)
+    listener = tf.TransformListener()
+    fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
+    baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
+    rospy.Subscriber("icp_odom", Odometry, callback, queue_size=1)
+    slamPoses = []
+    gtPoses = []
+    stamps = []
+    rmse = []
+    lastTime = rospy.get_time()
+    rospy.spin()
+    fileSlam = open('slam_poses.txt','w')
+    fileGt = open('gt_poses.txt','w')
+    fileRMSE = open('rmse.txt','w')
+    print "slam= " + str(len(slamPoses))
+    print "gt= " + str(len(gtPoses))
+    print "stamps= " + str(len(stamps))
+    for c, g, t, r in zip(slamPoses, gtPoses, stamps, rmse):
+        fileSlam.write('%f %f %f 0 0 0 0 1\n' % (t, c.x, c.y))
+        fileGt.write('%f %f %f 0 0 0 0 1\n' % (t, g.x, g.y))
+        fileRMSE.write('%f %f\n' % (t, r))
+    fileSlam.close()
+    fileGt.close()
+    fileRMSE.close()

launch/jfr2018/ethzasl_icp_mapping.patch

@@ -0,0 +1,54 @@
+diff --git a/ethzasl_icp_mapper/launch/2D_scans/icp.yaml b/ethzasl_icp_mapper/launch/2D_scans/icp.yaml
+index 67820e7..aec1839 100644
+--- a/ethzasl_icp_mapper/launch/2D_scans/icp.yaml
++++ b/ethzasl_icp_mapper/launch/2D_scans/icp.yaml
+@@ -1,12 +1,12 @@
+ matcher:
+   KDTreeMatcher:
+-    maxDist: 1.0
++    maxDist: 1
+     knn: 5
+     epsilon: 3.16
+ 
+ outlierFilters:
+   - TrimmedDistOutlierFilter:
+-     ratio: 0.85
++     ratio: 0.95
+   - SurfaceNormalOutlierFilter:
+      maxAngle: 0.42
+ 
+@@ -25,7 +25,11 @@ transformationCheckers:
+       maxTranslationNorm: 5.00
+ 
+ inspector:
+-# VTKFileInspector
++# VTKFileInspector:
++#   baseFileName : debug--
++#   dumpDataLinks : 1
++#   dumpReading : 1
++#   dumpReference : 1
+  NullInspector
+ 
+ logger:
+diff --git a/libpointmatcher_ros/src/point_cloud.cpp b/libpointmatcher_ros/src/point_cloud.cpp
+index b77651d..8eb1f6c 100644
+--- a/libpointmatcher_ros/src/point_cloud.cpp
++++ b/libpointmatcher_ros/src/point_cloud.cpp
+@@ -449,7 +449,7 @@ namespace PointMatcher_ros
+         try
+         {
+           listener->transformPoint(
+-						fixedFrame,
++						pin.header.frame_id,
+             rosMsg.header.stamp,
+             pin,
+             fixedFrame,
+@@ -459,7 +459,7 @@ namespace PointMatcher_ros
+           if(addObservationDirection)
+           {
+             listener->transformPoint(
+-                fixedFrame,
++                s_in.header.frame_id,
+                 curTime,
+                 s_in,
+                 fixedFrame,

launch/jfr2018/evaluate_ate.py

@@ -0,0 +1,195 @@
+#!/usr/bin/python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2013, Juergen Sturm, TUM
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of TUM nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Requirements: 
+# sudo apt-get install python-argparse
+
+"""
+This script computes the absolute trajectory error from the ground truth
+trajectory and the estimated trajectory.
+"""
+
+import sys
+import numpy
+import argparse
+import associate
+
+def align(model,data):
+    """Align two trajectories using the method of Horn (closed-form).
+    
+    Input:
+    model -- first trajectory (3xn)
+    data -- second trajectory (3xn)
+    
+    Output:
+    rot -- rotation matrix (3x3)
+    trans -- translation vector (3x1)
+    trans_error -- translational error per point (1xn)
+    
+    """
+    numpy.set_printoptions(precision=3,suppress=True)
+    model_zerocentered = model - model.mean(1)
+    data_zerocentered = data - data.mean(1)
+    
+    W = numpy.zeros( (3,3) )
+    for column in range(model.shape[1]):
+        W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])
+    U,d,Vh = numpy.linalg.linalg.svd(W.transpose())
+    S = numpy.matrix(numpy.identity( 3 ))
+    if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0):
+        S[2,2] = -1
+    rot = U*S*Vh
+    trans = data.mean(1) - rot * model.mean(1)
+    
+    model_aligned = rot * model + trans
+    alignment_error = model_aligned - data
+    
+    trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]
+        
+    return rot,trans,trans_error
+
+def plot_traj(ax,stamps,traj,style,color,label):
+    """
+    Plot a trajectory using matplotlib. 
+    
+    Input:
+    ax -- the plot
+    stamps -- time stamps (1xn)
+    traj -- trajectory (3xn)
+    style -- line style
+    color -- line color
+    label -- plot legend
+    
+    """
+    stamps.sort()
+    interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])])
+    x = []
+    y = []
+    last = stamps[0]
+    for i in range(len(stamps)):
+        if stamps[i]-last < 2*interval:
+            x.append(traj[i][0])
+            y.append(traj[i][1])
+        elif len(x)>0:
+            ax.plot(x,y,style,color=color,label=label)
+            label=""
+            x=[]
+            y=[]
+        last= stamps[i]
+    if len(x)>0:
+        ax.plot(x,y,style,color=color,label=label)
+            
+
+if __name__=="__main__":
+    # parse command line
+    parser = argparse.ArgumentParser(description='''
+    This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory. 
+    ''')
+    parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)')
+    parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)')
+    parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
+    parser.add_argument('--scale', help='scaling factor for the second trajectory (default: 1.0)',default=1.0)
+    parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
+    parser.add_argument('--save', help='save aligned second trajectory to disk (format: stamp2 x2 y2 z2)')
+    parser.add_argument('--save_associations', help='save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2)')
+    parser.add_argument('--plot', help='plot the first and the aligned second trajectory to an image (format: png)')
+    parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed)', action='store_true')
+    args = parser.parse_args()
+
+    first_list = associate.read_file_list(args.first_file)
+    second_list = associate.read_file_list(args.second_file)
+
+    matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference))    
+    if len(matches)<2:
+        sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")
+
+
+    first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose()
+    second_xyz = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for a,b in matches]).transpose()
+    rot,trans,trans_error = align(second_xyz,first_xyz)
+    
+    second_xyz_aligned = rot * second_xyz + trans
+    
+    first_stamps = first_list.keys()
+    first_stamps.sort()
+    first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose()
+    
+    second_stamps = second_list.keys()
+    second_stamps.sort()
+    second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for b in second_stamps]).transpose()
+    second_xyz_full_aligned = rot * second_xyz_full + trans
+    
+    if args.verbose:
+        print "compared_pose_pairs %d pairs"%(len(trans_error))
+
+        print "absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
+        print "absolute_translational_error.mean %f m"%numpy.mean(trans_error)
+        print "absolute_translational_error.median %f m"%numpy.median(trans_error)
+        print "absolute_translational_error.std %f m"%numpy.std(trans_error)
+        print "absolute_translational_error.min %f m"%numpy.min(trans_error)
+        print "absolute_translational_error.max %f m"%numpy.max(trans_error)
+    else:
+        print "%f"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
+        
+    if args.save_associations:
+        file = open(args.save_associations,"w")
+        file.write("\n".join(["%f %f %f %f %f %f %f %f"%(a,x1,y1,z1,b,x2,y2,z2) for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A)]))
+        file.close()
+        
+    if args.save:
+        file = open(args.save,"w")
+        file.write("\n".join(["%f "%stamp+" ".join(["%f"%d for d in line]) for stamp,line in zip(second_stamps,second_xyz_full_aligned.transpose().A)]))
+        file.close()
+
+    if args.plot:
+        import matplotlib
+        matplotlib.use('Agg')
+        import matplotlib.pyplot as plt
+        import matplotlib.pylab as pylab
+        from matplotlib.patches import Ellipse
+        fig = plt.figure()
+        ax = fig.add_subplot(111)
+        plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth")
+        plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated")
+
+        #label="difference"
+        #for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A):
+        #    ax.plot([x1,x2],[y1,y2],'-',color="red",label=label)
+        #    label=""
+            
+        ax.legend()
+            
+        ax.set_xlabel('x [m]')
+        ax.set_ylabel('y [m]')
+        plt.savefig(args.plot,dpi=300, format='pdf')
+        

launch/jfr2018/extract_rgbd.py

@@ -0,0 +1,16 @@
+import rosbag
+import tf
+import sys
+from tf.msg import tfMessage
+
+if len(sys.argv) < 2:
+	print 'Usage: $ python extract_rgbd.py "2012-01-25-12-14-25"'
+	sys.exit(0)
+
+bagName = sys.argv[1]
+with rosbag.Bag(bagName + '_rgbd.bag', 'w') as outbag:
+	print 'Processing ' + bagName + '.bag...'
+	for topic, msg, t in rosbag.Bag(bagName + '.bag').read_messages():
+		if topic == "/tf" or topic == "/camera/depth/image_raw" or topic == "/camera/rgb/camera_info" or topic == "/camera/rgb/image_raw":
+			outbag.write(topic, msg, t)
+print 'Output: ' + bagName + '_out.bag'

launch/jfr2018/extract_scans.py

@@ -0,0 +1,21 @@
+import rosbag
+import tf
+import sys
+from tf.msg import tfMessage
+
+if len(sys.argv) < 2:
+	print 'Usage: $ python extract_scans.py "2012-01-25-12-14-25"'
+	sys.exit(0)
+
+bagName = sys.argv[1]
+with rosbag.Bag(bagName + '_scans.bag', 'w') as outbag:
+	print 'Processing ' + bagName + '.bag...'
+	for topic, msg, t in rosbag.Bag(bagName + '.bag').read_messages():
+		if topic == "/tf":
+			outbag.write(topic, msg, t)
+		elif topic == "/base_scan":
+			outbag.write(topic, msg, t)
+                elif topic == "/robot_pose_ekf/odom_combined":
+			outbag.write(topic, msg, t)
+
+print 'Output: ' + bagName + '_out.bag'

launch/jfr2018/extract_stereo.py

@@ -0,0 +1,37 @@
+import rosbag
+import tf
+import sys
+from tf.msg import tfMessage
+
+if len(sys.argv) < 2:
+	print 'Usage: $ python extract_stereo.py "2012-01-25-12-14-25"'
+	sys.exit(0)
+
+bagName = sys.argv[1]
+with rosbag.Bag(bagName + '_stereo.bag', 'w') as outbag:
+	print 'Processing ' + bagName + '.bag...'
+	leftCamInfoStatus = True
+	leftImageStatus = True
+	rightCamInfoStatus = True
+	rightImageStatus = True
+	for topic, msg, t in rosbag.Bag(bagName + '.bag').read_messages():
+		if topic == "/tf":
+			outbag.write(topic, msg, t)
+		elif topic == "/wide_stereo/left/camera_info":
+			if leftCamInfoStatus:
+				outbag.write(topic, msg, t)
+			leftCamInfoStatus = not leftCamInfoStatus
+		elif topic == "/wide_stereo/right/camera_info":
+			if rightCamInfoStatus:
+				outbag.write(topic, msg, t)
+			rightCamInfoStatus = not rightCamInfoStatus
+		elif topic == "/wide_stereo/left/image_raw":
+			if leftImageStatus:
+				outbag.write(topic, msg, t)
+			leftImageStatus = not leftImageStatus
+		elif topic == "/wide_stereo/right/image_raw":
+			if rightImageStatus:
+				outbag.write(topic, msg, t)
+			rightImageStatus = not rightImageStatus
+
+print 'Output: ' + bagName + '_out.bag'

launch/jfr2018/filterBagOdomTF.py

@@ -0,0 +1,16 @@
+import rosbag
+from tf.msg import tfMessage
+with rosbag.Bag('2012-01-25-12-33-29_scans-noodom.bag', 'w') as outbag:
+	for topic, msg, t in rosbag.Bag('2012-01-25-12-33-29_scans.bag').read_messages():
+		if topic == "/tf" and msg.transforms:
+			newList = [];
+			for m in msg.transforms:
+				if m.header.frame_id != "/odom_combined":
+					newList.append(m)
+				else:
+					print 'odom frame removed!'
+			if len(newList)>0:
+				msg.transforms = newList
+				outbag.write(topic, msg, t)
+		else:
+			outbag.write(topic, msg, t)

launch/jfr2018/filterBagTF.py

@@ -0,0 +1,16 @@
+import rosbag
+from tf.msg import tfMessage
+with rosbag.Bag('2012-01-25-12-14-25-noOdomCombined.bag', 'w') as outbag:
+	for topic, msg, t in rosbag.Bag('2012-01-25-12-14-25.bag').read_messages():
+		if topic == "/tf" and msg.transforms:
+			newList = [];
+			for m in msg.transforms:
+				if m.header.frame_id != "odom_combined":
+					newList.append(m)
+				else:
+					print 'map frame removed!'
+			if len(newList)>0:
+				msg.transforms = newList
+				outbag.write(topic, msg, t)
+		else:
+			outbag.write(topic, msg, t)

launch/jfr2018/gt_tf_broadcaster.py

@@ -0,0 +1,71 @@
+#!/usr/bin/env python  
+import roslib
+import rospy
+import os
+import tf
+import numpy
+
+if __name__ == '__main__':
+    rospy.init_node('groundtruth_tf_broadcaster')
+    fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
+    baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
+    offset_time = rospy.get_param('~offset_time', 0.0)
+    offset_x = rospy.get_param('~offset_x', 0.0)
+    offset_y = rospy.get_param('~offset_y', 0.0)
+    offset_theta = rospy.get_param('~offset_theta', 0.0)
+    gtFile = rospy.get_param('~file', 'groundtruth.txt')
+    gtFile = os.path.expanduser(gtFile) 
+    br = tf.TransformBroadcaster()
+
+    init_x = 0
+    init_y = 0
+    init = False
+
+    # assuming format "timestamp,x,y,theta"
+    for line in open(gtFile,'r'):
+        mylist = line.split(',')
+        if len(mylist) == 4 and not rospy.is_shutdown():
+            stamp = float(int(mylist[0]))/1000000.0 + offset_time
+            x = float(mylist[1])
+            y = float(mylist[2])
+            theta = float(mylist[3])
+
+            if not init:
+                init_x = x
+                init_y = y
+                init = True
+            
+            x -= init_x
+            y -= init_y
+
+            trans1_mat = tf.transformations.translation_matrix((x, y, 0))
+            rot1_mat   = tf.transformations.quaternion_matrix(tf.transformations.quaternion_from_euler(0, 0, theta))
+            mat1 = numpy.dot(trans1_mat, rot1_mat)
+
+            trans2_mat = tf.transformations.translation_matrix((offset_x, offset_y, 0))
+            rot2_mat   = tf.transformations.quaternion_matrix(tf.transformations.quaternion_from_euler(0, 0, offset_theta))
+            mat2 = numpy.dot(trans2_mat, rot2_mat)
+
+            mat3 = numpy.dot(mat1, mat2)
+            trans3 = tf.transformations.translation_from_matrix(mat3)
+            rot3 = tf.transformations.quaternion_from_matrix(mat3)
+
+            #print(mat1)
+            #print(mat2)
+            #print(mat3)
+
+            now = rospy.get_time()
+            while not rospy.is_shutdown() and now < stamp:
+                delay = stamp - now
+                if delay > 0.05:
+                    delay = 0.05
+                rospy.sleep(delay)
+                now = rospy.get_time()
+            if not rospy.is_shutdown():
+                br.sendTransform(trans3,
+                    rot3,
+                    rospy.Time.from_sec(stamp),
+                    baseFrame,
+                    fixedFrame)
+        else:
+            break

launch/jfr2018/launch_lidar

@@ -0,0 +1,103 @@
+// icp_odometry with guess from odom_combined frame
+$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Reg/Force3DoF true --Reg/Strategy 1 --RGBD/ProximityPathMaxNeighbors 10 --RGBD/ProximityPathFilteringRadius 1 --RGBD/ProximityByTime false --Mem/STMSize 30 --Mem/LaserScanVoxelSize 0.05 --Mem/LaserScanNormalK 5 --Mem/LaserScanNormalRadius 1 --Icp/Epsilon 0.001 --Icp/MaxTranslation 0.5 --RGBD/OptimizeMaxError 1 --Icp/PointToPlane true --Icp/CorrespondenceRatio 0.10 --Icp/PMOutlierRatio 0.95 --Mem/BinDataKept true --Grid/RangeMax 0 --Kp/DetectorStrategy 0 --Kp/MaxFeatures 200 --SURF/HessianThreshold 100 --Vis/MaxFeatures 500 --Mem/UseOdomFeatures false" odom_args:="--Icp/VoxelSize 0.05 --Icp/PointToPlaneRadius 1 --Odom/GuessMotion true --Odom/Strategy 0"  rgbd_sync:=true subscribe_scan:=true frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true visual_odometry:=false odom_guess_frame_id:=odom_combined odom_guess_min_translation:=0.1 odom_guess_min_rotation:=0.1 odom_topic:=odom icp_odometry:=true database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db
+// proximity space short range
+scan_topic:=/base_scan_t_filtered
+// proximity space longer range
+scan_topic:=/base_scan_t
+// RGB-D back-end
+rgb_topic:=/camera/rgb/image_raw depth_topic:=/camera/depth/image_raw camera_info_topic:=/camera/rgb/camera_info
+// Stereo back-end
+stereo:=true stereo_namespace:=/wide_stereo left_camera_info_topic:=/wide_stereo/left/camera_info right_camera_info_topic:=/wide_stereo/right/camera_info_scaled approx_rgbd_sync:=false approx_sync:=true
+// odometry
+odom_frame_id:="odom_combined" odom_tf_angular_variance:=0.0001 odom_tf_linear_variance:=0.0001 icp_odometry:=false
+// disable loop closure detection
+--Kp/MaxFeatures -1
+//neighbor link refine
+--RGBD/NeighborLinkRefining true --RGBD/OptimizeMaxError 1.5
+// odom frame to frame
+--Odom/Strategy 1 
+
+$ rosparam load scan_filter.yaml scan_to_scan_filter_chain
+$ rosrun laser_filters scan_to_scan_filter_chain scan:=/base_scan_t scan_filtered:=/base_scan_t_filtered
+
+$ ./republish_scan.py _offset:=82.2
+
+//stereo
+$ ./republish_camera_info.py camera_info_in:=/wide_stereo/right/camera_info camera_info_out:=/wide_stereo/right/camera_info_scaled 
+$ export ROS_NAMESPACE=wide_stereo
+$ rosrun stereo_image_proc stereo_image_proc left/image_raw:=left/image_raw right/image_raw:=right/image_raw left/camera_info:=left/camera_info right/camera_info:=right/camera_info_scaled
+
+$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-14-25_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
+// or
+// To make ground truth matching dataset 2012-01-25-12-14-25.bag (transform is found by exporting scans according to ground truth for each sequence, then use pcl_icp)
+// xyz=0.006236,-0.351500,0.000000 rpy=0.000000,0.000000,-0.017832
+// old -0.00273873 -0.38818 0 0 0 0
+$ rosrun tf static_transform_publisher 0.006236 -0.351500 0 -0.017832 0 0 world world_offset 100
+$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world_offset _offset_time:=82.2 _offset_x:=-0.275
+// otherwise
+$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
+
+// rtabmap
+$ ./sync_path_gt.py _frame_id:=scan_gt mapPath:=/rtabmap/mapPath
+
+$ rostopic hz /rtabmap/grid_map
+
+
+$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-14-25.bag
+// or
+$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-33-29.bag
+
+//copy results (assuming we are in bags directory)
+$ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-14-25/rtabmap
+//or
+$ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-33-29/rtabmap
+
+
+
+
+
+/// OTHER SCAN LIDAR SLAM
+$ roscore
+$ rosparam set use_sim_time true
+$ ./republish_scan.py _offset:=82.2
+//2012-01-25-12-14-25
+$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-14-25.bag
+./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-14-25_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
+//2012-01-25-12-33-29
+$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-33-29.bag
+$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
+// for short-lidar 
+$ rosparam load scan_filter.yaml scan_to_scan_filter_chain
+$ rosrun laser_filters scan_to_scan_filter_chain scan:=/base_scan_t scan_filtered:=/base_scan_t_filtered
+// for fake lidar kinect
+$ rosrun depthimage_to_laserscan depthimage_to_laserscan  image:=/camera/depth/image_raw camera_info:=/camera/rgb/camera_info scan:=/camera_scan _output_frame_id:=openni_rgb_frame _range_max:=6
+
+// gmapping
+$ rosrun gmapping slam_gmapping scan:=base_scan_t _odom_frame:=odom_combined _particles:=100 _base_frame:=base_footprint
+$ ./sync_path_gt.py _frame_id:=scan_gt
+
+// hector_slam
+$ rosrun hector_mapping hector_mapping _pub_map_odom_transform:=true _map_frame:=map _base_frame:=base_footprint _odom_frame:=odom_combined scan:=/base_scan_t _map_size:=4096
+$ ./sync_path_gt.py _frame_id:=scan_gt mapPath:=trajectory
+$ roslaunch hector_geotiff geotiff_mapper.launch trajectory_source_frame_name:=base_footprint
+
+// etchzasl_icp_mapper (in Mapper::gotScan(), change odomFrame to scanMsgIn.header.frame_id... OR in libpointmatcher_ros/pointcloud.cpp, change in rosMsgToPointMatcherCloud() the target_frame of transformPoint() to point's frame_id)
+// Remove _offset_x:=-0.275 from gt_tf_broadcaster.py above
+$ rosrun ethzasl_icp_mapper mapper scan:=/base_scan_t _odom_frame:=/odom_combined _map_frame:=map _subscribe_scan:=true _subscribe_cloud:=false _minMapPointCount:=1000 _minReadingPointCount:=150 _icpConfig:="/home/mathieu/catkin_ws/src/ethzasl_icp_mapping/ethzasl_icp_mapper/launch/2D_scans/icp.yaml" _inputFiltersConfig:="/home/mathieu/catkin_ws/src/ethzasl_icp_mapping/ethzasl_icp_mapper/launch/2D_scans/input_filters.yaml" _mapPostFiltersConfig:="/home/mathieu/catkin_ws/src/ethzasl_icp_mapping/ethzasl_icp_mapper/launch/2D_scans/map_post_filters.yaml" _minOverlap:=0.5
+$ rosrun ethzasl_icp_mapper occupancy_grid_builder scan:=/base_scan_t
+$ ./ethzasl_icp_mapper_sync_gt.py _frame_id:=scan_gt
+
+// cartographer (markers time may be slightly off as they are not updated at the sime time than scan)
+$ ./pose_to_odom.py
+$ roslaunch cartographer.launch bag_filename:=/home/mathieu/bags/2012-01-25-12-33-29.bag
+$ ./cartographer_sync_markers_gt.py _frame_id:=scan_gt
+
+// karto (don't use graph as markers, but GetAllProcessedScans with GetCorrectedPose for each scan pose, then set time of marker to last scan)
+$ rosrun slam_karto slam_karto _base_frame:=base_footprint _odom_frame:=odom_combined scan:=/base_scan_t _map_update_interval:=1
+$ ./slam_karto_sync_markers_gt.py _frame_id:=scan_gt mapPath:=/rtabmap/mapPath
+
+// mrpt_graphslam_2d (add scan_topic and odom_topic arguments to launch, don't play with odom_ombined tf)
+$ roslaunch mrpt_graphslam_2d graphslam.launch scan_topic:=base_scan_t odom_topic:=odom_combined base_link_frame_ID:=base_footprint odometry_frame_ID:=/odom_combined start_rviz:=true
+$ ./pose_to_odom.py
+$ ./sync_path_gt.py _frame_id:=scan_gt mapPath:=/feedback/robot_trajectory
+

launch/jfr2018/launch_rgbd

@@ -0,0 +1,31 @@
+// rgbd_odometry
+$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Rtabmap/StartNewMapOnLoopClosure true --Reg/Force3DoF false --RGBD/ProximityPathMaxNeighbors 0 --Mem/STMSize 15 --Mem/BinDataKept false --Kp/FlannRebalancingFactor 1.0 --RGBD/LinearUpdate 0 --RGBD/ProximityBySpace true --RGBD/OptimizeMaxError 0.5 --FAST/Threshold 7" odom_args:="--Odom/Strategy 0 --Vis/CorType 0 --Odom/KeyFrameThr 0.3 --OdomF2M/MaxSize 2000 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15" rgbd_sync:=true depth_scale:=1.043 frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true  odom_topic:=odom rgb_topic:=/camera/rgb/image_raw_throttle depth_topic:=/camera/depth/image_raw_throttle camera_info_topic:=/camera/rgb/camera_info_throttle approx_sync:=false database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db odom_guess_frame_id:=odom_combined 
+// use wheel odom
+odom_frame_id:=odom_combined odom_tf_angular_variance:=0.0001 odom_tf_linear_variance:=0.0001 visual_odometry:=false
+// flow 
+--Vis/CorType 1 --Odom/KeyFrameThr 0.6 --Vis/BundleAdjustment 0
+// robot_localization
+odom_topic:=/odometry/filtered visual_odometry:=false approx_sync:=true
+$ roslaunch sensor_fusion.launch odom_args:="--Odom/Strategy 0 --Vis/CorType 0 --Odom/KeyFrameThr 0.3 --OdomF2M/MaxSize 2000 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15 --FAST/Threshold 7" rgbd_image_topic:=/rtabmap/rgbd_image frame_id:=base_footprint imu_topic:=/torso_lift_imu/data wheelodom_topic:=/base_odometry/odom
+
+$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-14-25_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
+// or
+// To make ground truth matching dataset 2012-01-25-12-14-25.bag (offset found with pcl_icp2d between end of first bag and begin of second bag)
+$ rosrun tf static_transform_publisher -0.00273873 -0.38818 0 0 0 0 world world_offset 100
+$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world_offset _offset_time:=82.2 _offset_x:=-0.275
+// otherwise
+$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
+
+$ ./sync_path_gt.py _frame_id:=scan_gt mapPath:=/rtabmap/mapPath
+
+$ rostopic hz /rtabmap/grid_map
+
+$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-14-25_rgbd.bag
+// or
+$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-33-29_rgbd.bag
+
+//copy results (assuming we are in bags directory)
+$ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-14-25/rtabmap/rgbd
+//or
+$ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-33-29/rtabmap/rgbd
+

launch/jfr2018/launch_stereo

@@ -0,0 +1,34 @@
+// stereo_odometry
+$ roslaunch rtabmap_ros rtabmap.launch args:="-d --Rtabmap/PublishRAMUsage true --Rtabmap/StartNewMapOnLoopClosure true --Reg/Force3DoF false --RGBD/ProximityPathMaxNeighbors 0 --Mem/STMSize 15 --Mem/BinDataKept false --Kp/FlannRebalancingFactor 1.0 --RGBD/LinearUpdate 0 --RGBD/ProximityBySpace true --Odom/KeyFrameThr 0.3 --Odom/Strategy 0 --OdomF2M/MaxSize 2000 --RGBD/OptimizeMaxError 0.5 --OdomORBSLAM2/VocPath /home/mathieu/workspace/ORB_SLAM2/Vocabulary/ORBvoc.txt --OdomORBSLAM2/Fps 15 --FAST/Threshold 7" odom_args:="--Vis/CorType 0" frame_id:=base_footprint ground_truth_frame_id:=world ground_truth_base_frame_id:=scan_gt use_sim_time:=true stereo:=true stereo_namespace:=/wide_stereo left_camera_info_topic:=/wide_stereo/left/camera_info_throttle right_camera_info_topic:=/wide_stereo/right/camera_info_scaled odom_topic:=odom  approx_sync:=false database_path:=/media/mathieu/5B60E7B25BDFCB79/bags/rtabmap.db odom_guess_frame_id:=odom_combined 
+// with wheel odom
+odom_frame_id:=odom_combined odom_tf_angular_variance:=0.0001 odom_tf_linear_variance:=0.0001 visual_odometry:=false
+// flow 
+--Vis/CorType 1 --Odom/KeyFrameThr 0.6
+
+$ ./republish_camera_info.py camera_info_in:=/wide_stereo/right/camera_info_throttle camera_info_out:=/wide_stereo/right/camera_info_scaled 
+
+$ export ROS_NAMESPACE=wide_stereo
+$ rosrun stereo_image_proc stereo_image_proc left/image_raw:=left/image_raw_throttle right/image_raw:=right/image_raw_throttle left/camera_info:=left/camera_info_throttle right/camera_info:=right/camera_info_scaled
+<node if="$(arg gen_depth)" pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap_ros/disparity_to_depth"/>
+
+$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-14-25_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
+// or
+// To make ground truth matching dataset 2012-01-25-12-14-25.bag (offset found with pcl_icp2d between end of first bag and begin of second bag)
+$ rosrun tf static_transform_publisher -0.00273873 -0.38818 0 0 0 0 world world_offset 100
+$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world_offset _offset_time:=82.2 _offset_x:=-0.275
+// otherwise
+$ ./gt_tf_broadcaster.py _file:=./stata-mit/2012-01-25-12-33-29_part1_floor2.gt.laser.poses _frame_id:=scan_gt _fixed_frame_id:=world _offset_time:=82.2 _offset_x:=-0.275
+
+$ ./sync_path_gt.py _frame_id:=scan_gt mapPath:=/rtabmap/mapPath
+
+$ rostopic hz /rtabmap/grid_map
+
+$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-14-25_stereo.bag
+// or
+$ rosbag play --clock --pause ./stata-mit/2012-01-25-12-33-29_stereo.bag
+
+//copy results (assuming we are in bags directory)
+$ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-14-25/rtabmap/stereo
+//or
+$ cp rtabmap.db gt_poses.txt rmse.txt slam_poses.txt ../results/stata_mit/2012-01-25-12-33-29/rtabmap/stereo
+

launch/jfr2018/pose_to_odom.py

@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+import rospy
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from nav_msgs.msg import Odometry
+
+def callback(data):
+    odom = Odometry()
+    odom.header = data.header
+    odom.child_frame_id = child_frame_id
+    odom.pose = data.pose
+    pub.publish(odom)
+
+if __name__ == '__main__':
+    rospy.init_node('pose_to_odom', anonymous=True)
+    pub = rospy.Publisher('odom_combined', Odometry, queue_size=1)
+    child_frame_id = rospy.get_param('~child_frame_id', "base_footprint")
+    rospy.Subscriber("/robot_pose_ekf/odom_combined", PoseWithCovarianceStamped, callback)
+    rospy.spin()

launch/jfr2018/pr2.lua

@@ -0,0 +1,46 @@
+-- Copyright 2016 The Cartographer Authors
+--
+-- 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.
+
+include "map_builder.lua"
+include "trajectory_builder.lua"
+
+options = {
+  map_builder = MAP_BUILDER,
+  trajectory_builder = TRAJECTORY_BUILDER,
+  map_frame = "map",
+  tracking_frame = "base_footprint",
+  published_frame = "base_footprint",
+  odom_frame = "odom",
+  provide_odom_frame = false,
+  use_odometry = true,
+  num_laser_scans = 1,
+  num_multi_echo_laser_scans = 0,
+  num_subdivisions_per_laser_scan = 1,
+  num_point_clouds = 0,
+  lookup_transform_timeout_sec = 0.2,
+  submap_publish_period_sec = 0.3,
+  pose_publish_period_sec = 5e-3,
+  trajectory_publish_period_sec = 30e-3,
+}
+
+MAP_BUILDER.use_trajectory_builder_2d = true
+
+TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
+TRAJECTORY_BUILDER_2D.use_imu_data = false
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.15
+TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.angular_search_window = math.rad(35.)
+
+SPARSE_POSE_GRAPH.optimization_problem.huber_scale = 1e2
+
+return options

launch/jfr2018/republish_camera_info.py

@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+import rospy
+from sensor_msgs.msg import CameraInfo
+
+def callback(data):
+    P = list(data.P);
+    P[3] = P[3] * scale
+    data.P = tuple(P);
+    pub.publish(data)
+
+if __name__ == '__main__':
+    rospy.init_node('republish_camera_info', anonymous=True)
+    pub = rospy.Publisher('camera_info_out', CameraInfo, queue_size=1)
+    scale = rospy.get_param('~scale', 1.091664)
+    rospy.Subscriber("camera_info_in", CameraInfo, callback)
+    rospy.spin()

launch/jfr2018/republish_scan.py

@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+import rospy
+from sensor_msgs.msg import LaserScan
+
+def callback(data):
+    t = rospy.Time(data.header.stamp.secs, data.header.stamp.nsecs)
+    t+=rospy.Duration.from_sec(offset)
+    data.header.stamp = t
+    pub.publish(data)
+
+if __name__ == '__main__':
+    rospy.init_node('republish_scan', anonymous=True)
+    pub = rospy.Publisher('base_scan_t', LaserScan, queue_size=1)
+    offset = rospy.get_param('~offset', 0.0)
+    rospy.Subscriber("base_scan", LaserScan, callback)
+    rospy.spin()

launch/jfr2018/scan_filter.yaml

@@ -0,0 +1,6 @@
+scan_filter_chain:
+- name: range
+  type: LaserScanRangeFilter
+  params:
+    lower_threshold: 0
+    upper_threshold: 5.6

launch/jfr2018/slam_gmapping.patch

@@ -0,0 +1,127 @@
+diff --git a/gmapping/src/slam_gmapping.cpp b/gmapping/src/slam_gmapping.cpp
+index bd9977c..b4f9382 100644
+--- a/gmapping/src/slam_gmapping.cpp
++++ b/gmapping/src/slam_gmapping.cpp
+@@ -114,6 +114,7 @@ Initial map dimensions and resolution:
+ #include "ros/ros.h"
+ #include "ros/console.h"
+ #include "nav_msgs/MapMetaData.h"
++#include <nav_msgs/Path.h>
+ 
+ #include "gmapping/sensor/sensor_range/rangesensor.h"
+ #include "gmapping/sensor/sensor_odometry/odometrysensor.h"
+@@ -258,6 +259,7 @@ void SlamGMapping::startLiveSlam()
+   entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
+   sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
+   sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
++  pathPub_ = node_.advertise<nav_msgs::Path>("mapPath", 1, true);
+   ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
+   scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
+   scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
+@@ -273,6 +275,7 @@ void SlamGMapping::startReplay(const std::string & bag_fname, std::string scan_t
+   entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
+   sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
+   sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
++  pathPub_ = node_.advertise<nav_msgs::Path>("map_path", 1, true);
+   ss_ = node_.advertiseService("dynamic_map", &SlamGMapping::mapCallback, this);
+   
+   rosbag::Bag bag;
+@@ -410,6 +413,18 @@ SlamGMapping::initMapper(const sensor_msgs::LaserScan& scan)
+     return false;
+   }
+ 
++  try
++    {
++	  	tf_.lookupTransform(laser_frame_, base_frame_, scan.header.stamp, scan_to_base_);
++	  	scan_to_base_.getOrigin().m_floats[2] = 0;
++    }
++    catch(tf::TransformException e)
++    {
++      ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
++               e.what());
++      return false;
++    }
++
+   // create a point 1m above the laser position and transform it into the laser-frame
+   tf::Vector3 v;
+   v.setValue(0, 0, 1 + laser_pose.getOrigin().z());
+@@ -617,6 +632,8 @@ SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
+     ROS_DEBUG("scan processed");
+ 
+     GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
++    GMapping::GridSlamProcessor::TNode * node = gsp_->getParticles()[gsp_->getBestParticleIndex()].node;
++
+     ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
+     ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
+     ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
+@@ -699,6 +716,23 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
+                                 delta_);
+ 
+   ROS_DEBUG("Trajectory tree:");
++  nav_msgs::Path path;
++  int count = 0;
++  for(GMapping::GridSlamProcessor::TNode* n = best.node;
++        n;
++        n = n->parent)
++    {
++      if(!n->reading)
++      {
++        ROS_DEBUG("Reading is NULL");
++        continue;
++      }
++	  ++count;
++    }
++  path.poses.resize(count);
++  int oi = path.poses.size()-1;
++
++
+   for(GMapping::GridSlamProcessor::TNode* n = best.node;
+       n;
+       n = n->parent)
+@@ -712,6 +746,13 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
+       ROS_DEBUG("Reading is NULL");
+       continue;
+     }
++    path.poses[oi].header.frame_id = map_frame_;
++    path.poses[oi].header.stamp = ros::Time(n->reading->getTime());
++
++    tf::Transform tmp = tf::Transform(tf::createQuaternionFromRPY(0, 0, n->pose.theta), tf::Vector3(n->pose.x, n->pose.y, 0))*scan_to_base_;
++    tf::poseTFToMsg(tmp, path.poses[oi].pose);
++    --oi;
++
+     matcher.invalidateActiveArea();
+     matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
+     matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
+@@ -764,8 +805,11 @@ SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
+   map_.map.header.stamp = ros::Time::now();
+   map_.map.header.frame_id = tf_.resolve( map_frame_ );
+ 
++  path.header = map_.map.header;
++
+   sst_.publish(map_.map);
+   sstm_.publish(map_.map.info);
++  pathPub_.publish(path);
+ }
+ 
+ bool 
+diff --git a/gmapping/src/slam_gmapping.h b/gmapping/src/slam_gmapping.h
+index ae622b9..8d84645 100644
+--- a/gmapping/src/slam_gmapping.h
++++ b/gmapping/src/slam_gmapping.h
+@@ -53,6 +53,7 @@ class SlamGMapping
+     ros::Publisher entropy_publisher_;
+     ros::Publisher sst_;
+     ros::Publisher sstm_;
++    ros::Publisher pathPub_;
+     ros::ServiceServer ss_;
+     tf::TransformListener tf_;
+     message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
+@@ -92,6 +93,8 @@ class SlamGMapping
+     std::string map_frame_;
+     std::string odom_frame_;
+ 
++    tf::StampedTransform scan_to_base_;
++
+     void updateMap(const sensor_msgs::LaserScan& scan);
+     bool getOdomPose(GMapping::OrientedPoint& gmap_pose, const ros::Time& t);
+     bool initMapper(const sensor_msgs::LaserScan& scan);

launch/jfr2018/slam_karto.patch

@@ -0,0 +1,118 @@
+diff --git a/src/slam_karto.cpp b/src/slam_karto.cpp
+index 712a9ca..0c0d885 100644
+--- a/src/slam_karto.cpp
++++ b/src/slam_karto.cpp
+@@ -68,7 +68,7 @@ class SlamKarto
+     bool updateMap();
+     void publishTransform();
+     void publishLoop(double transform_publish_period);
+-    void publishGraphVisualization();
++    void publishGraphVisualization(const ros::Time & stamp);
+ 
+     // ROS handles
+     ros::NodeHandle node_;
+@@ -435,16 +435,22 @@ SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)
+ }
+ 
+ void
+-SlamKarto::publishGraphVisualization()
++SlamKarto::publishGraphVisualization(const ros::Time & stamp)
+ {
+   std::vector<float> graph;
+   solver_->getGraph(graph);
+ 
++  std::vector<karto::LocalizedRangeScan*> scans = mapper_->GetAllProcessedScans();
++
++  if(scans.empty())
++  {
++	  return;
++  }
+   visualization_msgs::MarkerArray marray;
+ 
+   visualization_msgs::Marker m;
+   m.header.frame_id = "map";
+-  m.header.stamp = ros::Time::now();
++  m.header.stamp = stamp;
+   m.id = 0;
+   m.ns = "karto";
+   m.type = visualization_msgs::Marker::SPHERE;
+@@ -462,7 +468,7 @@ SlamKarto::publishGraphVisualization()
+ 
+   visualization_msgs::Marker edge;
+   edge.header.frame_id = "map";
+-  edge.header.stamp = ros::Time::now();
++  edge.header.stamp = ros::Time(scans.back()->GetTime());
+   edge.action = visualization_msgs::Marker::ADD;
+   edge.ns = "karto";
+   edge.id = 0;
+@@ -477,14 +483,14 @@ SlamKarto::publishGraphVisualization()
+ 
+   m.action = visualization_msgs::Marker::ADD;
+   uint id = 0;
+-  for (uint i=0; i<graph.size()/2; i++) 
++  for (uint i=0; i<scans.size(); i++)
+   {
+     m.id = id;
+-    m.pose.position.x = graph[2*i];
+-    m.pose.position.y = graph[2*i+1];
++    m.pose.position.x = scans[i]->GetCorrectedPose().GetX();
++    m.pose.position.y = scans[i]->GetCorrectedPose().GetY();
+     marray.markers.push_back(visualization_msgs::Marker(m));
+     id++;
+-
++/*
+     if(i>0)
+     {
+       edge.points.clear();
+@@ -500,15 +506,15 @@ SlamKarto::publishGraphVisualization()
+ 
+       marray.markers.push_back(visualization_msgs::Marker(edge));
+       id++;
+-    }
++    }*/
+   }
+-
++/*
+   m.action = visualization_msgs::Marker::DELETE;
+   for (; id < marker_count_; id++) 
+   {
+     m.id = id;
+     marray.markers.push_back(visualization_msgs::Marker(m));
+-  }
++  }*/
+ 
+   marker_count_ = marray.markers.size();
+ 
+@@ -537,12 +543,14 @@ SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
+   karto::Pose2 odom_pose;
+   if(addScan(laser, scan, odom_pose))
+   {
+-    ROS_DEBUG("added scan at pose: %.3f %.3f %.3f", 
++    ROS_INFO("added scan at pose: %.3f %.3f %.3f", 
+               odom_pose.GetX(),
+               odom_pose.GetY(),
+               odom_pose.GetHeading());
+ 
+-    publishGraphVisualization();
++    publishGraphVisualization(scan->header.stamp);
++
++    ROS_INFO("published markers");
+ 
+     if(!got_map_ || 
+        (scan->header.stamp - last_map_update) > map_update_interval_)
+diff --git a/src/spa_solver.cpp b/src/spa_solver.cpp
+index 5d9a962..6a65211 100644
+--- a/src/spa_solver.cpp
++++ b/src/spa_solver.cpp
+@@ -46,9 +46,9 @@ void SpaSolver::Compute()
+ 
+   typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> > NodeVector;
+ 
+-  ROS_INFO("Calling doSPA for loop closure");
++  //ROS_INFO("Calling doSPA for loop closure");
+   m_Spa.doSPA(40);
+-  ROS_INFO("Finished doSPA for loop closure");
++  //ROS_INFO("Finished doSPA for loop closure");
+   NodeVector nodes = m_Spa.getNodes();
+   forEach(NodeVector, &nodes)
+   {

launch/jfr2018/slam_karto_sync_markers_gt.py

@@ -0,0 +1,83 @@
+#!/usr/bin/env python  
+import roslib
+import rospy
+import os
+import tf
+import numpy
+import evaluate_ate
+from visualization_msgs.msg import MarkerArray
+from geometry_msgs.msg import Point
+
+def callback(data):
+    global slamPoses
+    global gtPoses
+    global stamps
+    global listener
+    global lastSize
+    global slamPosesInd
+    global rmse
+  
+    point_markers = []
+    for m in data.markers:
+        if m.type==2:
+            point_markers.append(m) 
+
+    if len(point_markers) > 0 and lastSize != len(point_markers):
+        t = rospy.Time(point_markers[0].header.stamp.secs, point_markers[0].header.stamp.nsecs)
+        try:
+            listener.waitForTransform(fixedFrame, baseFrame, t, rospy.Duration(0.2))
+            (trans,rot) = listener.lookupTransform(fixedFrame, baseFrame, t)
+            gtPoses.append(Point(trans[0], trans[1], 0))
+            stamps.append(t.to_sec())
+            slamPoses.append(point_markers[len(point_markers)-1].pose.position)
+            slamPosesInd.append(len(point_markers)-1)
+            first_xyz = numpy.empty([0,3])
+            second_xyz = numpy.empty([0,3])
+            for g in gtPoses:
+                newrow = [g.x,g.y,g.z]
+                first_xyz = numpy.vstack([first_xyz, newrow])
+            for p in slamPoses:
+                newrow = [p.x,p.y,p.z]
+                second_xyz = numpy.vstack([second_xyz, newrow])
+            first_xyz = numpy.matrix(first_xyz).transpose()
+            second_xyz = numpy.matrix(second_xyz).transpose()
+            rot,trans,trans_error = evaluate_ate.align(second_xyz, first_xyz)
+            rmse_v = numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
+            rmse.append(rmse_v)
+            print "points= " + str(len(point_markers)) + " added=" + str(len(slamPoses)) + " rmse=" + str(rmse_v)
+        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
+            print str(e)
+    if len(data.markers) > 0 and len(slamPosesInd) > 0:
+        j=0
+        for i in slamPosesInd:
+           slamPoses[j] = point_markers[i].pose.position
+           j+=1
+    if len(data.markers) > 0:
+        lastSize = len(point_markers)
+
+if __name__ == '__main__':
+    rospy.init_node('sync_markers_gt', anonymous=True)
+    listener = tf.TransformListener()
+    fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
+    baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
+    rospy.Subscriber("visualization_marker_array", MarkerArray, callback, queue_size=1)
+    slamPoses = []
+    gtPoses = []
+    stamps = []
+    slamPosesInd = []
+    rmse = []
+    lastSize = 0
+    rospy.spin()
+    fileSlam = open('slam_poses.txt','w')
+    fileGt = open('gt_poses.txt','w')
+    fileRMSE = open('rmse.txt','w')
+    print "slam= " + str(len(slamPoses))
+    print "gt= " + str(len(gtPoses))
+    print "stamps= " + str(len(stamps))
+    for c, g, t, r in zip(slamPoses, gtPoses, stamps, rmse):
+        fileSlam.write('%f %f %f 0 0 0 0 1\n' % (t, c.x, c.y))
+        fileGt.write('%f %f %f 0 0 0 0 1\n' % (t, g.x, g.y))
+        fileRMSE.write('%f %f\n' % (t, r))
+    fileSlam.close()
+    fileGt.close()
+    fileRMSE.close()

launch/jfr2018/sync_path_gt.py

@@ -0,0 +1,77 @@
+#!/usr/bin/env python  
+import roslib
+import rospy
+import os
+import tf
+import numpy
+import evaluate_ate
+from nav_msgs.msg import Path
+from geometry_msgs.msg import Pose
+from geometry_msgs.msg import Point
+
+def callback(data):
+    global slamPoses
+    global gtPoses
+    global stamps
+    global listener
+    global lastSize
+    global slamPosesInd
+    global rmse
+    if lastSize != len(data.poses):
+        t = rospy.Time(data.poses[len(data.poses)-1].header.stamp.secs, data.poses[len(data.poses)-1].header.stamp.nsecs)
+        try:
+            listener.waitForTransform(fixedFrame, baseFrame, t, rospy.Duration(0.2))
+            (trans,rot) = listener.lookupTransform(fixedFrame, baseFrame, t)
+            gtPoses.append(Pose(trans, rot))
+            stamps.append(t.to_sec())
+            slamPoses.append(data.poses[len(data.poses)-1].pose)
+            slamPosesInd.append(len(data.poses)-1)
+            first_xyz = numpy.empty([0,3])
+            second_xyz = numpy.empty([0,3])
+            for g in gtPoses:
+                newrow = [g.position[0],g.position[1],g.position[2]]
+                first_xyz = numpy.vstack([first_xyz, newrow])
+            for p in slamPoses:
+                newrow = [p.position.x,p.position.y,p.position.z]
+                second_xyz = numpy.vstack([second_xyz, newrow])
+            first_xyz = numpy.matrix(first_xyz).transpose()
+            second_xyz = numpy.matrix(second_xyz).transpose()
+            rot,trans,trans_error = evaluate_ate.align(second_xyz, first_xyz)
+            rmse_v = numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
+            rmse.append(rmse_v)
+            print "points= " + str(len(data.poses)) + " added=" + str(len(slamPoses)) + " rmse=" + str(rmse_v)
+        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException), e:
+            print str(e)
+    if len(data.poses) > 0 and len(slamPosesInd) > 0:
+        j=0
+        for i in slamPosesInd:
+           slamPoses[j] = data.poses[i].pose
+           j+=1
+    lastSize = len(data.poses)
+
+if __name__ == '__main__':
+    rospy.init_node('sync_path_gt', anonymous=True)
+    listener = tf.TransformListener()
+    fixedFrame = rospy.get_param('~fixed_frame_id', 'world')
+    baseFrame = rospy.get_param('~frame_id', 'base_link_gt')
+    rospy.Subscriber("mapPath", Path, callback, queue_size=1)
+    slamPoses = []
+    gtPoses = []
+    stamps = []
+    slamPosesInd = []
+    rmse = []
+    lastSize = 0
+    rospy.spin()
+    fileSlam = open('slam_poses.txt','w')
+    fileGt = open('gt_poses.txt','w')
+    fileRMSE = open('rmse.txt','w')
+    print "slam= " + str(len(slamPoses))
+    print "gt= " + str(len(gtPoses))
+    print "stamps= " + str(len(stamps))
+    for c, g, t, r in zip(slamPoses, gtPoses, stamps, rmse):
+        fileSlam.write('%f %f %f 0 0 0 0 1\n' % (t, c.position.x, c.position.y))
+        fileGt.write('%f %f %f 0 0 0 0 1\n' % (t, g.position[0], g.position[1]))
+        fileRMSE.write('%f %f\n' % (t, r))
+    fileSlam.close()
+    fileGt.close()
+    fileRMSE.close()

launch/jfr2018/throttle_bag.launch

@@ -0,0 +1,29 @@
+
+<launch>
+	<param name="use_sim_time" value="true"/>
+	<group ns="/wide_stereo" >
+	    	<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="standalone rtabmap_ros/stereo_throttle">
+		    <remap from="left/image"        to="left/image_raw"/>
+		    <remap from="right/image"       to="right/image_raw"/>
+		    <remap from="left/camera_info"  to="left/camera_info"/>
+		    <remap from="right/camera_info" to="right/camera_info"/>
+		    
+		    <param name="queue_size" type="int" value="10"/>
+		    <param name="rate" type="double" value="15"/>
+		</node>
+	</group>
+
+	<group ns="camera">
+	    <node pkg="nodelet" type="nodelet" name="data_throttle" args="standalone rtabmap_ros/data_throttle">
+	      <param name="rate" type="double" value="15.0"/>
+	   
+	      <remap from="rgb/image_in"       to="rgb/image_raw"/>
+	      <remap from="depth/image_in"     to="depth/image_raw"/>
+	      <remap from="rgb/camera_info_in" to="rgb/camera_info"/>
+	    
+	      <remap from="rgb/image_out"       to="rgb/image_raw_throttle"/>
+	      <remap from="depth/image_out"     to="depth/image_raw_throttle"/>
+	      <remap from="rgb/camera_info_out" to="rgb/camera_info_throttle"/>
+	    </node>  
+	</group> 
+</launch>