mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +08:00
merged master
This commit is contained in:
matlabbe
+20
-20
@@ -31,7 +31,7 @@
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
# Requirements:
|
||||
# Requirements:
|
||||
# sudo apt-get install python-argparse
|
||||
|
||||
"""
|
||||
@@ -48,31 +48,31 @@ import numpy
|
||||
|
||||
def read_file_list(filename):
|
||||
"""
|
||||
Reads a trajectory from a text file.
|
||||
|
||||
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.
|
||||
|
||||
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")
|
||||
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
|
||||
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
|
||||
@@ -81,13 +81,13 @@ def associate(first_list, second_list,offset,max_difference):
|
||||
|
||||
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
|
||||
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 = []
|
||||
@@ -96,15 +96,15 @@ def associate(first_list, second_list,offset,max_difference):
|
||||
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
|
||||
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)')
|
||||
@@ -116,7 +116,7 @@ if __name__ == '__main__':
|
||||
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))
|
||||
matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))
|
||||
|
||||
if args.first_only:
|
||||
for a,b in matches:
|
||||
@@ -124,5 +124,5 @@ if __name__ == '__main__':
|
||||
else:
|
||||
for a,b in matches:
|
||||
print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Copyright 2016 The Cartographer Authors
|
||||
|
||||
@@ -22,7 +23,7 @@
|
||||
type="cartographer_node" args="
|
||||
-configuration_directory
|
||||
$(find cartographer_ros)/configuration_files
|
||||
-configuration_basename pr2.lua"
|
||||
-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" />
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python
|
||||
import roslib
|
||||
import rospy
|
||||
import os
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python
|
||||
import roslib
|
||||
import rospy
|
||||
import os
|
||||
@@ -15,7 +15,7 @@ def callback(data):
|
||||
global listener
|
||||
global rmse
|
||||
global lastTime
|
||||
|
||||
|
||||
if rospy.get_time() - lastTime < 1:
|
||||
return
|
||||
lastTime = rospy.get_time()
|
||||
|
||||
@@ -9,17 +9,17 @@ index 67820e7..aec1839 100644
|
||||
+ 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:
|
||||
@@ -28,7 +28,7 @@ index 67820e7..aec1839 100644
|
||||
+# 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
|
||||
|
||||
@@ -31,7 +31,7 @@
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
# Requirements:
|
||||
# Requirements:
|
||||
# sudo apt-get install python-argparse
|
||||
|
||||
"""
|
||||
@@ -46,21 +46,21 @@ 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])
|
||||
@@ -70,18 +70,18 @@ def align(model,data):
|
||||
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.
|
||||
|
||||
Plot a trajectory using matplotlib.
|
||||
|
||||
Input:
|
||||
ax -- the plot
|
||||
stamps -- time stamps (1xn)
|
||||
@@ -89,7 +89,7 @@ def plot_traj(ax,stamps,traj,style,color,label):
|
||||
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])])
|
||||
@@ -108,12 +108,12 @@ def plot_traj(ax,stamps,traj,style,color,label):
|
||||
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.
|
||||
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)')
|
||||
@@ -129,7 +129,7 @@ if __name__=="__main__":
|
||||
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))
|
||||
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?")
|
||||
|
||||
@@ -137,18 +137,18 @@ if __name__=="__main__":
|
||||
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))
|
||||
|
||||
@@ -160,12 +160,12 @@ if __name__=="__main__":
|
||||
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)]))
|
||||
@@ -186,10 +186,10 @@ if __name__=="__main__":
|
||||
#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')
|
||||
|
||||
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python
|
||||
import roslib
|
||||
import rospy
|
||||
import os
|
||||
@@ -14,7 +14,7 @@ if __name__ == '__main__':
|
||||
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)
|
||||
gtFile = os.path.expanduser(gtFile)
|
||||
br = tf.TransformBroadcaster()
|
||||
|
||||
init_x = 0
|
||||
@@ -34,7 +34,7 @@ if __name__ == '__main__':
|
||||
init_x = x
|
||||
init_y = y
|
||||
init = True
|
||||
|
||||
|
||||
x -= init_x
|
||||
y -= init_y
|
||||
|
||||
|
||||
@@ -15,7 +15,7 @@ odom_frame_id:="odom_combined" odom_tf_angular_variance:=0.0001 odom_tf_linear_v
|
||||
//neighbor link refine
|
||||
--RGBD/NeighborLinkRefining true --RGBD/OptimizeMaxError 1.5
|
||||
// odom frame to frame
|
||||
--Odom/Strategy 1
|
||||
--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
|
||||
@@ -23,7 +23,7 @@ $ rosrun laser_filters scan_to_scan_filter_chain scan:=/base_scan_t scan_filtere
|
||||
$ ./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
|
||||
$ ./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
|
||||
|
||||
@@ -66,7 +66,7 @@ $ rosbag play --clock --pause ./stata-mit/2012-01-25-12-14-25.bag
|
||||
//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
|
||||
// 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
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
// 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
|
||||
$ 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
|
||||
// flow
|
||||
--Vis/CorType 1 --Odom/KeyFrameThr 0.6 --Vis/BundleAdjustment 0
|
||||
// robot_localization
|
||||
odom_topic:=/odometry/filtered visual_odometry:=false approx_sync:=true
|
||||
|
||||
@@ -1,11 +1,11 @@
|
||||
// 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
|
||||
$ 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
|
||||
// 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
|
||||
$ ./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
|
||||
|
||||
@@ -7,7 +7,7 @@ index bd9977c..b4f9382 100644
|
||||
#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()
|
||||
@@ -24,12 +24,12 @@ index bd9977c..b4f9382 100644
|
||||
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_);
|
||||
@@ -47,7 +47,7 @@ index bd9977c..b4f9382 100644
|
||||
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;
|
||||
+
|
||||
@@ -56,7 +56,7 @@ index bd9977c..b4f9382 100644
|
||||
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;
|
||||
@@ -95,15 +95,15 @@ index bd9977c..b4f9382 100644
|
||||
@@ -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
|
||||
|
||||
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
|
||||
@@ -119,7 +119,7 @@ index ae622b9..8d84645 100644
|
||||
@@ -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);
|
||||
|
||||
@@ -8,19 +8,19 @@ index 712a9ca..0c0d885 100644
|
||||
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())
|
||||
@@ -28,7 +28,7 @@ index 712a9ca..0c0d885 100644
|
||||
+ return;
|
||||
+ }
|
||||
visualization_msgs::MarkerArray marray;
|
||||
|
||||
|
||||
visualization_msgs::Marker m;
|
||||
m.header.frame_id = "map";
|
||||
- m.header.stamp = ros::Time::now();
|
||||
@@ -37,7 +37,7 @@ index 712a9ca..0c0d885 100644
|
||||
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();
|
||||
@@ -46,10 +46,10 @@ index 712a9ca..0c0d885 100644
|
||||
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<graph.size()/2; i++)
|
||||
+ for (uint i=0; i<scans.size(); i++)
|
||||
{
|
||||
m.id = id;
|
||||
@@ -65,7 +65,7 @@ index 712a9ca..0c0d885 100644
|
||||
{
|
||||
edge.points.clear();
|
||||
@@ -500,15 +506,15 @@ SlamKarto::publishGraphVisualization()
|
||||
|
||||
|
||||
marray.markers.push_back(visualization_msgs::Marker(edge));
|
||||
id++;
|
||||
- }
|
||||
@@ -74,40 +74,40 @@ index 712a9ca..0c0d885 100644
|
||||
-
|
||||
+/*
|
||||
m.action = visualization_msgs::Marker::DELETE;
|
||||
for (; id < marker_count_; id++)
|
||||
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",
|
||||
- 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_ ||
|
||||
|
||||
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);
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python
|
||||
import roslib
|
||||
import rospy
|
||||
import os
|
||||
@@ -16,11 +16,11 @@ def callback(data):
|
||||
global lastSize
|
||||
global slamPosesInd
|
||||
global rmse
|
||||
|
||||
|
||||
point_markers = []
|
||||
for m in data.markers:
|
||||
if m.type==2:
|
||||
point_markers.append(m)
|
||||
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)
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#!/usr/bin/env python
|
||||
#!/usr/bin/env python
|
||||
import roslib
|
||||
import rospy
|
||||
import os
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<launch>
|
||||
<param name="use_sim_time" value="true"/>
|
||||
@@ -7,7 +8,7 @@
|
||||
<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>
|
||||
@@ -16,14 +17,14 @@
|
||||
<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>
|
||||
</node>
|
||||
</group>
|
||||
</launch>
|
||||
|
||||
Reference in New Issue
Block a user