Posted by
dgmiller on
URL: http://official-rtab-map-forum.206.s1.nabble.com/rtabmap-process-error-tp1031.html
Hi
I'm using rtabmap (installed with apt-get install ros-indigo-rtabmap-ros) with robot_localization node, which provides the transforms from odom to base_link and map to odom.
I'm having some error and I've not been able to get everything running correctly, does anyone see anything I'm missing or a source of this error? I've included the output and the launch file i'm using.
Starting RTABMAP...
... logging to /home/ubuntu/.ros/log/de84fd10-c5e6-11e5-8b8d-0005b7e12d1d/roslaunch-tegra-ubuntu-5254.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
started roslaunch server http://tegra-ubuntu:59511/
SUMMARY
========
PARAMETERS
* /ORB/Gpu: True
* /points_xyzrgb/decimation: 2.0
* /points_xyzrgb/voxel_size: 0.02
* /rosdistro: indigo
* /rosversion: 1.11.16
* /rtabmap/rgbd_odometry/Odom/EstimationType: 1
* /rtabmap/rgbd_odometry/Odom/FeatureType: 2
* /rtabmap/rgbd_odometry/Odom/FillInfoData: true
* /rtabmap/rgbd_odometry/Odom/InlierDistance: 0.1
* /rtabmap/rgbd_odometry/Odom/MaxDepth: 10
* /rtabmap/rgbd_odometry/Odom/MinInliers: 8
* /rtabmap/rgbd_odometry/Odom/Strategy: 0
* /rtabmap/rgbd_odometry/Odom/VarianceFromInliersCount: true
* /rtabmap/rgbd_odometry/OdomBow/LocalHistorySize: 1200
* /rtabmap/rgbd_odometry/OdomBow/NNType: 3
* /rtabmap/rgbd_odometry/frame_id: base_link
* /rtabmap/rgbd_odometry/publish_tf: False
* /rtabmap/rgbd_odometry/wait_for_transform_duration: 5.0
* /rtabmap/rtabmap/LccBow/EstimationType: 1
* /rtabmap/rtabmap/LccBow/InlierDistance: 0.1
* /rtabmap/rtabmap/LccBow/MinInliers: 10
* /rtabmap/rtabmap/LccBow/VarianceFromInliersCount: true
* /rtabmap/rtabmap/LccIcp/Type: 2
* /rtabmap/rtabmap/LccIcp2/CorrespondenceRatio: 0.25
* /rtabmap/rtabmap/Mem/SaveDepth16Format: true
* /rtabmap/rtabmap/RGBD/LocalLoopDetectionSpace: true
* /rtabmap/rtabmap/RGBD/OptimizeFromGraphEnd: false
* /rtabmap/rtabmap/RGBD/OptimizeSlam2D: true
* /rtabmap/rtabmap/Rtabmap/TimeThr: 0
* /rtabmap/rtabmap/database_path: ~/.ros/rtabmap.db
* /rtabmap/rtabmap/frame_id: base_link
* /rtabmap/rtabmap/publish_tf: False
* /rtabmap/rtabmap/subscribe_depth: True
* /rtabmap/rtabmap/subscribe_laserScan: True
* /rtabmap/rtabmap/wait_for_transform_duration: 5.0
NODES
/rtabmap/
rgbd_odometry (rtabmap_ros/rgbd_odometry)
rtabmap (rtabmap_ros/rtabmap)
/
base_link_to_camera_link_rgb (tf/static_transform_publisher)
base_link_to_imu_link (tf/static_transform_publisher)
base_link_to_lidar_link (tf/static_transform_publisher)
camera_link_rgb_to_camera_link_depth (tf/static_transform_publisher)
data_odom_sync (nodelet/nodelet)
points_xyzrgb (nodelet/nodelet)
rviz (rviz/rviz)
standalone_nodelet (nodelet/nodelet)
ROS_MASTER_URI=http://rcu:11311
core service [/rosout] found
process[rtabmap/rgbd_odometry-1]: started with pid [5263]
process[rtabmap/rtabmap-2]: started with pid [5264]
process[rviz-3]: started with pid [5265]
process[standalone_nodelet-4]: started with pid [5266]
process[data_odom_sync-5]: started with pid [5267]
process[points_xyzrgb-6]: started with pid [5268]
process[base_link_to_camera_link_rgb-7]: started with pid [5269]
process[camera_link_rgb_to_camera_link_depth-8]: started with pid [5270]
process[base_link_to_imu_link-9]: started with pid [5278]
[ INFO] [1454003433.618625017]: Initializing nodelet with 4 worker threads.
process[base_link_to_lidar_link-10]: started with pid [5284]
[ INFO] [1454003434.414070736]: Starting node...
[ INFO] [1454003434.637458068]: rtabmap: frame_id = base_link
[ INFO] [1454003434.637987541]: rtabmap: map_frame_id = map
[ INFO] [1454003434.638446026]: rtabmap: queue_size = 10
[ INFO] [1454003434.638861021]: rtabmap: tf_delay = 0.050000
[ INFO] [1454003434.639309818]: rtabmap: depth_cameras = 1
[ INFO] [1454003434.939830936]: Approximate time sync = true
[ INFO] [1454003435.116408158]: Setting RTAB-Map parameter "LccBow/EstimationType"="1"
[ INFO] [1454003435.127690846]: Setting RTAB-Map parameter "LccBow/InlierDistance"="0.1"
[ INFO] [1454003435.142697399]: Setting RTAB-Map parameter "LccBow/MinInliers"="10"
[ INFO] [1454003435.172621341]: Setting RTAB-Map parameter "LccBow/VarianceFromInliersCount"="true"
[ INFO] [1454003435.192843878]: Setting RTAB-Map parameter "LccIcp/Type"="2"
[ INFO] [1454003435.197244559]: Setting RTAB-Map parameter "LccIcp2/CorrespondenceRatio"="0.25"
[ INFO] [1454003435.554775465]: Setting RTAB-Map parameter "Mem/SaveDepth16Format"="true"
[ INFO] [1454003435.644204845]: Setting odometry parameter "Odom/EstimationType"="1"
[ INFO] [1454003435.649044011]: Setting odometry parameter "Odom/FeatureType"="2"
[ INFO] [1454003435.652543451]: Setting odometry parameter "Odom/FillInfoData"="true"
[ INFO] [1454003435.699107678]: Setting odometry parameter "Odom/InlierDistance"="0.1"
[ INFO] [1454003435.711999671]: Setting odometry parameter "Odom/MaxDepth"="10"
[ INFO] [1454003435.721044103]: Setting odometry parameter "Odom/MinInliers"="8"
[ INFO] [1454003435.833681196]: Setting odometry parameter "Odom/Strategy"="0"
[ INFO] [1454003435.836902358]: Setting odometry parameter "Odom/VarianceFromInliersCount"="true"
[ INFO] [1454003435.854360968]: Setting odometry parameter "OdomBow/LocalHistorySize"="1200"
[ INFO] [1454003435.876393538]: Setting odometry parameter "OdomBow/NNType"="3"
[ INFO] [1454003436.211979894]: Setting RTAB-Map parameter "RGBD/LocalLoopDetectionSpace"="true"
[ INFO] [1454003436.276944596]: Setting RTAB-Map parameter "RGBD/OptimizeFromGraphEnd"="false"
[ INFO] [1454003436.310375425]: Setting RTAB-Map parameter "RGBD/OptimizeSlam2D"="true"
[ INFO] [1454003436.567685508]: Setting RTAB-Map parameter "Rtabmap/TimeThr"="0"
[ INFO] [1454003436.707835371]: Using OdometryBOW
[ INFO] [1454003437.252283333]:
/rtabmap/rgbd_odometry subscribed to:
/rgb/image_raw,
/depth/image_raw,
/camera/rgb/camera_info
[ INFO] [1454003437.447158947]: RTAB-Map rate detection = 1.000000 Hz
[ INFO] [1454003437.447444361]: rtabmap: Deleted database "/home/ubuntu/.ros/rtabmap.db" (--delete_db_on_start is set).
[ INFO] [1454003437.447493423]: rtabmap: Using database from "/home/ubuntu/.ros/rtabmap.db".
[ INFO] [1454003437.597778642]: rtabmap: Database version = "0.10.10".
[ INFO] [1454003437.782211663]: Odom: quality=0, std dev=1.000000m, update time=0.270015s
[ INFO] [1454003438.063291432]: Odom: quality=598, std dev=0.040893m, update time=0.263357s
[ INFO] [1454003438.150913748]: Registering Depth+LaserScan callback...
[ INFO] [1454003438.170949413]:
/rtabmap/rtabmap subscribed to:
/rgb/image_raw,
/depth/image_raw,
/camera/rgb/camera_info,
/rtabmap/odom,
/scan
[ INFO] [1454003438.171391856]: rtabmap 0.10.10 started...
[ INFO] [1454003438.280886588]: Odom: quality=743, std dev=0.036686m, update time=0.204283s
[ INFO] [1454003438.548614160]: Odom: quality=789, std dev=0.035601m, update time=0.256528s
[ INFO] [1454003438.764788446]: Odom: quality=826, std dev=0.034794m, update time=0.203122s
[ INFO] [1454003438.995827151]: Odom: quality=819, std dev=0.034943m, update time=0.220147s
[ INFO] [1454003439.216630032]: Odom: quality=827, std dev=0.034773m, update time=0.196765s
[ INFO] [1454003439.294480528]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.2766s, Pub=0.0003s (local map=1, WM=1)
[ INFO] [1454003439.449376479]: Odom: quality=842, std dev=0.034462m, update time=0.218309s
[ INFO] [1454003439.722004413]: Odom: quality=845, std dev=0.034401m, update time=0.260282s
[ INFO] [1454003439.955540487]: Odom: quality=846, std dev=0.034381m, update time=0.218447s
[ INFO] [1454003440.234688142]: Odom: quality=853, std dev=0.034239m, update time=0.263945s
[ INFO] [1454003440.539707594]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.2933s, Pub=0.0029s (local map=2, WM=2)
[ INFO] [1454003440.647702655]: Odom: quality=853, std dev=0.034239m, update time=0.378670s
[ INFO] [1454003440.944696155]: Odom: quality=844, std dev=0.034421m, update time=0.280578s
[ INFO] [1454003441.194215646]: Odom: quality=858, std dev=0.034139m, update time=0.213585s
[ INFO] [1454003441.424423943]: Odom: quality=862, std dev=0.034060m, update time=0.216162s
[ INFO] [1454003441.643553509]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.2094s, Pub=0.0028s (local map=3, WM=3)
[ INFO] [1454003441.722403057]: Odom: quality=857, std dev=0.034159m, update time=0.286194s
[ INFO] [1454003442.016757992]: Odom: quality=846, std dev=0.034381m, update time=0.278563s
[ INFO] [1454003442.322764048]: Odom: quality=851, std dev=0.034280m, update time=0.292477s
[ INFO] [1454003442.563848945]: Odom: quality=859, std dev=0.034120m, update time=0.228997s
[ INFO] [1454003442.893571355]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.3344s, Pub=0.0024s (local map=4, WM=4)
[ INFO] [1454003442.897631675]: Odom: quality=856, std dev=0.034179m, update time=0.277875s
[ INFO] [1454003443.254344829]: Odom: quality=857, std dev=0.034159m, update time=0.316897s
[ INFO] [1454003443.588625990]: Odom: quality=839, std dev=0.034524m, update time=0.302811s
[ INFO] [1454003443.836702372]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.2195s, Pub=0.0039s (local map=5, WM=5)
[ INFO] [1454003443.960814859]: Odom: quality=873, std dev=0.033845m, update time=0.332508s
[ INFO] [1454003444.299921905]: Odom: quality=856, std dev=0.034179m, update time=0.311066s
[ INFO] [1454003444.546244817]: Odom: quality=873, std dev=0.033845m, update time=0.229930s
[ INFO] [1454003444.896702519]: Odom: quality=856, std dev=0.034179m, update time=0.265016s
[ INFO] [1454003445.138748915]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.2387s, Pub=0.0035s (local map=6, WM=6)
[ INFO] [1454003445.158346980]: Odom: quality=847, std dev=0.034360m, update time=0.245244s
[ INFO] [1454003445.398133818]: Odom: quality=872, std dev=0.033864m, update time=0.225927s
[ INFO] [1454003445.815288494]: Odom: quality=858, std dev=0.034139m, update time=0.385810s
[ INFO] [1454003446.060735269]: Odom: quality=848, std dev=0.034340m, update time=0.231690s
[ INFO] [1454003446.337693416]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.2664s, Pub=0.0032s (local map=7, WM=7)
[ INFO] [1454003446.415500059]: Odom: quality=861, std dev=0.034080m, update time=0.281957s
[ INFO] [1454003446.754204661]: Odom: quality=867, std dev=0.033962m, update time=0.325091s
[ INFO] [1454003447.040904784]: Odom: quality=881, std dev=0.033691m, update time=0.269118s
[ INFO] [1454003447.322784647]: Odom: quality=867, std dev=0.033962m, update time=0.253321s
[ INFO] [1454003447.599087124]: Odom: quality=849, std dev=0.034320m, update time=0.262256s
[ INFO] [1454003447.643883402]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.3046s, Pub=0.0029s (local map=8, WM=8)
[ INFO] [1454003447.925344103]: Odom: quality=880, std dev=0.033710m, update time=0.284981s
[ INFO] [1454003448.237528424]: Odom: quality=856, std dev=0.034179m, update time=0.256861s
[ INFO] [1454003448.484500912]: Odom: quality=850, std dev=0.034300m, update time=0.233491s
[ INFO] [1454003448.805798153]: Odom: quality=861, std dev=0.034080m, update time=0.275600s
[ INFO] [1454003448.880540924]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.3749s, Pub=0.0049s (local map=9, WM=9)
[ INFO] [1454003449.129034853]: Odom: quality=864, std dev=0.034021m, update time=0.305907s
[ INFO] [1454003449.442775303]: Odom: quality=874, std dev=0.033826m, update time=0.288522s
[ INFO] [1454003449.739601043]: Odom: quality=877, std dev=0.033768m, update time=0.264686s
[ INFO] [1454003450.009105886]: Odom: quality=865, std dev=0.034001m, update time=0.254554s
[ INFO] [1454003450.061208437]: rtabmap: Rate=1.00s, Limit=0.000s, RTAB-Map=0.3104s, Pub=0.0026s (local map=10, WM=10)
[ INFO] [1454003450.360625190]: Odom: quality=864, std dev=0.034021m, update time=0.279793s
[ INFO] [1454003450.626431325]: Odom: quality=865, std dev=0.034001m, update time=0.245982s
[ INFO] [1454003450.880701336]: Odom: quality=868, std dev=0.033942m, update time=0.238697s
[ INFO] [1454003451.136993668]: Odom: quality=865, std dev=0.034001m, update time=0.242286s
[ INFO] [1454003451.442077494]: Odom: quality=876, std dev=0.033787m, update time=0.287708s
[rtabmap/rtabmap-2] process has died [pid 5264, exit code -7, cmd /opt/ros/indigo/lib/rtabmap_ros/rtabmap --delete_db_on_start rgb/image:=/rgb/image_raw depth/image:=/depth/image_raw rgb/camera_info:=/camera/rgb/camera_info scan:=/scan __name:=rtabmap __log:=/home/ubuntu/.ros/log/de84fd10-c5e6-11e5-8b8d-0005b7e12d1d/rtabmap-rtabmap-2.log].
log file: /home/ubuntu/.ros/log/de84fd10-c5e6-11e5-8b8d-0005b7e12d1d/rtabmap-rtabmap-2*.log
[ INFO] [1454003451.795017096]: Odom: quality=861, std dev=0.034080m, update time=0.325295s
[ INFO] [1454003452.060178289]: Odom: quality=856, std dev=0.034179m, update time=0.247488s
[ INFO] [1454003452.330763953]: Odom: quality=882, std dev=0.033672m, update time=0.255222s
[ INFO] [1454003452.629505714]: Odom: quality=883, std dev=0.033653m, update time=0.279952s
[ INFO] [1454003452.905441580]: Odom: quality=837, std dev=0.034565m, update time=0.260659s
[ INFO] [1454003453.193096848]: Odom: quality=830, std dev=0.034711m, update time=0.272320s
[ INFO] [1454003453.468929538]: Odom: quality=805, std dev=0.035245m, update time=0.259039s
[ INFO] [1454003453.728837976]: Odom: quality=785, std dev=0.035692m, update time=0.246060s
[ INFO] [1454003453.975332031]: Odom: quality=793, std dev=0.035511m, update time=0.229520s
[ INFO] [1454003454.227449305]: Odom: quality=792, std dev=0.035533m, update time=0.239423s
[ INFO] [1454003454.458918838]: Odom: quality=797, std dev=0.035422m, update time=0.218938s
[ INFO] [1454003454.747518315]: Odom: quality=777, std dev=0.035875m, update time=0.272971s
[ INFO] [1454003455.008956424]: Odom: quality=827, std dev=0.034773m, update time=0.224598s
[ INFO] [1454003455.249694188]: Odom: quality=841, std dev=0.034483m, update time=0.225562s
[ INFO] [1454003455.484237958]: Odom: quality=810, std dev=0.035136m, update time=0.220540s
[ INFO] [1454003455.762137708]: Odom: quality=832, std dev=0.034669m, update time=0.263387s
[ INFO] [1454003456.023034677]: Odom: quality=841, std dev=0.034483m, update time=0.235387s
[ INFO] [1454003456.254209369]: Odom: quality=848, std dev=0.034340m, update time=0.217289s
[ INFO] [1454003456.514968423]: Odom: quality=850, std dev=0.034300m, update time=0.245986s
Here is our Launch File for RTABMAP:
<launch>
<!-- Your RGB-D sensor should be already started with "depth_registration:=true".
Examples:
$ roslaunch freenect_launch freenect.launch depth_registration:=true
$ roslaunch openni2_launch openni2.launch depth_registration:=true -->
<!-- Choose visualization -->
<arg name="rviz" default="true" />
<arg name="rtabmapviz" default="false" />
<!-- Corresponding config files -->
<arg name="rtabmapviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" />
<arg name="rviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
<arg name="frame_id" default="base_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="time_threshold" default="0"/> <!-- (ms) If not 0 ms, memory management is used to keep processing time on this fixed limit. -->
<arg name="optimize_from_last_node" default="false"/> <!-- Optimize the map from the last node. Should be true on multi-session mapping and when time threshold is set -->
<arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="rtabmap_args" default="--delete_db_on_start"/> <!-- delete_db_on_start, udebug -->
<arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
<arg name="rgb_topic" default="/rgb/image_raw" />
<arg name="depth_registered_topic" default="/depth/image_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<arg name="compressed" default="false"/>
<arg name="convert_depth_to_mm" default="true"/>
<arg name="subscribe_scan" default="true"/> <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
<arg name="scan_topic" default="/scan"/>
<arg name="visual_odometry" default="true"/> <!-- Generate visual odometry -->
<arg name="odom_topic" default="/odom"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="namespace" default="rtabmap"/>
<arg name="wait_for_transform" default="5"/>
<!-- Odometry parameters: -->
<arg name="strategy" default="0" /> <!-- Strategy: 0=BOW (bag-of-words) 1=Optical Flow -->
<arg name="feature" default="2" /> <!-- Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK -->
<arg name="estimation" default="1" /> <!-- Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP) -->
<arg name="nn" default="3" /> <!-- Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE (SIFT, SURF), 2=FLANN_LSH, 3=BRUTEFORCE (ORB/FREAK/BRIEF/BRISK) -->
<arg name="max_depth" default="10" /> <!-- Maximum features depth (m) -->
<arg name="min_inliers" default="8" /> <!-- Minimum visual correspondences to accept a transformation (m) -->
<arg name="inlier_distance" default="0.1" /> <!-- RANSAC maximum inliers distance (m) -->
<arg name="local_map" default="1200" /> <!-- Local map size: number of unique features to keep track -->
<arg name="variance_inliers" default="true"/> <!-- Variance from inverse of inliers count -->
<!-- Other params specific to SLAMbot -->
<param name="ORB/Gpu" type="bool" value="true" /> <!-- Use GPU version of ORB -->
<!-- Nodes -->
<group ns="$(arg namespace)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=$(arg rgb_topic) raw out:=$(arg rgb_topic)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_registered_topic) raw out:=$(arg depth_registered_topic)" />
<!-- Odometry -->
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" launch-prefix="$(arg launch_prefix)">
<!-- <remap from="odom" to="/odometry/filtered" /> -->
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<param name="publish_tf" type="bool" value="false"/> <!-- Dont publish a tf, robot_localization will do that -->
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="Odom/EstimationType" type="string" value="$(arg estimation)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="true"/>
<param name="Odom/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>
</node>
<!-- Visual SLAM (robot side) -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="$(arg subscribe_scan)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="publish_tf" type="bool" value="false"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
<param name="Rtabmap/TimeThr" type="string" value="$(arg time_threshold)"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="LccBow/EstimationType" type="string" value="$(arg estimation)"/>
<param name="LccBow/VarianceFromInliersCount" type="string" value="$(arg variance_inliers)"/>
<param name="Mem/SaveDepth16Format" type="string" value="$(arg convert_depth_to_mm)"/>
<!-- when 2D scan is set -->
<param if="$(arg subscribe_scan)" name="RGBD/OptimizeSlam2D" type="string" value="true"/>
<param if="$(arg subscribe_scan)" name="RGBD/LocalLoopDetectionSpace" type="string" value="true"/>
<param if="$(arg subscribe_scan)" name="LccIcp/Type" type="string" value="2"/>
<param if="$(arg subscribe_scan)" name="LccIcp2/CorrespondenceRatio" type="string" value="0.25"/>
</node>
<!--
Visualisation RTAB-Map
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
</node>
-->
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="$(arg rgb_topic)"/>
<remap from="depth/image_in" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info_in" to="$(arg camera_info_topic)"/>
<remap if="$(arg visual_odometry)" from="odom_in" to="rtabmap/odom"/>
<remap unless="$(arg visual_odometry)" from="odom_in" to="$(arg odom_topic)"/>
<remap from="rgb/image_out" to="data_odom_sync/image"/>
<remap from="depth/image_out" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
<remap from="odom_out" to="odom_sync"/>
</node>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="data_odom_sync/image"/>
<remap from="depth/image" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="decimation" type="double" value="2"/>
<param name="voxel_size" type="double" value="0.02"/>
</node>
<node pkg="tf" type="static_transform_publisher" name="base_link_to_camera_link_rgb" args="-0.36195 0 0.6604 -1.5707963267948966 0 -1.5707963267948966 base_link zed_rgb_optical_frame 100" >
</node>
<node pkg="tf" type="static_transform_publisher" name="camera_link_rgb_to_camera_link_depth" args="0 0 0 0 0 0 zed_rgb_optical_frame zed_depth_optical_frame 100" >
</node>
<node pkg="tf" type="static_transform_publisher" name="base_link_to_imu_link" args="0.4318 0.1143 0.2413 0 0 0 base_link imu_link 100" >
</node>
<node pkg="tf" type="static_transform_publisher" name="base_link_to_lidar_link" args="0.381 -0.0762 0.2413 0 0 0 base_link lidar_link 100" >
</node>
</launch>