First version rtabmap_launch working

2026-09-14 15:20:19 +08:00 · 2023-02-19 18:55:24 -08:00
parent 2f4aadacbb
commit 2dd931248e
418 changed files with 5304 additions and 3493 deletions
@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(rtabmap_examples)
+
+catkin_package()
+
+#############
+## Install ##
+#############
+
+install(DIRECTORY launch
+   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
@@ -0,0 +1,363 @@
+[Gui]
+AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\0\0\0\0\0\x1b\0\0\x2\xf7\0\0\x2\xfc\0\0\0\0\0\0\0\x1b\0\0\x2\xf7\0\0\x2\xfc\0\0\0\0\0\0\0\0\a\x80)
+DepthCalibrationDialog\bin_depth=2
+DepthCalibrationDialog\bin_height=6
+DepthCalibrationDialog\bin_width=8
+DepthCalibrationDialog\cone_radius=0.02
+DepthCalibrationDialog\cone_stddev_thresh=0.1
+DepthCalibrationDialog\decimation=1
+DepthCalibrationDialog\laser_scan=false
+DepthCalibrationDialog\max_depth=3.5
+DepthCalibrationDialog\max_model_depth=10
+DepthCalibrationDialog\min_depth=0
+DepthCalibrationDialog\smoothing=1
+DepthCalibrationDialog\voxel=0.01
+ExportBundlerDialog\exportPoints=false
+ExportBundlerDialog\laplacianThr=0
+ExportBundlerDialog\maxAngularSpeed=0
+ExportBundlerDialog\maxLinearSpeed=0
+ExportBundlerDialog\sba_iterations=20
+ExportBundlerDialog\sba_rematch_features=true
+ExportBundlerDialog\sba_type=0
+ExportBundlerDialog\sba_variance=1
+ExportCloudsDialog\assemble=true
+ExportCloudsDialog\assemble_voxel=0.005
+ExportCloudsDialog\bilateral=false
+ExportCloudsDialog\bilateral_sigma_r=0.1
+ExportCloudsDialog\bilateral_sigma_s=10
+ExportCloudsDialog\binary=true
+ExportCloudsDialog\cputsdf_flattenRadius=0.005
+ExportCloudsDialog\cputsdf_minWeight=0
+ExportCloudsDialog\cputsdf_randomSplit=1
+ExportCloudsDialog\cputsdf_resolution=0.01
+ExportCloudsDialog\cputsdf_size=12
+ExportCloudsDialog\cputsdf_truncNeg=0.03
+ExportCloudsDialog\cputsdf_truncPos=0.03
+ExportCloudsDialog\filtering=false
+ExportCloudsDialog\filtering_min_neighbors=2
+ExportCloudsDialog\filtering_radius=0.02
+ExportCloudsDialog\frame=0
+ExportCloudsDialog\from_depth=true
+ExportCloudsDialog\gain=false
+ExportCloudsDialog\gain_beta=10
+ExportCloudsDialog\gain_full=false
+ExportCloudsDialog\gain_overlap=0
+ExportCloudsDialog\gain_radius=0.02
+ExportCloudsDialog\gain_rgb=true
+ExportCloudsDialog\mesh=false
+ExportCloudsDialog\mesh_angle_tolerance=15
+ExportCloudsDialog\mesh_clean=true
+ExportCloudsDialog\mesh_color_radius=0.03
+ExportCloudsDialog\mesh_decimation_factor=0
+ExportCloudsDialog\mesh_dense_strategy=1
+ExportCloudsDialog\mesh_k=20
+ExportCloudsDialog\mesh_max_polygons=0
+ExportCloudsDialog\mesh_min_cluster_size=0
+ExportCloudsDialog\mesh_mu=2.5
+ExportCloudsDialog\mesh_quad=false
+ExportCloudsDialog\mesh_radius=0.04
+ExportCloudsDialog\mesh_texture=false
+ExportCloudsDialog\mesh_textureBlending=true
+ExportCloudsDialog\mesh_textureBlendingDecimation=0
+ExportCloudsDialog\mesh_textureBrightnessConstrastRatioHigh=0
+ExportCloudsDialog\mesh_textureBrightnessConstrastRatioLow=0
+ExportCloudsDialog\mesh_textureCameraFiltering=false
+ExportCloudsDialog\mesh_textureCameraFilteringAngle=30
+ExportCloudsDialog\mesh_textureCameraFilteringLaplacian=0
+ExportCloudsDialog\mesh_textureCameraFilteringRadius=0
+ExportCloudsDialog\mesh_textureCameraFilteringVel=0
+ExportCloudsDialog\mesh_textureCameraFilteringVelRad=0
+ExportCloudsDialog\mesh_textureExposureFusion=false
+ExportCloudsDialog\mesh_textureFormat=0
+ExportCloudsDialog\mesh_textureMaxAngle=0
+ExportCloudsDialog\mesh_textureMaxCount=1
+ExportCloudsDialog\mesh_textureMaxDepthError=0
+ExportCloudsDialog\mesh_textureMaxDistance=3
+ExportCloudsDialog\mesh_textureMinCluster=50
+ExportCloudsDialog\mesh_textureMultiband=false
+ExportCloudsDialog\mesh_textureRoiRatios=0.0 0.0 0.0 0.0
+ExportCloudsDialog\mesh_textureSize=5
+ExportCloudsDialog\mesh_triangle_size=2
+ExportCloudsDialog\mls=false
+ExportCloudsDialog\mls_dilation_iterations=0
+ExportCloudsDialog\mls_dilation_voxel_size=0.01
+ExportCloudsDialog\mls_output_voxel_size=0
+ExportCloudsDialog\mls_point_density=0
+ExportCloudsDialog\mls_polygonial_order=2
+ExportCloudsDialog\mls_radius=0.04
+ExportCloudsDialog\mls_upsampling_method=0
+ExportCloudsDialog\mls_upsampling_radius=0.01
+ExportCloudsDialog\mls_upsampling_step=0
+ExportCloudsDialog\normals_k=10
+ExportCloudsDialog\normals_radius=0
+ExportCloudsDialog\openchisel_carving_dist_m=0.05
+ExportCloudsDialog\openchisel_chunk_size_x=16
+ExportCloudsDialog\openchisel_chunk_size_y=16
+ExportCloudsDialog\openchisel_chunk_size_z=16
+ExportCloudsDialog\openchisel_far_plane_dist=1.1
+ExportCloudsDialog\openchisel_integration_weight=1
+ExportCloudsDialog\openchisel_merge_vertices=true
+ExportCloudsDialog\openchisel_near_plane_dist=0.05
+ExportCloudsDialog\openchisel_truncation_constant=0.001504
+ExportCloudsDialog\openchisel_truncation_linear=0.00152
+ExportCloudsDialog\openchisel_truncation_quadratic=0.0019
+ExportCloudsDialog\openchisel_truncation_scale=10
+ExportCloudsDialog\openchisel_use_voxel_carving=false
+ExportCloudsDialog\pipeline=0
+ExportCloudsDialog\poisson_depth=0
+ExportCloudsDialog\poisson_iso=8
+ExportCloudsDialog\poisson_manifold=true
+ExportCloudsDialog\poisson_minDepth=5
+ExportCloudsDialog\poisson_outputPolygons=false
+ExportCloudsDialog\poisson_pointWeight=4
+ExportCloudsDialog\poisson_samples=1
+ExportCloudsDialog\poisson_scale=1.1
+ExportCloudsDialog\poisson_solver=8
+ExportCloudsDialog\regenerate=true
+ExportCloudsDialog\regenerate_decimation=1
+ExportCloudsDialog\regenerate_distortion_model=
+ExportCloudsDialog\regenerate_fill_error=2
+ExportCloudsDialog\regenerate_fill_size=0
+ExportCloudsDialog\regenerate_max_depth=4
+ExportCloudsDialog\regenerate_min_depth=0
+ExportCloudsDialog\regenerate_roi=0.0 0.0 0.0 0.0
+ExportCloudsDialog\regenerate_scan_decimation=1
+ExportCloudsDialog\regenerate_scan_max_range=0
+ExportCloudsDialog\regenerate_scan_min_range=0
+ExportCloudsDialog\regenerate_voxel=0.005
+ExportCloudsDialog\subtract=false
+ExportCloudsDialog\subtract_min_neighbors=5
+ExportCloudsDialog\subtract_point_angle=0
+ExportCloudsDialog\subtract_point_radius=0.02
+ExportScansDialog\assemble=true
+ExportScansDialog\assemble_voxel=0.01
+ExportScansDialog\binary=true
+ExportScansDialog\filtering=false
+ExportScansDialog\filtering_min_neighbors=2
+ExportScansDialog\filtering_radius=0.02
+ExportScansDialog\normals_k=20
+ExportScansDialog\regenerate=false
+ExportScansDialog\regenerate_decimation=1
+Figures\counts=
+Figures\curves=
+General\beep=false
+General\cloudCeilingHeight=0
+General\cloudFiltering=false
+General\cloudFilteringAngle=20
+General\cloudFilteringRadius=0.2
+General\cloudFloorHeight=0
+General\cloudNoiseMinNeighbors=5
+General\cloudNoiseRadius=0
+General\cloudVoxel=0
+General\cloudsKept=true
+General\colorScheme0=0
+General\colorScheme1=0
+General\colorSchemeScan0=0
+General\colorSchemeScan1=0
+General\decimation0=4
+General\decimation1=4
+General\downsamplingScan0=1
+General\downsamplingScan1=1
+General\figure_cache=true
+General\figure_time=true
+General\gravityLength0=1
+General\gravityLength1=1
+General\gravityShown0=false
+General\gravityShown1=false
+General\gridMapOpacity=0.75
+General\gridMapShown=false
+General\gtAlign=true
+General\imageHighestHypShown=false
+General\imageRejectedShown=true
+General\imagesKept=true
+General\landmarkSize=0
+General\localizationsGraphView=false
+General\loggerEventLevel=3
+General\loggerLevel=2
+General\loggerPauseLevel=3
+General\loggerPrintThreadId=false
+General\loggerPrintTime=true
+General\loggerType=1
+General\maxDepth0=4
+General\maxDepth1=0
+General\maxRange0=0
+General\maxRange1=0
+General\meshing=false
+General\meshing_angle=15
+General\meshing_quad=false
+General\meshing_texture=false
+General\meshing_triangle_size=2
+General\minDepth0=0
+General\minDepth1=0
+General\minRange0=0
+General\minRange1=0
+General\noFiltering=true
+General\nochangeGraphView=false
+General\normalKSearch=10
+General\normalRadiusSearch=0
+General\notifyNewGlobalPath=false
+General\octomap=false
+General\octomap_2dgrid=true
+General\octomap_3dmap=true
+General\octomap_depth=16
+General\octomap_point_size=5
+General\octomap_rendering_type=0
+General\odomDisabled=false
+General\odomOnlyInliersShown=false
+General\odomQualityThr=50
+General\odomRegistration=3
+General\opacity0=1
+General\opacity1=1
+General\opacityScan0=1
+General\opacityScan1=1
+General\posteriorGraphView=true
+General\ptSize0=1
+General\ptSize1=2
+General\ptSizeFeatures0=3
+General\ptSizeFeatures1=3
+General\ptSizeScan0=1
+General\ptSizeScan1=1
+General\roiRatios0=0.0 0.0 0.0 0.0
+General\roiRatios1=0.0 0.0 0.0 0.0
+General\scanCeilingHeight=0
+General\scanFloorHeight=0
+General\scanNormalKSearch=0
+General\scanNormalRadiusSearch=0
+General\showClouds0=true
+General\showClouds1=true
+General\showFeatures0=false
+General\showFeatures1=true
+General\showFrames=false
+General\showFrustums0=false
+General\showFrustums1=false
+General\showGraphs=true
+General\showIMUAcc=false
+General\showIMUGravity=false
+General\showLabels=false
+General\showLandmarks=true
+General\showScans0=true
+General\showScans1=true
+General\subtractFiltering=false
+General\subtractFilteringAngle=0
+General\subtractFilteringMinPts=5
+General\subtractFilteringRadius=0.02
+General\verticalLayoutUsed=true
+General\voxelSizeScan0=0
+General\voxelSizeScan1=0
+General\wordsGraphView=false
+MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\0\x43\0\0\0\x1b\0\0\x5\xa5\0\0\x3\xf\0\0\0\x43\0\0\0\x39\0\0\x5\xa5\0\0\x3\xf\0\0\0\0\0\0\0\0\a\x80)
+MainWindow\maximized=false
+MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x1N\0\0\x2\x9a\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\x1\0\0\0=\0\0\x2\x9a\0\0\x2)\0\xff\xff\xff\0\0\0\x1\0\0\x4\xf\0\0\x2\x9a\xfc\x2\0\0\0\x2\xfc\0\0\0=\0\0\x2\x9a\0\0\0\xdb\0\xff\xff\xff\xfc\x1\0\0\0\x3\xfc\0\0\x1T\0\0\x1\x6\0\0\0h\0\xff\xff\xff\xfc\x2\0\0\0\x2\xfb\0\0\0&\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\0\0\0\0(\0\0\x1%\0\0\0\x13\0\xff\xff\xff\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\0=\0\0\x2\x9a\0\0\0\x37\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\x2`\0\0\x1\xde\0\0\0\xc8\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\x4\x44\0\0\x1\x1f\0\0\0\x46\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\x1I\0\0\0\xf7\0\0\0\xf2\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9a\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\0\0\0\0\0\0\0\x5\0\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\0\0\x5\0\0\0\x1\x33\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\xcb\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0i\0\xff\xff\xff\0\0\0\0\0\0\x2\x9a\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x2\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0\0\0\0\x12\0t\0o\0o\0l\0\x42\0\x61\0r\0_\0\x32\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)"
+MainWindow\status_bar=false
+PostProcessingDialog\cluster_angle=30
+PostProcessingDialog\cluster_radius=0.3
+PostProcessingDialog\detect_more_lc=true
+PostProcessingDialog\inter_session=true
+PostProcessingDialog\intra_session=true
+PostProcessingDialog\iterations=1
+PostProcessingDialog\reextract_features=false
+PostProcessingDialog\refine_lc=false
+PostProcessingDialog\refine_neigbors=false
+PostProcessingDialog\sba=false
+PostProcessingDialog\sba_epsilon=0
+PostProcessingDialog\sba_iterations=20
+PostProcessingDialog\sba_rematch_features=true
+PostProcessingDialog\sba_type=1
+PostProcessingDialog\sba_variance=1
+PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x2\0\0\0\0\x1\f\0\0\0\x1b\0\0\x4\xdb\0\0\x3\x9a\0\0\x1\f\0\0\0\x1b\0\0\x4\xdb\0\0\x3\x9a\0\0\0\0\0\0\0\0\a\x80)
+graphicsView_graphView\current_goal_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0)
+graphicsView_graphView\global_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
+graphicsView_graphView\global_path_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0)
+graphicsView_graphView\global_path_visible=true
+graphicsView_graphView\gps_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\x80\x80\x80\x80\0\0)
+graphicsView_graphView\gps_graph_visible=true
+graphicsView_graphView\graph_visible=true
+graphicsView_graphView\grid_visible=true
+graphicsView_graphView\gt_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0)
+graphicsView_graphView\gt_graph_visible=true
+graphicsView_graphView\inter_session_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\0\0\0\0)
+graphicsView_graphView\intra_inter_session_colors_enabled=false
+graphicsView_graphView\intra_session_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
+graphicsView_graphView\link_width=0
+graphicsView_graphView\local_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
+graphicsView_graphView\local_path_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
+graphicsView_graphView\local_path_visible=true
+graphicsView_graphView\local_radius_visible=false
+graphicsView_graphView\loop_closure_outlier_thr=@Variant(\0\0\0\x87\0\0\0\0)
+graphicsView_graphView\max_link_length=@Variant(\0\0\0\x87<\xa3\xd7\n)
+graphicsView_graphView\neighbor_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0)
+graphicsView_graphView\neighbor_merged_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xaa\xaa\0\0\0\0)
+graphicsView_graphView\node_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0)
+graphicsView_graphView\node_radius=0.009999999776482582
+graphicsView_graphView\orientation_ENU=false
+graphicsView_graphView\origin_visible=true
+graphicsView_graphView\referential_visible=true
+graphicsView_graphView\rejected_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
+graphicsView_graphView\user_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
+graphicsView_graphView\virtual_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
+imageView_loopClosure\alpha=150
+imageView_loopClosure\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
+imageView_loopClosure\colormap=0
+imageView_loopClosure\depth_shown=false
+imageView_loopClosure\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
+imageView_loopClosure\features_shown=true
+imageView_loopClosure\features_size=0
+imageView_loopClosure\graphics_view=false
+imageView_loopClosure\graphics_view_scale=true
+imageView_loopClosure\graphics_view_scale_to_height=false
+imageView_loopClosure\image_shown=true
+imageView_loopClosure\lines_shown=true
+imageView_loopClosure\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
+imageView_loopClosure\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
+imageView_odometry\alpha=200
+imageView_odometry\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
+imageView_odometry\colormap=0
+imageView_odometry\depth_shown=false
+imageView_odometry\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
+imageView_odometry\features_shown=true
+imageView_odometry\features_size=0
+imageView_odometry\graphics_view=false
+imageView_odometry\graphics_view_scale=true
+imageView_odometry\graphics_view_scale_to_height=false
+imageView_odometry\image_shown=true
+imageView_odometry\lines_shown=true
+imageView_odometry\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
+imageView_odometry\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
+imageView_source\alpha=150
+imageView_source\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
+imageView_source\colormap=0
+imageView_source\depth_shown=false
+imageView_source\feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\0\0\0\0)
+imageView_source\features_shown=true
+imageView_source\features_size=0
+imageView_source\graphics_view=false
+imageView_source\graphics_view_scale=true
+imageView_source\graphics_view_scale_to_height=false
+imageView_source\image_shown=true
+imageView_source\lines_shown=true
+imageView_source\matching_feature_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\xff\xff\0\0)
+imageView_source\matching_line_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\xff\xff\xff\xff\0\0)
+widget_cloudViewer\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
+widget_cloudViewer\camera_axis_shown=true
+widget_cloudViewer\camera_focal="@Variant(\0\0\0T?>\"\xb0=!\xa5\0?>!\xe6)"
+widget_cloudViewer\camera_free=false
+widget_cloudViewer\camera_lockZ=true
+widget_cloudViewer\camera_ortho=false
+widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xc0G\xad|>\r\xa4\x10@h\xe3\x46)
+widget_cloudViewer\camera_target_follow=true
+widget_cloudViewer\camera_target_locked=false
+widget_cloudViewer\camera_up=@Variant(\0\0\0T\0\0\0\0\0\0\0\0?\x80\0\0)
+widget_cloudViewer\frustum_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0)
+widget_cloudViewer\frustum_scale=@Variant(\0\0\0\x87?\0\0\0)
+widget_cloudViewer\frustum_shown=true
+widget_cloudViewer\grid=false
+widget_cloudViewer\grid_cell_count=50
+widget_cloudViewer\grid_cell_size=1
+widget_cloudViewer\intensity_max=0
+widget_cloudViewer\intensity_red_colormap=false
+widget_cloudViewer\normals=false
+widget_cloudViewer\normals_scale=0.20000000298023224
+widget_cloudViewer\normals_step=1
+widget_cloudViewer\rendering_rate=5
+widget_cloudViewer\trajectory_shown=true
+widget_cloudViewer\trajectory_size=100
@@ -0,0 +1,291 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Info1
+        - /PointCloud23
+        - /PointCloud23/Status1
+      Splitter Ratio: 0.529595
+    Tree Height: 422
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: map
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 2.02684
+        Min Value: -1.12646
+        Value: true
+      Axis: Z
+      Channel Name: rgb
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 2.34177e-38
+      Min Color: 0; 0; 0
+      Min Intensity: 9.21942e-41
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /voxel_cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        kinect:
+          Value: true
+        kinect_est:
+          Value: true
+        map:
+          Value: true
+        odom:
+          Value: true
+        openni_camera:
+          Value: true
+        openni_depth_frame:
+          Value: true
+        openni_depth_optical_frame:
+          Value: true
+        openni_rgb_frame:
+          Value: true
+        openni_rgb_optical_frame:
+          Value: true
+        world:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        world:
+          kinect:
+            openni_camera:
+              openni_depth_frame:
+                openni_depth_optical_frame:
+                  {}
+              openni_rgb_frame:
+                openni_rgb_optical_frame:
+                  {}
+          map:
+            odom:
+              kinect_est:
+                {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /camera/rgb/image_rect_color
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rtabmap_ros/MapCloud
+      Cloud decimation: 4
+      Cloud max depth (m): 4
+      Cloud voxel size (m): 0.01
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Download graph: false
+      Download map: false
+      Enabled: true
+      Filter floor (m): 0
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: MapCloud
+      Node filtering angle (degrees): 15
+      Node filtering radius (m): 0.1
+      Position Transformer: XYZ
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /rtabmap/mapData
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rtabmap_ros/Info
+      Enabled: true
+      Name: Info
+      Topic: /rtabmap/info
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /rtabmap/odom_last_frame
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 1.17984
+        Min Value: 0.0147058
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 85; 255; 127
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /rtabmap/odom_local_map
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 3.32013
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -0.216951
+        Y: 0.819032
+        Z: 1.30659
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.554801
+      Target Frame: base_link
+      Value: Orbit (rviz)
+      Yaw: 4.9892
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 824
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001530000031afc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000235000000dd00fffffffb0000000a0049006d006100670065010000023b000000df0000001600fffffffb0000000a0049006d00610067006501000001fd0000011d0000000000000000000000010000010f00000312fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000312000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003a00000031a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1273
+  X: 316
+  Y: 95
@@ -0,0 +1,240 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /TF1/Frames1
+      Splitter Ratio: 0.411058
+    Tree Height: 422
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+    Name: Tool Properties
+    Splitter Ratio: 0.588679
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Image
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.03
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: odom
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 2.02684
+        Min Value: -1.12646
+        Value: true
+      Axis: Z
+      Channel Name: rgb
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 2.34177e-38
+      Min Color: 0; 0; 0
+      Min Intensity: 9.21942e-41
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /voxel_cloud
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+        base_link:
+          Value: true
+        camera_depth_frame:
+          Value: true
+        camera_depth_optical_frame:
+          Value: true
+        camera_link:
+          Value: true
+        camera_rgb_frame:
+          Value: true
+        camera_rgb_optical_frame:
+          Value: true
+        odom:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        odom:
+          base_link:
+            camera_link:
+              camera_depth_frame:
+                camera_depth_optical_frame:
+                  {}
+              camera_rgb_frame:
+                camera_rgb_optical_frame:
+                  {}
+      Update Interval: 0
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /camera/rgb/image_rect_color
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 0; 255; 0
+      Color Transformer: FlatColor
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 2
+      Size (m): 0.01
+      Style: Points
+      Topic: /odom_local_map
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.01
+      Style: Points
+      Topic: /odom_last_frame
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Topic: /initialpose
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 7.06539
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.06
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 1.45837
+        Y: 0.109967
+        Z: -0.602093
+      Name: Current View
+      Near Clip Distance: 0.01
+      Pitch: 0.839796
+      Target Frame: base_link
+      Value: Orbit (rviz)
+      Yaw: 3.58419
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 824
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001530000031afc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000235000000dd00fffffffb0000000a0049006d006100670065010000023b000000df0000001600fffffffb0000000a0049006d00610067006501000001fd0000011d0000000000000000000000010000010f00000312fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000312000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003a00000031a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1273
+  X: 229
+  Y: 149
@@ -0,0 +1,41 @@
+<?xml version="1.0"?>
+<launch>
+
+    <node pkg="camera1394stereo" type="camera1394stereo_node" name="camera1394stereo_node" output="screen" >
+	    <param name="video_mode" value="format7_mode3" />
+	    <param name="format7_color_coding" value="raw16" />
+	    <param name="bayer_pattern" value="bggr" />
+	    <param name="bayer_method" value="" />
+	    <param name="stereo_method" value="Interlaced" />
+	    <param name="camera_info_url_left" value="" />
+	    <param name="camera_info_url_right" value="" />
+  	</node>
+
+	<arg name="gen_depth" default="false"/>
+  	<arg name="pi/2" value="1.5707963267948966" />
+    	<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
+  	<node pkg="tf" type="static_transform_publisher" name="camera_base_link"
+        args="$(arg optical_rotate) base_link stereo_camera 100" />
+
+  	<!-- Run the ROS package stereo_image_proc (throttle to 10 Hz to avoid rectifying all images) -->
+	<group ns="/stereo_camera" >
+	    	<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="10"/>
+		</node>
+
+		<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
+			<remap from="left/image_raw"    to="left/image_raw_throttle"/>
+			<remap from="left/camera_info"  to="left/camera_info_throttle"/>
+			<remap from="right/image_raw"   to="right/image_raw_throttle"/>
+			<remap from="right/camera_info" to="right/camera_info_throttle"/>
+		</node>
+
+  		<node if="$(arg gen_depth)" pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap_ros/disparity_to_depth"/>
+	</group>
+</launch>
@@ -0,0 +1,120 @@
+<?xml version="1.0"?>
+<launch>
+
+<!--
+Examples:
+   F2M (default VO):
+      $ roslaunch rtabmap_ros euroc_datasets.launch
+      $ rosbag play -.-clock V1_01_easy.bag
+
+      For MH sequences, we should set MH_seq to true because the ground truth source is different.
+      $ roslaunch rtabmap_ros euroc_datasets.launch MH_seq:=true
+      $ rosbag play -.-clock MH_01_easy.bag
+
+   MSCKF (VIO):
+      $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 8"
+      $ rosbag play -.-clock V1_01_easy.bag
+
+      We need to ignore the first 24 seconds for correct VIO initialization (drone should not move).
+      $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 8" MH_seq:=true
+      $ rosbag play -.-clock -s 24 MH_01_easy.bag
+
+   OKVIS (VIO):
+      $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 6 OdomOKVIS/ConfigPath ~/okvis/config/config_fpga_p2_euroc.yaml" MH_seq:=true raw_images_for_odom:=true
+      $ rosbag play -.-clock MH_01_easy.bag
+
+   VINS (VIO):
+      $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml" MH_seq:=true raw_images_for_odom:=true
+      $ rosbag play -.-clock MH_01_easy.bag
+
+   VINS (VO):
+      $ roslaunch rtabmap_ros euroc_datasets.launch args:="Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml" MH_seq:=true raw_images_for_odom:=true
+      $ rosbag play -.-clock MH_01_easy.bag
+-->
+
+<param name="use_sim_time" value="true"/>
+
+<arg name="feature_type" default="6"/>
+<arg name="gravity_opt" default="false"/> <!-- Rtabmap will use IMU data to add gravity constraints to graph -->
+
+<arg name="args" default=""/>
+<arg     if="$(arg gravity_opt)" name="common_args" default="-d --RGBD/CreateOccupancyGrid false --Odom/FeatureType $(arg feature_type) --Kp/DetectorStrategy $(arg feature_type) --Optimizer/GravitySigma 0.3 $(arg args)"/>
+<arg unless="$(arg gravity_opt)" name="common_args" default="-d --RGBD/CreateOccupancyGrid false --Odom/FeatureType $(arg feature_type) --Kp/DetectorStrategy $(arg feature_type) $(arg args) "/>
+
+<arg name="cfg" default=""/>
+<arg name="MH_seq" default="false"/> <!-- For MH sequences, the ground truth is coming from a different topic -->
+<arg name="raw_images_for_odom" default="false"/>
+<arg name="record_ground_truth" default="false"/>
+<arg name="rtabmapviz" default="true"/>
+<arg name="rviz" default="false"/>
+
+<!-- Image rectification and publishing synchronized camera_info-->
+<group ns="stereo_camera">
+
+  <node pkg="rtabmap_ros" type="yaml_to_camera_info.py" name="yaml_to_camera_info_left">
+    <param name="yaml_path" value="$(find rtabmap_ros)/launch/calibration/euroc_left.yaml"/>
+    <remap from="image" to="/cam0/image_raw"/>
+    <remap from="camera_info" to="left/camera_info"/>
+  </node>
+  <node pkg="rtabmap_ros" type="yaml_to_camera_info.py" name="yaml_to_camera_info_right">
+    <param name="yaml_path" value="$(find rtabmap_ros)/launch/calibration/euroc_right.yaml"/>
+    <param name="frame_id" value="cam1"/>
+    <remap from="image" to="/cam1/image_raw"/>
+    <remap from="camera_info" to="right/camera_info"/>
+  </node>
+
+  <node unless="$(arg raw_images_for_odom)" pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
+    <remap from="left/image_raw"    to="/cam0/image_raw"/>
+    <remap from="right/image_raw"   to="/cam1/image_raw"/>
+  </node>
+
+</group>
+
+<!-- TF frames -->
+<node pkg="tf" type="static_transform_publisher" name="imu_base_link" args="0 0 0 3.1415926 -1.570796 0 base_link imu4 5"/>
+<node pkg="tf" type="static_transform_publisher" name="cam0_imu_link" args="-0.021640 -0.064677 0.009811 1.555925 0.025777 0.003757 imu4 cam0 50"/>
+<node pkg="tf" type="static_transform_publisher" name="cam1_imu_link" args="-0.019844 0.045369 0.007862 1.558237 0.025393 0.017907 imu4 cam1 50"/>
+
+<!-- For MH sequences, /leica/position doesn't give the orientation, so minimal ground truth error could be as high as 12 cm -->
+<node     if="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="leica_base_link" args="0.120209 -0.0184772 -0.0748903 0 0 0 leica base_link_gt 100"/>
+<node unless="$(arg MH_seq)" pkg="tf" type="static_transform_publisher" name="vicon_base_link" args="0.12395 -0.02781 -0.06901 0 0 0 vicon/firefly_sbx/firefly_sbx base_link_gt 100"/>
+
+<node     if="$(arg MH_seq)" pkg="rtabmap_ros" type="point_to_tf.py" name="point_to_tf">
+  <remap from="point" to="/leica/position"/>
+  <param name="frame_id" value="leica"/>
+  <param name="fixed_frame_id" value="world"/>
+</node>
+<node unless="$(arg MH_seq)" pkg="rtabmap_ros" type="transform_to_tf.py" name="transform_to_tf">
+  <remap from="transform" to="/vicon/firefly_sbx/firefly_sbx"/>
+  <param name="frame_id" value="world"/>
+  <param name="child_frame_id" value="vicon/firefly_sbx/firefly_sbx"/>
+</node>
+<node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />
+
+<node pkg="imu_complementary_filter" type="complementary_filter_node" name="imu_filter" output="screen">
+   <remap from="imu/data_raw" to="/imu0"/>
+   <param name="use_mag" value="false"/>
+   <param name="world_frame" value="enu"/>
+   <param name="publish_tf" value="false"/>
+</node>
+
+<!-- RTAB-Map -->
+<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+   <arg     if="$(arg raw_images_for_odom)" name="rtabmap_args" value="$(arg common_args) --Rtabmap/ImagesAlreadyRectified false"/>
+   <arg unless="$(arg raw_images_for_odom)" name="rtabmap_args" value="$(arg common_args)"/>
+   <arg     if="$(arg raw_images_for_odom)" name="odom_args" value="--Rtabmap/ImagesAlreadyRectified false"/>
+   <arg     if="$(arg raw_images_for_odom)" name="left_image_topic" value="/cam0/image_raw"/>
+   <arg     if="$(arg raw_images_for_odom)" name="right_image_topic" value="/cam1/image_raw"/>
+   <arg name="stereo" value="true"/>
+   <arg name="frame_id" value="base_link"/>
+   <arg name="wait_for_transform" value="0.1"/>
+   <arg if="$(arg record_ground_truth)" name="ground_truth_frame_id" value="world"/>
+   <arg if="$(arg record_ground_truth)" name="ground_truth_base_frame_id" value="base_link_gt"/>
+   <arg name="cfg" value="$(arg cfg)"/>
+   <arg name="imu_topic" value="/imu/data"/>
+   <arg name="rtabmapviz" value="$(arg rtabmapviz)"/>
+   <arg name="rviz" value="$(arg rviz)"/>
+   <arg name="wait_imu_to_init" value="true"/>
+</include>
+
+</launch>
@@ -0,0 +1,93 @@
+<?xml version="1.0"?>
+
+<launch>
+
+   <!-- Example to run rgbd datasets:
+   $ wget http://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag
+   $ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag
+   $ wget https://gist.githubusercontent.com/matlabbe/897b775c38836ed8069a1397485ab024/raw/6287ce3def8231945326efead0c8a7730bf6a3d5/tum_rename_world_kinect_frame.py
+   $ python tum_rename_world_kinect_frame.py rgbd_dataset_freiburg3_long_office_household.bag
+
+   $ roslaunch rtabmap_ros rgbdslam_datasets.launch
+   $ rosbag play -.-clock rgbd_dataset_freiburg3_long_office_household.bag
+   -->
+
+   <param name="use_sim_time" type="bool" value="True"/>
+
+   <!-- Choose visualization -->
+   <arg name="rviz" default="true" />
+   <arg name="rtabmapviz" default="false" />
+
+  <!-- TF FRAMES -->
+  <node pkg="tf" type="static_transform_publisher" name="world_to_map"
+    args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />
+
+
+  <group ns="rtabmap">
+
+    <!-- Odometry -->
+    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
+      <remap from="rgb/image"       to="/camera/rgb/image_color"/>
+      <remap from="depth/image"     to="/camera/depth/image"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
+
+      <param name="Odom/Strategy" type="string" value="0"/>      <!-- 0=Frame-to-Map, 1=Frame-to-KeyFrame -->
+      <param name="Odom/ResetCountdown" type="string" value="15"/>
+      <param name="Odom/GuessSmoothingDelay" type="string" value="0"/>
+
+      <param name="frame_id" type="string" value="kinect"/>
+      <param name="queue_size" type="int" value="10"/>
+      <param name="wait_for_transform" type="bool" value="true"/>
+      <param name="ground_truth_frame_id" type="string" value="world"/>
+      <param name="ground_truth_base_frame_id" type="string" value="kinect_gt"/>
+    </node>
+
+    <!-- Visual SLAM -->
+    <!-- args: "delete_db_on_start" and "udebug" -->
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
+      <param name="subscribe_depth" type="bool" value="true"/>
+
+      <param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="true"/>
+      <param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
+      <param name="Rtabmap/CreateIntermediateNodes" type="string" value="true"/>
+      <param name="RGBD/LinearUpdate" type="string" value="0"/>
+      <param name="RGBD/AngularUpdate" type="string" value="0"/>
+
+      <param name="frame_id" type="string" value="kinect"/>
+      <param name="ground_truth_frame_id" type="string" value="world"/>
+      <param name="ground_truth_base_frame_id" type="string" value="kinect_gt"/>
+
+      <remap from="rgb/image" to="/camera/rgb/image_color"/>
+      <remap from="depth/image" to="/camera/depth/image"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
+
+      <param name="queue_size" type="int" value="10"/>
+    </node>
+
+    <!-- Visualisation  -->
+    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+      <param name="subscribe_depth" type="bool" value="true"/>
+      <param name="subscribe_odom_info" type="bool" value="true"/>
+      <param name="queue_size" type="int" value="30"/>
+
+      <param name="frame_id" type="string" value="kinect"/>
+
+      <remap from="rgb/image" to="/camera/rgb/image_color"/>
+      <remap from="depth/image" to="/camera/depth/image"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
+    </node>
+
+  </group>
+
+  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbdslam_datasets.rviz"/>
+  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
+    <remap from="rgb/image"       to="/camera/rgb/image_color"/>
+    <remap from="depth/image"     to="/camera/depth/image"/>
+    <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
+    <remap from="cloud"           to="voxel_cloud" />
+
+    <param name="queue_size" type="int" value="10"/>
+    <param name="decimation" type="double" value="4"/>
+  </node>
+
+</launch>
@@ -0,0 +1,146 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- This launch assumes that you have already
+       started you preferred RGB-D sensor and your IMU.
+       TF between frame_id and the sensors should already be set too. -->
+
+  <arg name="frame_id"                default="base_link" />
+  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
+  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
+  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
+  <arg name="imu_topic"               default="/imu/data" />
+  <arg name="imu_ignore_acc"          default="true" />
+  <arg name="imu_remove_gravitational_acceleration" default="true" />
+
+  <!-- Localization-only mode -->
+  <arg name="localization"      default="false"/>
+  <arg     if="$(arg localization)" name="rtabmap_args"  default=""/>
+  <arg unless="$(arg localization)" name="rtabmap_args"  default="--delete_db_on_start"/>
+
+  <group ns="rtabmap">
+    <!-- Visual Odometry -->
+    <node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen" args="$(arg rtabmap_args)">
+      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
+      <remap from="depth/image"     to="$(arg depth_topic)"/>
+      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
+      <remap from="odom"            to="/vo"/>
+
+      <param name="frame_id"               type="string" value="$(arg frame_id)"/>
+      <param name="publish_tf"             type="bool"   value="false"/>
+      <param name="publish_null_when_lost" type="bool"   value="true"/>
+      <param name="guess_from_tf"          type="bool"   value="true"/>
+
+      <param name="Odom/FillInfoData"      type="string" value="true"/>
+      <param name="Odom/ResetCountdown"    type="string" value="1"/>
+      <param name="Vis/FeatureType"        type="string" value="6"/>
+      <param name="OdomF2M/MaxSize"        type="string" value="1000"/>
+    </node>
+
+    <!-- SLAM -->
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
+      <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+
+      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
+      <remap from="depth/image"     to="$(arg depth_topic)"/>
+      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
+      <remap from="odom"            to="/odometry/filtered"/>
+
+      <param name="Kp/DetectorStrategy"    type="string" value="6"/> <!-- use same features as odom -->
+
+      <!-- localization mode -->
+      <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
+      <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
+      <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
+
+    </node>
+  </group>
+
+  <!-- Odometry fusion (EKF), refer to demo launch file in robot_localization for more info -->
+  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">
+
+      <param name="frequency" value="50"/>
+      <param name="sensor_timeout" value="0.1"/>
+      <param name="two_d_mode" value="false"/>
+
+      <param name="odom_frame" value="odom"/>
+      <param name="base_link_frame" value="$(arg frame_id)"/>
+      <param name="world_frame" value="odom"/>
+
+      <param name="transform_time_offset" value="0.0"/>
+
+      <param name="odom0" value="/vo"/>
+      <param name="imu0" value="$(arg imu_topic)"/>
+
+      <!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
+      <rosparam param="odom0_config">[true, true, true,
+                                      false, false, false,
+                                      true, true, true,
+                                      false, false, false,
+                                      false, false, false]</rosparam>
+
+      <rosparam     if="$(arg imu_ignore_acc)" param="imu0_config">[
+                                     false, false, false,
+                                     true,  true,  true,
+                                     false, false, false,
+                                     true,  true,  true,
+                                     false,  false,  false] </rosparam>
+      <rosparam unless="$(arg imu_ignore_acc)" param="imu0_config">[
+                                     false, false, false,
+                                     true,  true,  true,
+                                     false, false, false,
+                                     true,  true,  true,
+                                     true,  true,  true] </rosparam>
+
+      <param name="odom0_differential" value="false"/>
+      <param name="imu0_differential" value="false"/>
+
+      <param name="odom0_relative" value="true"/>
+      <param name="imu0_relative" value="true"/>
+
+      <param name="imu0_remove_gravitational_acceleration" value="$(arg imu_remove_gravitational_acceleration)"/>
+
+      <param name="print_diagnostics" value="true"/>
+
+      <!-- ======== ADVANCED PARAMETERS ======== -->
+      <param name="odom0_queue_size" value="5"/>
+      <param name="imu0_queue_size" value="50"/>
+
+      <!-- The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
+           vroll, vpitch, vyaw, ax, ay, az. -->
+      <rosparam param="process_noise_covariance">[0.005, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                                                  0,    0.005, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                                                  0,    0,    0.006, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                                                  0,    0,    0,    0.003, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                                                  0,    0,    0,    0,    0.003, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
+                                                  0,    0,    0,    0,    0,    0.006, 0,     0,     0,    0,    0,    0,    0,    0,    0,
+                                                  0,    0,    0,    0,    0,    0,    0.0025, 0,     0,    0,    0,    0,    0,    0,    0,
+                                                  0,    0,    0,    0,    0,    0,    0,     0.0025, 0,    0,    0,    0,    0,    0,    0,
+                                                  0,    0,    0,    0,    0,    0,    0,     0,     0.004, 0,    0,    0,    0,    0,    0,
+                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0.001, 0,    0,    0,    0,    0,
+                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.001, 0,    0,    0,    0,
+                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.002, 0,    0,    0,
+                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.001, 0,    0,
+                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.001, 0,
+                                                  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.0015]</rosparam>
+
+      <!-- The values are ordered as x, y,
+           z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
+           <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                                                          0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                                                          0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                                                          0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                                                          0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
+                                                          0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
+                                                          0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
+                                                          0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
+                                                          0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
+                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
+                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
+                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
+                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
+                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
+                                                          0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>
+
+    </node>
+</launch>
@@ -0,0 +1,39 @@
+<?xml version="1.0"?>
+<launch>
+
+  <arg name="localization"      default="false"/>
+  <arg name="uid"/>
+
+  <!-- Kinect: -->
+  <include file="$(find freenect_launch)/launch/freenect.launch">
+    <arg name="depth_registration" value="true" />
+    <arg name="publish_tf" value="false" />
+  </include>
+
+  <!-- IMU Sensor: -->
+  <node pkg="imu_brick" type="imu_brick_node" name="imu_brick">
+      <param name="frame_id"                       value="imu_link"/>
+      <param name="period_ms"                      value="10"/>
+      <param name="uid"              type="string" value="$(arg uid)"/>
+      <param name="cov_orientation"  type="double" value="0.0005"/>
+      <param name="cov_velocity"     type="double" value="0.00025"/>
+      <param name="cov_acceleration" type="double" value="0.1"/>
+      <param name="remove_gravitational_acceleration" type="bool" value="true"/>
+   </node>
+
+  <!-- IMU frame: just over the RGB camera -->
+  <node pkg="tf" type="static_transform_publisher" name="rgb_to_imu_tf"
+      args="-0.032 0.0 0.032 0.0 0.0 0.0 /camera_rgb_frame /imu_link 100" />
+
+  <arg name="pi/2" value="1.5707963267948966" />
+   	<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
+  <node pkg="tf" type="static_transform_publisher" name="optical_rotation"
+        args="$(arg optical_rotate) /camera_rgb_frame /camera_rgb_optical_frame 100" />
+
+  <include file="$(find rtabmap_ros)/launch/tests/sensor_fusion.launch">
+      <arg name="frame_id" value="camera_rgb_frame"/>
+      <arg name="localization" value="$(arg localization)"/>
+      <arg name="imu_remove_gravitational_acceleration" value="false"/>
+  </include>
+
+</launch>
@@ -0,0 +1,14 @@
+# AprilTags 2 code parameters
+# Find descriptions in apriltags2/include/apriltag.h:struct apriltag_detector
+#                      apriltags2/include/apriltag.h:struct apriltag_family
+tag_family:        'tag36h11' # options: tag36h11, tag36h10, tag25h9, tag25h7, tag16h5
+tag_border:        1          # default: 1
+tag_threads:       2          # default: 2
+tag_decimate:      1.0        # default: 1.0
+tag_blur:          0.0        # default: 0.0
+tag_refine_edges:  1          # default: 1
+tag_refine_decode: 0          # default: 0
+tag_refine_pose:   0          # default: 0
+tag_debug:         0          # default: 0
+# Other parameters
+publish_tf:        true       # default: false
@@ -0,0 +1,59 @@
+# # Definitions of tags to detect
+#
+# ## General remarks
+#
+# - All length in meters
+# - Ellipsis (...) signifies that the previous element can be repeated multiple times.
+#
+# ## Standalone tag definitions
+# ### Remarks
+#
+# - name is optional
+#
+# ### Syntax
+#
+# standalone_tags:
+#   [
+#     {id: ID, size: SIZE, name: NAME},
+#     ...
+#   ]
+standalone_tags:
+  [
+  ]
+# ## Tag bundle definitions
+# ### Remarks
+#
+# - name is optional
+# - x, y, z have default values of 0 thus they are optional
+# - qw has default value of 1 and qx, qy, qz have default values of 0 thus they are optional
+#
+# ### Syntax
+#
+# tag_bundles:
+#   [
+#     {
+#       name: 'CUSTOM_BUNDLE_NAME',
+#       layout:
+#         [
+#           {id: ID, size: SIZE, x: X_POS, y: Y_POS, z: Z_POS, qw: QUAT_W_VAL, qx: QUAT_X_VAL, qy: QUAT_Y_VAL, qz: QUAT_Z_VAL},
+#           ...
+#         ]
+#     },
+#     ...
+#   ]
+tag_bundles:
+   [
+     {
+       name: 'tag_bundle1',
+       layout:
+         [
+           # for a pixel = ~8mm
+           {id: 1,   size: 0.0620, x: 0.0000, y:  0.0000, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 2,   size: 0.0620, x: 0.0770, y:  0.0000, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 25,  size: 0.0620, x: 0.0000, y: -0.0770, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 26,  size: 0.0620, x: 0.0770, y: -0.0770, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 49,  size: 0.0620, x: 0.0000, y: -0.1540, z: 0, qw: 1, qx: 0, qy: 0, qz: 0},
+           {id: 50,  size: 0.0620, x: 0.0770, y: -0.1540, z: 0, qw: 1, qx: 0, qy: 0, qz: 0}
+         ]
+     },
+   ]
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Print on the file tag_bundle1.png (with GIMP: set size in Image Settings to 160x240mm) -->
+  <!-- Definition of the tag bundle is tags.yaml, make sure the camera is calibrated or adjust the size of the tag if needed -->
+  <!-- The TF published by apriltag_ros should match the point cloud created by the camera -->
+  <!-- Change Optimizer/Strategy below between 1 (g2o) and 2 (GTSAM), and landmark_angular_variance between 0.005 (optimize rotation) and 9999 (optimize only tag's XYZ) -->
+
+  <!-- $ roslaunch realsense2_camera rs_camera.launch align_depth:=true -->
+  <!-- $ roslaunch rtabmap_ros test_apriltag_ros.launch rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info -->
+  <!-- $ roslaunch rtabmap_ros rtabmap.launch depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info rviz:=true rtabmapviz:=false args:="-d -Optimizer/Strategy 1" landmark_angular_variance:=9999 -->
+
+  <arg name="camera_frame_id"         default="camera_color_optical_frame"/>
+  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
+  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
+
+  <!-- Set parameters -->
+  <rosparam command="load" file="$(find rtabmap_ros)/launch/tests/tag_settings.yaml" ns="apriltag_ros_continuous_node" />
+  <rosparam command="load" file="$(find rtabmap_ros)/launch/tests/tags.yaml" ns="apriltag_ros_continuous_node" />
+
+  <node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="apriltag_ros_continuous_node" clear_params="true" output="screen">
+    <remap from="image_rect" to="$(arg rgb_topic)" />
+    <remap from="camera_info" to="$(arg camera_info_topic)" />
+
+    <param name="camera_frame" type="str" value="$(arg camera_frame_id)" />
+    <param name="publish_tag_detections_image" type="bool" value="true" />      <!-- default: false -->
+  </node>
+</launch>
@@ -0,0 +1,75 @@
+<?xml version="1.0"?>
+<launch>
+
+<!-- Example usage of RTAB-Map with VINS-Fusion support for realsense D435i.
+     Make sure to disable the IR emitter or put a tape on the IR emitter to
+     avoid VINS tracking the fixed IR points (that would cause large drifts) -->
+
+<arg name="rtabmapviz" default="true"/>
+<arg name="rviz"       default="false"/>
+<arg name="depth_mode" default="true"/>
+<arg name="odom_strategy" default="9"/> <!-- default VINS -->
+<arg name="unite_imu_method" default="copy"/> <!-- "copy" or "linear_interpolation" -->
+
+<include file="$(find realsense2_camera)/launch/rs_camera.launch">
+   <arg name="align_depth"   value="$(arg depth_mode)"/>
+   <arg name="unite_imu_method" value="$(arg unite_imu_method)"/>
+   <arg name="enable_gyro"   value="true"/>
+   <arg name="enable_accel"  value="true"/>
+   <arg name="enable_infra1" value="true"/>
+   <arg name="enable_infra2" value="true"/>
+   <arg name="gyro_fps"      value="200"/>
+   <arg name="accel_fps"     value="250"/>
+   <arg name="enable_sync"   value="true"/>
+</include>
+
+<node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_node">
+   <param name="use_mag"       value="false"/>
+   <param name="publish_tf"    value="false"/>
+   <param name="world_frame"   value="enu"/>
+   <remap from="/imu/data_raw" to="/camera/imu"/>
+   <remap from="/imu/data"     to="/rtabmap/imu"/>
+</node>
+
+<!-- RTAB-Map: depth mode -->
+<!-- We have to launch stereo_odometry externally from rtabmap.launch so that rtabmap can use RGB-D input -->
+<group ns="rtabmap">
+<node if="$(arg depth_mode)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" args="--Optimizer/GravitySigma 0.3 --Odom/Strategy $(arg odom_strategy) --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml" output="screen">
+   <remap from="left/image_rect"   to="/camera/infra1/image_rect_raw"/>
+   <remap from="right/image_rect"  to="/camera/infra2/image_rect_raw"/>
+   <remap from="left/camera_info"  to="/camera/infra1/camera_info"/>
+   <remap from="right/camera_info" to="/camera/infra2/camera_info"/>
+   <remap from="imu"               to="/rtabmap/imu"/>
+   <param name="frame_id"          value="camera_link"/>
+   <param name="wait_imu_to_init"  value="true"/>
+</node>
+</group>
+<include if="$(arg depth_mode)" file="$(find rtabmap_ros)/launch/rtabmap.launch">
+   <arg name="rtabmap_args"      value="--delete_db_on_start --Optimizer/GravitySigma 0.3"/>
+   <arg name="rgb_topic"         value="/camera/color/image_raw"/>
+   <arg name="depth_topic"       value="/camera/aligned_depth_to_color/image_raw"/>
+   <arg name="camera_info_topic" value="/camera/color/camera_info"/>
+   <arg name="visual_odometry"   value="false"/>
+   <arg name="approx_sync"       value="false"/>
+   <arg name="frame_id"          value="camera_link"/>
+   <arg name="imu_topic"         value="/rtabmap/imu"/>
+   <arg name="rtabmapviz"        value="$(arg rtabmapviz)"/>
+   <arg name="rviz"              value="$(arg rviz)"/>
+</include>
+
+<!-- RTAB-Map: Stereo mode -->
+<include unless="$(arg depth_mode)" file="$(find rtabmap_ros)/launch/rtabmap.launch">
+   <arg name="rtabmap_args"            value="--delete_db_on_start --Optimizer/GravitySigma 0.3 --Odom/Strategy $(arg odom_strategy) --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml"/>
+   <arg name="left_image_topic"        value="/camera/infra1/image_rect_raw"/>
+   <arg name="right_image_topic"       value="/camera/infra2/image_rect_raw"/>
+   <arg name="left_camera_info_topic"  value="/camera/infra1/camera_info"/>
+   <arg name="right_camera_info_topic" value="/camera/infra2/camera_info"/>
+   <arg name="stereo"                  value="true"/>
+   <arg name="frame_id"                value="camera_link"/>
+   <arg name="imu_topic"               value="/rtabmap/imu"/>
+   <arg name="wait_imu_to_init"        value="true"/>
+   <arg name="rtabmapviz"              value="$(arg rtabmapviz)"/>
+   <arg name="rviz"                    value="$(arg rviz)"/>
+</include>
+
+</launch>
@@ -0,0 +1,99 @@
+<?xml version="1.0"?>
+
+<launch>
+
+  <!-- We test here ICP odometry using a guess from visual odometry -->
+
+  <arg name="rgbd"    default="false"/>
+  <arg name="pm"      default="false"/>
+  <arg name="nodelet" default="false"/>
+
+  <include file="$(find freenect_launch)/launch/freenect.launch" >
+    <arg name="depth_registration" value="true"/>
+    <arg name="data_skip" value="3"/>
+  </include>
+
+  <group ns="camera">
+    <node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
+      <remap from="depth/image"       to="depth_registered/image_raw"/>
+      <remap from="depth/camera_info" to="depth_registered/camera_info"/>
+      <remap from="cloud"             to="/voxel_cloud" />
+
+      <param name="voxel_size" type="double" value="0.05"/>
+      <param name="decimation" type="int" value="8"/>
+
+      <param name="Odom/AlignWithGround"  type="string" value="true"/>
+    </node>
+
+    <group if="$(arg nodelet)">
+      <node if="$(arg rgbd)" pkg="nodelet" type="nodelet" name="rgbdicp_odometry" args="load rtabmap_ros/rgbdicp_odometry camera_nodelet_manager">
+        <remap from="scan_cloud"        to="/voxel_cloud"/>
+        <remap from="depth/image"       to="depth_registered/image_raw"/>
+        <remap from="rgb/camera_info"   to="rgb/camera_info"/>
+        <remap from="rgb/image"         to="rgb/image_rect_mono"/>
+
+        <param name="frame_id"        type="string" value="camera_link"/>
+        <param name="scan_normal_k"   type="int" value="10"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+
+        <param name="Icp/PointToPlane"  type="string" value="true"/>
+        <param name="Icp/VoxelSize"     type="string" value="0"/>
+        <param name="Icp/PM"            type="string" value="$(arg pm)"/>
+        <param name="Icp/PMOutlierRatio" type="string" value="0.65"/>
+      </node>
+      <node unless="$(arg rgbd)" pkg="nodelet" type="nodelet" name="icp_odometry" args="load rtabmap_ros/icp_odometry camera_nodelet_manager" output="screen">
+        <remap from="scan_cloud"      to="/voxel_cloud"/>
+
+        <param name="frame_id"        type="string" value="camera_link"/>
+        <param name="scan_normal_k"   type="int" value="10"/>
+
+        <param name="Icp/PointToPlane"  type="string" value="true"/>
+        <param name="Icp/VoxelSize"     type="string" value="0"/>
+        <param name="Icp/PM"            type="string" value="$(arg pm)"/>
+        <param name="Icp/PMOutlierRatio" type="string" value="0.65"/>
+        <param name="Odom/GuessMotion"  type="string" value="true"/>
+        <param name="Odom/ResetCountdown"  type="string" value="1"/>
+      </node>
+    </group>
+
+    <group unless="$(arg nodelet)">
+      <node if="$(arg rgbd)" pkg="rtabmap_ros" type="rgbdicp_odometry" name="rgbdicp_odometry" output="screen">
+        <remap from="scan_cloud"        to="/voxel_cloud"/>
+        <remap from="depth/image"       to="depth_registered/image_raw"/>
+        <remap from="rgb/camera_info"   to="rgb/camera_info"/>
+        <remap from="rgb/image"         to="rgb/image_rect_mono"/>
+
+        <param name="frame_id"        type="string" value="camera_link"/>
+        <param name="scan_normal_k"   type="int" value="10"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+
+        <param name="Icp/PointToPlane"  type="string" value="true"/>
+        <param name="Icp/VoxelSize"     type="string" value="0"/>
+        <param name="Icp/PM"            type="string" value="$(arg pm)"/>
+        <param name="Icp/PMOutlierRatio" type="string" value="0.65"/>
+      </node>
+      <node unless="$(arg rgbd)" pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
+        <remap from="scan_cloud"      to="/voxel_cloud"/>
+
+        <param name="frame_id"        type="string" value="camera_link"/>
+        <param name="scan_normal_k"   type="int" value="10"/>
+
+        <param name="Icp/PointToPlane"  type="string" value="true"/>
+        <param name="Icp/VoxelSize"     type="string" value="0"/>
+        <param name="Icp/PM"            type="string" value="$(arg pm)"/>
+        <param name="Icp/PMOutlierRatio" type="string" value="0.65"/>
+        <param name="Odom/GuessMotion"  type="string" value="true"/>
+        <param name="Odom/ResetCountdown"  type="string" value="1"/>
+      </node>
+    </group>
+
+  </group>
+
+  <!-- We just use odometry without rtabmap node, so set a static /map->/odom
+       transform so that rviz config below works out-of-the-box -->
+  <node pkg="tf" type="static_transform_publisher" name="map_odom"
+        args="0 0 0 0 0 0 map odom 100" />
+
+  <!-- Visualization RVIZ -->
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
+</launch>
@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<launch>
+
+    <arg name="rtabmapviz"    default="true"/>
+
+    <include file="$(find azure_kinect_ros_driver)/launch/driver.launch">
+       <arg name="point_cloud" value="false"/>
+       <arg name="rgb_point_cloud" value="false"/>
+       <arg name="fps" value="15"/>
+    </include>
+
+    <group ns="rtabmap">
+
+      <node pkg="nodelet" type="nodelet" name="points_xyz" args="standalone rtabmap_ros/point_cloud_xyz" output="screen">
+        <remap from="depth/image"       to="/depth_to_rgb/image_raw"/>
+        <remap from="depth/camera_info" to="/depth_to_rgb/camera_info"/>
+        <remap from="cloud"             to="voxel_cloud" />
+
+        <param name="decimation"  type="double" value="4"/>
+        <param name="voxel_size"  type="double" value="0.05"/>
+        <param name="max_depth"   type="double" value="5"/>
+        <param name="normal_k"    type="int"    value="10"/>
+        <param name="approx_sync" type="bool"   value="false"/>
+      </node>
+
+      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
+        <remap from="scan_cloud" to="voxel_cloud"/>
+        <param name="frame_id" type="string" value="camera_base"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/PointToPlane"        type="string" value="true"/>
+        <param name="Icp/PM"                  type="string" value="true"/>
+        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
+
+        <!-- Odom parameters -->
+        <param name="OdomF2M/ScanSubtractRadius" type="string" value="0.05"/>
+        <param name="OdomF2M/ScanMaxSize"        type="string" value="5000"/>
+      </node>
+
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
+        <param name="frame_id"             type="string" value="camera_base"/>
+        <param name="subscribe_depth"      type="bool" value="false"/>
+        <param name="subscribe_rgb"        type="bool" value="false"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync"          type="bool" value="false"/>
+
+        <remap from="scan_cloud" to="voxel_cloud"/>
+
+        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="0"/>
+        <param name="RGBD/ProximityOdomGuess"        type="string" value="true"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/PointToPlane"        type="string" value="true"/>
+        <param name="Icp/PM"                  type="string" value="true"/>
+        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
+        <param name="Icp/MaxTranslation"      type="string" value="0.5"/>
+      </node>
+
+      <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
+        <param name="frame_id" type="string" value="camera_base"/>
+        <param name="subscribe_rgb" type="bool" value="false"/>
+        <param name="subscribe_depth" type="bool" value="false"/>
+        <param name="subscribe_odom_info" type="bool" value="true"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync" type="bool" value="false"/>
+        <remap from="scan_cloud" to="voxel_cloud"/>
+      </node>
+
+  </group>
+
+</launch>
@@ -0,0 +1,51 @@
+<?xml version="1.0"?>
+<launch>
+
+  <!-- Build laser_assembler package from source to have periodic_snapshotter node -->
+  <!-- Also use assemble_scans2 service in periodic_snapshotter, publish PointCloud2 and set duration to 1 second -->
+
+  <!-- Arguments -->
+  <arg name="rtabmap_args"            default="--delete_db_on_start --Rtabmap/DetectionRate 0 --RGBD/ProximityBySpace false --RGBD/LinearUpdate 0 --RGBD/AngularUpdate 0 --RGBD/ProximityPathMaxNeighbors 0"/>
+  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
+  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
+  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
+  <arg name="rtabmapviz"              default="true" />
+  <arg name="rviz"                    default="false" />
+
+  <!-- Kinect -->
+  <include file="$(find freenect_launch)/launch/freenect.launch" >
+    <arg name="depth_registration" value="true"/>
+    <arg name="data_skip"          value="3"/>
+  </include>
+
+  <!-- Laser scan from Depth image -->
+  <node type="depthimage_to_laserscan" pkg="depthimage_to_laserscan" name="depthimage_to_laserscan">
+    <remap from="image"       to="$(arg depth_topic)"/>
+    <remap from="camera_info" to="$(arg camera_info_topic)"/>
+  </node>
+
+  <!-- Laser assembler (convert to PointCloud2) -->
+  <node type="laser_scan_assembler" pkg="laser_assembler" name="laser_assembler">
+    <remap from="scan"        to="scan"/>
+    <param name="max_scans"   type="int"    value="400" />
+    <param name="fixed_frame" type="string" value="odom" />
+  </node>
+  <node type="periodic_snapshotter" pkg="laser_assembler" name="periodic_snapshotter"/>
+
+  <!-- RTAB-Map -->
+  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+    <arg name="rtabmap_args"            value="$(arg rtabmap_args)"/>
+
+    <arg name="queue_size"              value="30" />
+    <arg name="rgb_topic"               value="$(arg rgb_topic)" />
+    <arg name="depth_topic"             value="$(arg depth_topic)" />
+    <arg name="camera_info_topic"       value="$(arg camera_info_topic)" />
+
+    <arg name="subscribe_scan_cloud"    value="true"/>
+    <arg name="scan_cloud_topic"        value="/assembled_cloud2"/>
+
+    <arg name="rtabmapviz"              value="$(arg rtabmapviz)"/>
+    <arg name="rviz"                    value="$(arg rviz)"/>
+  </include>
+
+</launch>
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Example to assemble 3D point clouds from depth when rtabmap is using scan for 2d occupancy grid :
+        $ roslaunch rtabmap_ros demo_robot_mapping.launch rviz:=true rtabmapviz:=false
+        $ roslaunch rtabmap_ros test_map_assembler.launch
+        $ rosbag play -clock demo_mapping.bag
+  -->
+
+  <group ns="rtabmap">
+     <node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
+        <remap from="mapData" to="mapData"/>
+        <param name="regenerate_local_grids" value="true"/>
+     </node>
+  </group>
+</launch>
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<launch>
+   <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+     <arg name="rtabmap_args" value="
+                                     --delete_db_on_start
+                                     --RGBD/OptimizeMaxError 0
+                                     --Optimizer/Iterations 0
+                                     --RGBD/ProximityBySpace false"/>
+     <arg name="rtabmapviz" value="false"/>
+  </include>
+
+  <group ns="rtabmap">
+     <node pkg="rtabmap_ros" type="map_optimizer" name="map_optimizer"/>
+
+     <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
+        <remap from="mapData" to="mapData_optimized"/>
+        <param name="frame_id" value="camera_link"/>
+        <param name="subscribe_depth" value="false"/>
+     </node>
+
+     <node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
+        <remap from="mapData" to="mapData_optimized"/>
+     </node>
+  </group>
+</launch>
@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<launch>
+
+<!-- Use stereo_outdoorA.bag for testing -->
+<include file="$(find rtabmap_ros)/launch/demo/demo_stereo_outdoor.launch"/>
+
+<group ns="/stereo_camera" >
+    <node pkg="nodelet" type="nodelet" name="obstacles_manager" args="manager"/>
+    <node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/point_cloud_xyz obstacles_manager">
+		<remap from="disparity/image"       to="disparity"/>
+		<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
+		<remap from="cloud"                 to="cloudXYZ"/>
+
+		<param name="voxel_size" type="double" value="0.05"/>
+		<param name="decimation" type="int" value="4"/>
+		<param name="max_depth" type="double" value="4"/>
+	</node>
+
+	<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection obstacles_manager">
+	    <remap from="cloud" to="cloudXYZ"/>
+
+		<param name="frame_id" type="string" value="base_footprint"/>
+		<param name="wait_for_transform" type="bool" value="true"/>
+		<param name="min_cluster_size" type="int" value="20"/>
+		<param name="max_obstacles_height" type="double" value="0.0"/>
+    </node>
+</group>
+
+</launch>
@@ -0,0 +1,139 @@
+<?xml version="1.0"?>
+
+<launch>
+
+    <!--
+    Hand-held 3D lidar mapping example using only a Ouster OS-1 (no camera).
+    Prerequisities: rtabmap should be built with libpointmatcher
+    Example:
+     $ roslaunch rtabmap_ros test_ouster.launch os1_hostname:=os1-XXXXXXXXXXXX.local os1_udp_dest:=192.168.1.XXX
+     $ rosrun rviz rviz -f map
+     $ Show TF and /rtabmap/cloud_map topics
+    ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems
+           coming from the first cloud sent by os1_cloud_node, which may be poorly synchronized with IMU data.
+    -->
+
+    <arg name="use_sim_time"  default="false"/>
+
+    <!-- Required: -->
+    <arg unless="$(arg use_sim_time)" name="os1_hostname"/>
+    <arg unless="$(arg use_sim_time)" name="os1_udp_dest"/>
+
+    <arg name="frame_id"      default="os1_sensor"/>
+    <arg name="rtabmapviz"    default="true"/>
+    <arg name="scan_20_hz"    default="true"/>
+    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
+
+    <!-- Ouster -->
+    <remap unless="$(arg use_sim_time)" from="/os1_cloud_node/imu" to="/os1_cloud_node/imu/data_raw"/>
+    <include unless="$(arg use_sim_time)" file="$(find ouster_ros)/os1.launch">
+      <arg name="os1_hostname" value="$(arg os1_hostname)"/>
+      <arg name="os1_udp_dest" value="$(arg os1_udp_dest)"/>
+      <arg     if="$(arg scan_20_hz)" name="lidar_mode" value="1024x20"/>
+      <arg unless="$(arg scan_20_hz)" name="lidar_mode" value="1024x10"/>
+    </include>
+
+    <!-- IMU orientation estimation and publish tf accordingly to os1_sensor frame -->
+    <node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
+      <remap from="imu" to="/os1_cloud_node/imu"/>
+    </node>
+    <node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
+      <param name="use_mag" value="false"/>
+      <param name="world_frame" value="enu"/>
+      <param name="publish_tf" value="false"/>
+    </node>
+    <node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
+      <remap from="imu/data" to="/os1_cloud_node/imu/data"/>
+      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
+      <param name="base_frame_id" value="$(arg frame_id)"/>
+    </node>
+
+    <group ns="rtabmap">
+      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
+        <remap from="scan_cloud" to="/os1_cloud_node/points"/>
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id"   type="string" value="odom"/>
+        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
+        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
+
+        <remap from="imu" to="/os1_cloud_node/imu/data"/>
+        <param name="guess_frame_id"   type="string" value="$(arg frame_id)_stabilized"/>
+        <param name="wait_imu_to_init" type="bool" value="true"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/PointToPlane"        type="string" value="true"/>
+        <param name="Icp/Iterations"          type="string" value="10"/>
+        <param name="Icp/VoxelSize"           type="string" value="0.2"/>
+        <param name="Icp/DownsamplingStep"    type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
+        <param name="Icp/Epsilon"             type="string" value="0.001"/>
+        <param name="Icp/PointToPlaneK"       type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
+        <param name="Icp/MaxTranslation"      type="string" value="2"/>
+        <param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
+        <param name="Icp/PM"                  type="string" value="true"/>
+        <param name="Icp/PMOutlierRatio"      type="string" value="0.1"/>
+        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
+
+        <!-- Odom parameters -->
+        <param name="Odom/ScanKeyFrameThr"       type="string" value="0.95"/>
+        <param name="Odom/Strategy"              type="string" value="0"/>
+        <param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
+        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>
+      </node>
+
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
+        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
+        <param name="subscribe_depth"      type="bool" value="false"/>
+        <param name="subscribe_rgb"        type="bool" value="false"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync"          type="bool" value="false"/>
+
+        <remap from="scan_cloud" to="/os1_cloud_node/points"/>
+        <remap from="imu" to="/os1_cloud_node/imu/data"/>
+
+        <!-- RTAB-Map's parameters -->
+        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>
+        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
+        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
+        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
+        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
+        <param name="RGBD/AngularUpdate"             type="string" value="0.05"/>
+        <param name="RGBD/LinearUpdate"              type="string" value="0.05"/>
+        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
+        <param name="Mem/STMSize"                    type="string" value="30"/>
+        <!-- param name="Mem/LaserScanVoxelSize"     type="string" value="0.1"/ -->
+        <!-- param name="Mem/LaserScanNormalK"       type="string" value="10"/ -->
+        <!-- param name="Mem/LaserScanRadius"        type="string" value="0"/ -->
+
+        <param name="Reg/Strategy"                   type="string" value="1"/>
+        <param name="Grid/CellSize"                  type="string" value="0.1"/>
+        <param name="Grid/RangeMax"                  type="string" value="20"/>
+        <param name="Grid/ClusterRadius"             type="string" value="1"/>
+        <param name="Grid/GroundIsObstacle"          type="string" value="true"/>
+        <param name="Optimizer/GravitySigma"         type="string" value="0.3"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/VoxelSize"                  type="string" value="0.3"/>
+        <param name="Icp/PointToPlaneK"              type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
+        <param name="Icp/PointToPlane"               type="string" value="false"/>
+        <param name="Icp/Iterations"                 type="string" value="10"/>
+        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
+        <param name="Icp/MaxTranslation"             type="string" value="3"/>
+        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="1"/>
+        <param name="Icp/PM"                         type="string" value="true"/>
+        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
+        <param name="Icp/CorrespondenceRatio"        type="string" value="0.4"/>
+      </node>
+
+      <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
+        <param name="frame_id" type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id" type="string" value="odom"/>
+        <param name="subscribe_odom_info" type="bool" value="true"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync" type="bool" value="false"/>
+        <remap from="scan_cloud" to="/os1_cloud_node/points"/>
+      </node>
+  </group>
+
+</launch>
@@ -0,0 +1,191 @@
+<?xml version="1.0"?>
+
+<launch>
+
+    <!--
+    Hand-held 3D lidar mapping example using only a Ouster GEN2 (no camera).
+    Prerequisities: rtabmap should be built with libpointmatcher
+
+    Example:
+
+      $ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX
+      $ rosrun rviz rviz -f map
+      RVIZ: Show TF and /rtabmap/cloud_map topics
+
+    ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems
+           coming from the first cloud sent by os_cloud_node, which may be poorly synchronized with IMU data.
+
+    PTP mode (synchronize timestamp with host computer time)
+
+      * Install:
+
+        $ sudo apt install linuxptp httpie
+        $ printf "[global]\ntx_timestamp_timeout 10\n" >> ~/os.conf
+
+      * Running:
+
+        (replace "XXXXXXXXXXXX" by your ouster serial, as well as XXX by its IP address)
+        (replace "eth0" by the network interface used to communicate with ouster)
+
+        $ http PUT http://os-XXXXXXXXXXXX.local/api/v1/time/ptp/profile <<< '"default-relaxed"'
+        $ sudo ptp4l -i eth0 -m -f ~/os.conf -S
+        $ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX ptp:=true
+
+    -->
+
+    <arg name="use_sim_time"  default="false"/>
+
+    <!-- Required: -->
+    <arg unless="$(arg use_sim_time)" name="sensor_hostname"/>
+    <arg unless="$(arg use_sim_time)" name="udp_dest"/>
+
+    <arg name="frame_id"      default="os_sensor"/>
+    <arg name="rtabmapviz"    default="true"/>
+    <arg name="deskewing"     default="true"/>
+    <arg name="slerp"         default="false"/>
+    <arg name="scan_20_hz"    default="true"/>
+    <arg name="voxel_size"    default="0.15"/>           <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 -->
+    <arg name="assemble"      default="false"/>
+    <arg name="ptp"           default="false"/>          <!-- See comments in header to start before launching the launch -->
+    <arg name="imu_topic"     default="/os_cloud_node/imu"/>
+    <arg name="scan_topic"    default="/os_cloud_node/points"/>
+
+    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
+
+    <!-- Ouster -->
+    <include unless="$(arg use_sim_time)" file="$(find ouster_ros)/ouster.launch">
+      <arg name="sensor_hostname" value="$(arg sensor_hostname)"/>
+      <arg name="udp_dest" value="$(arg udp_dest)"/>
+      <arg name="image" value="true"/>
+      <arg     if="$(arg scan_20_hz)" name="lidar_mode" value="1024x20"/>
+      <arg unless="$(arg scan_20_hz)" name="lidar_mode" value="1024x10"/>
+      <arg if="$(arg ptp)" name="timestamp_mode" value="TIME_FROM_PTP_1588"/>
+    </include>
+
+    <!-- IMU orientation estimation and publish tf accordingly to os_sensor frame -->
+    <node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
+      <remap from="imu/data_raw" to="$(arg imu_topic)"/>
+      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
+    </node>
+    <node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
+      <param name="use_mag" value="false"/>
+      <param name="world_frame" value="enu"/>
+      <param name="publish_tf" value="false"/>
+    </node>
+    <node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
+      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
+      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
+      <param name="base_frame_id" value="$(arg frame_id)"/>
+    </node>
+
+    <!-- Lidar Deskewing -->
+    <node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
+      <param name="wait_for_transform" value="0.01"/>
+      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
+      <param name="slerp" value="$(arg slerp)"/>
+      <remap from="input_cloud" to="$(arg scan_topic)"/>
+    </node>
+
+    <arg     if="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)/deskewed"/>
+    <arg unless="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)"/>
+
+    <group ns="rtabmap">
+      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
+        <remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
+        <remap from="imu"        to="$(arg imu_topic)/filtered"/>
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id"   type="string" value="odom"/>
+        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
+        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
+
+        <param name="guess_frame_id"   type="string" value="$(arg frame_id)_stabilized"/>
+        <param name="wait_imu_to_init" type="bool" value="true"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/PointToPlane"        type="string" value="true"/>
+        <param name="Icp/Iterations"          type="string" value="10"/>
+        <param name="Icp/VoxelSize"           type="string" value="$(arg voxel_size)"/>
+        <param name="Icp/DownsamplingStep"    type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
+        <param name="Icp/Epsilon"             type="string" value="0.001"/>
+        <param name="Icp/PointToPlaneK"       type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
+        <param name="Icp/MaxTranslation"      type="string" value="2"/>
+        <param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
+        <param name="Icp/PM"                  type="string" value="true"/>
+        <param name="Icp/PMOutlierRatio"      type="string" value="0.1"/>
+        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
+
+        <!-- Odom parameters -->
+        <param name="Odom/ScanKeyFrameThr"       type="string" value="0.95"/>
+        <param name="Odom/Strategy"              type="string" value="0"/>
+        <param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg voxel_size)"/>
+        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>
+      </node>
+
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
+        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
+        <param name="subscribe_depth"      type="bool" value="false"/>
+        <param name="subscribe_rgb"        type="bool" value="false"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync"          type="bool" value="false"/>
+
+        <remap     if="$(arg assemble)" from="scan_cloud" to="assembled_cloud"/>
+        <remap unless="$(arg assemble)" from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
+        <remap from="imu"        to="$(arg imu_topic)/filtered"/>
+
+        <!-- RTAB-Map's parameters -->
+        <param     if="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- already set 1 Hz in point_cloud_assembler -->
+        <param unless="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="1"/>
+        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
+        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
+        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
+        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
+        <param name="RGBD/LocalRadius"               type="string" value="2"/>
+        <param name="RGBD/AngularUpdate"             type="string" value="0.05"/>
+        <param name="RGBD/LinearUpdate"              type="string" value="0.05"/>
+        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
+        <param name="Mem/STMSize"                    type="string" value="30"/>
+        <!-- param name="Mem/LaserScanVoxelSize"     type="string" value="0.1"/ -->
+        <!-- param name="Mem/LaserScanNormalK"       type="string" value="10"/ -->
+        <!-- param name="Mem/LaserScanRadius"        type="string" value="0"/ -->
+
+        <param name="Reg/Strategy"                   type="string" value="1"/>
+        <param name="Optimizer/GravitySigma"         type="string" value="0.5"/>
+        <param name="Optimizer/Strategy"             type="string" value="1"/>
+        <param name="Grid/CellSize"                  type="string" value="0.1"/>
+        <param name="Grid/RangeMax"                  type="string" value="20"/>
+        <param name="Grid/ClusterRadius"             type="string" value="1"/>
+        <param name="Grid/GroundIsObstacle"          type="string" value="true"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/VoxelSize"                  type="string" value="$(arg voxel_size)"/>
+        <param name="Icp/PointToPlaneK"              type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
+        <param name="Icp/PointToPlane"               type="string" value="true"/>
+        <param name="Icp/Iterations"                 type="string" value="10"/>
+        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
+        <param name="Icp/MaxTranslation"             type="string" value="3"/>
+        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="1"/>
+        <param name="Icp/PM"                         type="string" value="true"/>
+        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
+        <param name="Icp/CorrespondenceRatio"        type="string" value="0.2"/>
+      </node>
+
+      <node if="$(arg assemble)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
+        <remap from="cloud" to="$(arg scan_topic_deskewed)"/>
+        <remap from="odom"            to="odom"/>
+        <param name="assembling_time" type="double" value="1" />
+        <param name="fixed_frame_id"  type="string" value="" />
+      </node>
+
+      <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
+        <param name="frame_id" type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id" type="string" value="odom"/>
+        <param name="subscribe_odom_info" type="bool" value="true"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync" type="bool" value="false"/>
+        <remap from="scan_cloud" to="odom_filtered_input_scan"/>
+      </node>
+  </group>
+
+</launch>
@@ -0,0 +1,83 @@
+<?xml version="1.0"?>
+
+<launch>
+
+  <include file="$(find freenect_launch)/launch/freenect.launch" >
+    <arg name="depth_registration" value="true"/>
+    <arg name="data_skip" value="3"/>
+  </include>
+
+  <group ns="camera">
+  <node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager">
+    <remap from="rgb/image_in"       to="rgb/image_rect_color"/>
+    <remap from="depth/image_in"     to="depth_registered/image_raw"/>
+    <remap from="rgb/camera_info_in" to="depth_registered/camera_info"/>
+
+    <param name="decimation" type="int" value="2"/>
+  </node>
+
+  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb camera_nodelet_manager">
+    <remap from="rgb/image"       to="rgb/image_out"/>
+    <remap from="depth/image"     to="depth/image_out"/>
+    <remap from="rgb/camera_info" to="rgb/camera_info_out"/>
+    <remap from="cloud"           to="/voxel_cloud_xyzrgb" />
+
+    <param name="voxel_size" type="double" value="0.02"/>
+    <param name="decimation" type="int" value="2"/>
+    <param name="noise_filter_radius" type="double" value="0.05"/>
+    <param name="normal_k" type="int" value="6"/>
+  </node>
+
+  <node pkg="nodelet" type="nodelet" name="points_xyz" args="load rtabmap_ros/point_cloud_xyz camera_nodelet_manager">
+    <remap from="depth/image"     to="depth/image_out"/>
+    <remap from="depth/camera_info" to="rgb/camera_info_out"/>
+    <remap from="cloud"           to="/voxel_cloud_xyz" />
+
+    <param name="voxel_size" type="double" value="0.02"/>
+    <param name="decimation" type="int" value="2"/>
+    <param name="noise_filter_radius" type="double" value="0.05"/>
+    <param name="normal_k" type="int" value="6"/>
+  </node>
+  </group>
+
+  <!-- stereo test with stereo_outdoorA.bag -->
+<!--
+   <param name="use_sim_time" value="true"/>
+
+   <node name="republish_left" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/left/image_raw_throttle raw out:=/stereo_camera/left/image_raw_throttle_relay" />
+   <node name="republish_right" type="republish" pkg="image_transport" args="compressed in:=/stereo_camera/right/image_raw_throttle raw out:=/stereo_camera/right/image_raw_throttle_relay" />
+
+   <group ns="/stereo_camera" >
+      <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
+         <remap from="left/image_raw"    to="left/image_raw_throttle_relay"/>
+         <remap from="left/camera_info"  to="left/camera_info_throttle"/>
+         <remap from="right/image_raw"   to="right/image_raw_throttle_relay"/>
+         <remap from="right/camera_info" to="right/camera_info_throttle"/>
+         <param name="disparity_range" value="128"/>
+      </node>
+   </group>
+
+  <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
+  <node pkg="nodelet" type="nodelet" name="stereo_throttle" args="load rtabmap_ros/stereo_throttle standalone_nodelet">
+    <remap from="left/image"         to="/stereo_camera/left/image_rect_color"/>
+    <remap from="right/image"        to="/stereo_camera/right/image_rect"/>
+    <remap from="left/camera_info"   to="/stereo_camera/left/camera_info_throttle"/>
+    <remap from="right/camera_info"  to="/stereo_camera/right/camera_info_throttle"/>
+
+    <param name="decimation" type="int" value="2"/>
+    <param name="approx_sync" type="bool" value="false"/>
+  </node>
+
+  <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
+    <remap from="left/image"         to="/stereo_camera/left/image_rect_color_throttle"/>
+    <remap from="right/image"        to="/stereo_camera/right/image_rect_throttle"/>
+    <remap from="left/camera_info"   to="/stereo_camera/left/camera_info_throttle_throttle"/>
+    <remap from="right/camera_info"  to="/stereo_camera/right/camera_info_throttle_throttle"/>
+    <remap from="cloud"              to="/voxel_cloud" />
+
+    <param name="decimation" type="int" value="2"/>
+    <param name="approx_sync" type="bool" value="false"/>
+  </node>
+-->
+
+</launch>
@@ -0,0 +1,84 @@
+<?xml version="1.0"?>
+
+<launch>
+
+   <!-- Example with rgbd datasets:
+   $ wget http://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.bag
+   $ rosbag decompress rgbd_dataset_freiburg3_long_office_household.bag
+   $ chmod +x test_prior_rename_kinect_bag_tf.py
+   $ ./test_prior_rename_kinect_bag_tf.py
+
+   We simulate an external "global_pose" by republishing ground truth TF (VICON) with some
+   covariance, normally you should not have to use test_prior_tf_to_pose.py as the node
+   publishing the global pose would give it directly as a pose with correct covariance.
+
+   Rename all child_frame_id "/kinect" to "/kinect_gt" Tf in the bag
+   using test_prior_rename_kinect_bag_tf.py in this directory!
+
+   $ roslaunch rtabmap_ros test_prior.launch
+   $ chmod +x test_prior_tf_to_pose.py
+   $ ./test_prior_tf_to_pose.py
+   $ rosbag play -.-clock rgbd_dataset_freiburg3_long_office_household_tf_renamed.bag
+   -->
+
+   <param name="use_sim_time" type="bool" value="True"/>
+
+   <!-- Choose visualization -->
+   <arg name="rviz" default="true" />
+   <arg name="rtabmapviz" default="false" />
+
+   <!-- TF FRAMES -->
+  <node pkg="tf" type="static_transform_publisher" name="world_to_map"
+    args="0.0 0.0 0.0 0.0 0.0 0.0 /world /map 100" />
+
+  <group ns="rtabmap">
+
+    <!-- Odometry -->
+    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
+      <remap from="rgb/image"       to="/camera/rgb/image_color"/>
+      <remap from="depth/image"     to="/camera/depth/image"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
+      <remap from="odom" to="vis_odom"/>
+
+      <param name="odom_frame_id" type="string" value="vis_odom"/>
+      <param name="frame_id" type="string" value="kinect"/>
+    </node>
+
+    <!-- Visual SLAM -->
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
+      <param name="subscribe_depth" type="bool" value="true"/>
+      <param name="frame_id" type="string" value="kinect"/>
+
+      <param name="RGBD/CreateOccupancyGrid" type="string" value="false"/>
+      <param name="Optimizer/PriorsIgnored" type="string" value="false"/>
+      <param name="Optimizer/Strategy" type="string" value="1"/>
+
+      <remap from="rgb/image" to="/camera/rgb/image_color"/>
+      <remap from="depth/image" to="/camera/depth/image"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
+      <remap from="odom" to="vis_odom"/>
+      <remap from="global_pose" to="/global_pose"/>
+      <param name="ground_truth_frame_id" type="string" value="world"/>
+      <param name="ground_truth_base_frame_id" type="string" value="kinect_gt"/>
+    </node>
+
+    <!-- Visualisation  -->
+    <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+      <param name="subscribe_depth" type="bool" value="true"/>
+      <param name="subscribe_laserScan" type="bool" value="false"/>
+      <param name="subscribe_odom_info" type="bool" value="true"/>
+      <param name="queue_size" type="int" value="30"/>
+
+      <param name="frame_id" type="string" value="kinect"/>
+
+      <remap from="rgb/image" to="/camera/rgb/image_color"/>
+      <remap from="depth/image" to="/camera/depth/image"/>
+      <remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
+      <remap from="odom" to="vis_odom"/>
+    </node>
+
+  </group>
+
+  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbdslam_datasets.rviz"/>
+
+</launch>
@@ -0,0 +1,19 @@
+#!/usr/bin/env python
+import rosbag
+from tf.msg import tfMessage
+with rosbag.Bag('rgbd_dataset_freiburg3_long_office_household_tf_renamed.bag', 'w') as outbag:
+	for topic, msg, t in rosbag.Bag('rgbd_dataset_freiburg3_long_office_household.bag').read_messages():
+		if topic == "/tf" and msg.transforms:
+			newList = [];
+			for m in msg.transforms:
+				if m.child_frame_id != "/kinect":
+					newList.append(m)
+				else:
+					m.child_frame_id = "/kinect_gt"
+					newList.append(m)
+					print 'kinect frame renamed!'
+			if len(newList)>0:
+				msg.transforms = newList
+				outbag.write(topic, msg, t)
+		else:
+			outbag.write(topic, msg, t)
@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+import rospy
+import tf
+import numpy
+import tf2_ros
+from geometry_msgs.msg import PoseWithCovarianceStamped
+
+if __name__ == '__main__':
+    rospy.init_node('tf_to_pose', anonymous=True)
+    listener = tf.TransformListener()
+    frame = rospy.get_param('~frame', 'world')
+    childFrame = rospy.get_param('~child_frame', 'kinect_gt')
+    outputFrame = rospy.get_param('~output_frame', 'kinect')
+    cov = rospy.get_param('~cov', 1)
+    rateParam = rospy.get_param('~rate', 30) # 10hz
+    pub = rospy.Publisher('global_pose', PoseWithCovarianceStamped, queue_size=1)
+
+    print 'start loop!'
+    rate = rospy.Rate(rateParam)
+    while not rospy.is_shutdown():
+        poseOut = PoseWithCovarianceStamped()
+        try:
+            now = rospy.get_rostime()
+            listener.waitForTransform(frame, childFrame, now, rospy.Duration(0.033))
+            (trans,rot) = listener.lookupTransform(frame, childFrame, now)
+        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException, tf2_ros.TransformException), e:
+            print str(e)
+            rate.sleep()
+            continue
+
+        poseOut.header.stamp.nsecs = now.nsecs
+        poseOut.header.stamp.secs = now.secs
+        poseOut.header.frame_id = outputFrame
+        poseOut.pose.pose.position.x = trans[0]
+        poseOut.pose.pose.position.y = trans[1]
+        poseOut.pose.pose.position.z = trans[2]
+        poseOut.pose.pose.orientation.x = rot[0]
+        poseOut.pose.pose.orientation.y = rot[1]
+        poseOut.pose.pose.orientation.z = rot[2]
+        poseOut.pose.pose.orientation.w = rot[3]
+        poseOut.pose.covariance = (cov * numpy.eye(6, dtype=numpy.float64)).tolist()
+        poseOut.pose.covariance = [item for sublist in poseOut.pose.covariance for item in sublist]
+
+        print str(poseOut)
+        pub.publish(poseOut)
+        rate.sleep()
@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+
+<launch>
+
+  <!-- Kinect: -->
+  <include file="$(find freenect_launch)/launch/freenect.launch">
+    <arg name="depth_registration" value="true" />
+  </include>
+
+  <arg name="compressed" default="false"/>
+  <arg name="compressed_rate" default="0"/>
+
+  <group ns="camera">
+    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager" output="screen">
+      <remap from="rgb/image"         to="rgb/image_rect_color"/>
+      <remap from="depth/image"       to="depth_registered/image_raw"/>
+      <remap from="rgb/camera_info"   to="rgb/camera_info"/>
+      <param name="compressed_rate" value="$(arg compressed_rate)"/>
+    </node>
+  </group>
+
+   <!-- Nodes -->
+  <group ns="rtabmap">
+
+    <!-- RGB-D Odometry -->
+    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
+      <param name="subscribe_rgbd"       type="bool"   value="true"/>
+      <remap     if="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image/compressed"/>
+      <remap unless="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image"/>
+
+	  <param name="Odom/AlignWithGround"  type="string" value="true"/>
+      <param name="frame_id"         type="string" value="camera_link"/>
+    </node>
+
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
+      <param name="subscribe_depth"       type="bool"   value="false"/>
+      <param name="subscribe_rgbd"       type="bool"   value="true"/>
+      <param name="frame_id"             type="string" value="camera_link"/>
+      <param name="approx_sync"          type="string" value="false"/>
+      <remap     if="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image/compressed"/>
+      <remap unless="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image"/>
+    </node>
+
+    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
+      <param name="subscribe_depth"      type="bool"   value="false"/>
+      <param name="subscribe_rgbd"       type="bool"   value="true"/>
+      <param name="frame_id"             type="string" value="camera_link"/>
+      <param name="approx_sync"          type="string" value="false"/>
+      <remap     if="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image/compressed"/>
+      <remap unless="$(arg compressed)"    from="rgbd_image"   to="/camera/rgbd_image"/>
+      <param name="subscribe_odom_info"  type="bool"   value="true"/>
+    </node>
+
+  </group>
+
+</launch>
@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+
+<launch>
+
+  <!-- Kinect: -->
+  <include file="$(find freenect_launch)/launch/freenect.launch">
+    <arg name="depth_registration" value="true" />
+  </include>
+
+  <arg name="frame_id"                default="camera_link"/>
+  <arg name="rtabmap_args"            default="--delete_db_on_start"/>   <!-- delete_db_on_start, udebug -->
+  <arg name="odom_args"               default="$(arg rtabmap_args)"/>
+
+  <!-- RGB-D related topics -->
+  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
+  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
+  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
+
+  <group ns="camera">
+
+    <!-- Use RGBD synchronization -->
+    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager">
+      <remap from="rgb/image"         to="$(arg rgb_topic)"/>
+      <remap from="depth/image"       to="$(arg depth_topic)"/>
+      <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
+    </node>
+
+    <!-- RGB-D Odometry -->
+      <node pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_ros/rgbd_odometry camera_nodelet_manager $(arg odom_args)">
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+        <param name="subscribe_depth" type="bool"   value="false"/>
+        <param name="subscribe_rgbd"  type="bool"   value="true"/>
+      </node>
+
+    <!-- RTAB-Map -->
+    <node pkg="nodelet" type="nodelet" name="rtabmap" args="load rtabmap_ros/rtabmap camera_nodelet_manager $(arg rtabmap_args)">
+      <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+      <param name="subscribe_depth" type="bool"   value="false"/>
+      <param name="subscribe_rgbd"  type="bool"   value="true"/>
+      <param name="approx_sync"     type="bool"   value="false"/>
+    </node>
+
+    <!-- Visualisation -->
+    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
+      <param name="frame_id" type="string" value="$(arg frame_id)"/>
+      <param name="subscribe_rgbd"  type="bool"   value="true"/>
+      <param name="subscribe_odom_info"  type="bool"   value="true"/>
+      <param name="approx_sync"     type="bool"   value="false"/>
+    </node>
+
+  </group>
+
+</launch>
@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+
+<launch>
+
+  <!-- Testing 2 Kinects localizing in the same map at the same time -->
+  <!-- Prerequisities: the default ~/.ros/rtabmap.db should be
+                       already created with one of the Kinect using mapping tutorial:
+                       http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping
+              $ roslaunch freenect_launch freenect.launch depth_registration:=true
+              $ roslaunch rtabmap_ros rtabmap.launch args:="-d"
+  -->
+
+
+  <!-- Cameras -->
+  <include file="$(find freenect_launch)/launch/freenect.launch">
+    <arg name="depth_registration" value="True" />
+    <arg name="camera" value="camera1" />
+    <arg name="device_id" value="#1" />
+  </include>
+
+  <include file="$(find freenect_launch)/launch/freenect.launch">
+    <arg name="depth_registration" value="True" />
+    <arg name="camera" value="camera2" />
+    <arg name="device_id" value="#2" />
+  </include>
+
+  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+     <arg name="namespace"         value="rtabmap1"/>
+     <arg name="localization"      value="true" />
+     <arg name="frame_id"          value="camera1_link" />
+     <arg name="vo_frame_id"       value="odom1" />
+
+     <arg name="rgb_topic"         value="/camera1/rgb/image_rect_color" />
+     <arg name="depth_topic"       value="/camera1/depth_registered/image_raw" />
+     <arg name="camera_info_topic" value="/camera1/rgb/camera_info" />
+
+     <arg name="rtabmapviz" value="false"/>
+  </include>
+
+  <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
+     <arg name="namespace"         value="rtabmap2"/>
+     <arg name="localization"      value="true" />
+     <arg name="frame_id"          value="camera2_link" />
+     <arg name="vo_frame_id"       value="odom2" />
+
+     <arg name="rgb_topic"         value="/camera2/rgb/image_rect_color" />
+     <arg name="depth_topic"       value="/camera2/depth_registered/image_raw" />
+     <arg name="camera_info_topic" value="/camera2/rgb/camera_info" />
+
+     <arg name="rtabmapviz" value="false"/>
+  </include>
+
+  <!-- Visualization RVIZ -->
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
+
+</launch>
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+
+<launch>
+
+  <!-- Kinect: -->
+  <include file="$(find openni2_launch)/launch/openni2.launch">
+    <arg name="depth_registration" value="true" />
+  </include>
+
+  <arg name="depth"  default="/camera/depth_registered/image_raw" />
+  <arg name="model"  default="$(find rtabmap_ros)/launch/calibration/distortion_model_PS1080.bin" /> <!-- XTION Live Pro -->
+
+  <group ns="camera">
+    <!-- Undistort depth image -->
+    <node pkg="nodelet" type="nodelet" name="undistort" args="load rtabmap_ros/undistort_depth camera_nodelet_manager">
+      <remap from="depth" to="$(arg depth)"/>
+      <param name="model" value="$(arg model)"/>
+    </node>
+  </group>
+
+</launch>
@@ -0,0 +1,54 @@
+<?xml version="1.0"?>
+
+<launch>
+
+  <!-- Kinect: -->
+  <include file="$(find freenect_launch)/launch/freenect.launch">
+    <arg name="depth_registration" value="true" />
+  </include>
+
+  <arg name="frame_id"                default="camera_link"/>
+  <arg name="rtabmap_args"            default="--delete_db_on_start"/>   <!-- delete_db_on_start, udebug -->
+  <arg name="odom_args"               default="$(arg rtabmap_args)"/>
+
+  <!-- RGB-D related topics -->
+  <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
+  <arg name="depth_topic"             default="/camera/depth_registered/image_raw" />
+  <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
+
+  <group ns="camera">
+
+    <!-- Use RGBD synchronization -->
+    <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync camera_nodelet_manager">
+      <remap from="rgb/image"         to="$(arg rgb_topic)"/>
+      <remap from="depth/image"       to="$(arg depth_topic)"/>
+      <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>
+    </node>
+
+    <!-- RGB-D Odometry -->
+      <node pkg="nodelet" type="nodelet" name="rgbd_odometry" args="load rtabmap_ros/rgbd_odometry camera_nodelet_manager $(arg odom_args)">
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+        <param name="subscribe_depth" type="bool"   value="false"/>
+        <param name="subscribe_rgbd"  type="bool"   value="true"/>
+        <param name="keep_color"      type="bool"   value="true"/>
+      </node>
+
+    <!-- RTAB-Map -->
+    <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" args="$(arg rtabmap_args)" output="screen">
+      <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+      <param name="subscribe_rgbd"  type="bool"   value="true"/>
+      <param name="approx_sync"     type="bool"   value="false"/>
+      <remap from="rgbd_image" to="odom_rgbd_image"/>
+    </node>
+
+    <!-- Visualisation -->
+    <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" output="screen">
+      <param name="frame_id" type="string" value="$(arg frame_id)"/>
+      <param name="subscribe_rgbd"  type="bool"   value="true"/>
+      <param name="subscribe_odom_info"  type="bool"   value="true"/>
+      <param name="approx_sync"     type="bool"   value="false"/>
+    </node>
+
+  </group>
+
+</launch>
@@ -0,0 +1,179 @@
+<?xml version="1.0"?>
+<!-- -->
+<launch>
+
+    <!--
+    Hand-held 3D lidar mapping example using only a Velodyne PUCK (no camera).
+    Prerequisities: rtabmap should be built with libpointmatcher
+    Example:
+     $ roslaunch rtabmap_ros test_velodyne.launch
+     $ rosrun rviz rviz -f map
+     $ Show TF and /rtabmap/cloud_map topics
+    -->
+
+    <arg name="rtabmapviz"    default="true"/>
+    <arg name="use_imu"       default="false"/> <!-- Assuming IMU fixed to lidar with /velodyne -> /imu_link TF -->
+    <arg name="imu_topic"     default="/imu/data"/>
+    <arg name="scan_20_hz"    default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
+    <arg name="deskewing"     default="true"/>
+    <arg name="slerp"         default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
+    <arg name="organize_cloud" default="$(arg deskewing)"/> <!-- Should be organized if deskewing is enabled -->
+    <arg name="scan_topic"    default="/velodyne_points"/>
+    <arg name="use_sim_time"  default="false"/>
+    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
+
+    <arg name="frame_id"        default="velodyne"/>
+    <arg name="queue_size"      default="10"/> <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
+    <arg name="queue_size_odom" default="1"/>  <!-- Set to 100 for kitti dataset to make sure all scans are processed -->
+    <arg name="loop_ratio"      default="0.2"/>
+
+    <arg name="resolution"         default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor (0.4 for kitti) -->
+    <arg name="iterations"         default="10"/>
+
+    <!-- Grid parameters -->
+    <arg name="ground_is_obstacle" default="true"/>
+    <arg name="grid_max_range"     default="20"/>
+
+    <!-- For F2M Odometry -->
+    <arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car, kitti) -->
+    <arg name="local_map_size"    default="15000"/>
+    <arg name="key_frame_thr"     default="0.6"/>
+
+    <!-- For FLOAM Odometry -->
+    <arg name="floam"    default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
+    <arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings (kitti dataset) -->
+
+    <include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
+       <arg     if="$(arg scan_20_hz)" name="rpm" value="1200"/>
+       <arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
+       <arg name="organize_cloud" value="$(arg organize_cloud)"/>
+    </include>
+
+    <!-- IMU orientation estimation and publish tf -->
+    <node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
+      <remap from="imu/data_raw" to="$(arg imu_topic)"/>
+      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
+    </node>
+    <node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
+      <param name="use_mag" value="false"/>
+      <param name="world_frame" value="enu"/>
+      <param name="publish_tf" value="false"/>
+    </node>
+    <node if="$(arg use_imu)" pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
+      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
+      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
+      <param name="base_frame_id" value="$(arg frame_id)"/>
+    </node>
+
+    <group ns="rtabmap">
+      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
+        <remap from="scan_cloud" to="$(arg scan_topic)"/>
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id"   type="string" value="odom"/>
+        <param name="deskewing"       type="bool"   value="$(arg deskewing)"/>
+        <param name="deskewing_slerp" type="bool"   value="$(arg slerp)"/>
+        <param name="queue_size"      type="int"    value="$(arg queue_size_odom)"/>
+        <param name="wait_for_transform_duration" type="double" value="0.2"/>
+        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
+        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
+
+        <remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)/filtered"/>
+        <param if="$(arg use_imu)" name="guess_frame_id"   type="string" value="$(arg frame_id)_stabilized"/>
+        <param if="$(arg use_imu)" name="wait_imu_to_init" type="bool" value="true"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/PointToPlane"        type="string" value="true"/>
+        <param name="Icp/Iterations"          type="string" value="$(arg iterations)"/>
+        <param     if="$(arg floam)" name="Icp/VoxelSize" type="string" value="0"/>
+        <param unless="$(arg floam)" name="Icp/VoxelSize" type="string" value="$(arg resolution)"/>
+        <param name="Icp/DownsamplingStep"    type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
+        <param name="Icp/Epsilon"             type="string" value="0.001"/>
+        <param     if="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="0"/>
+        <param unless="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
+        <param name="Icp/MaxTranslation"      type="string" value="2"/>
+        <param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
+        <param name="Icp/PM"                  type="string" value="true"/>
+        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
+        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
+        <param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp"  type="string" value="0.8"/>
+
+        <!-- Odom parameters -->
+        <param name="Odom/ScanKeyFrameThr"       type="string" value="$(arg key_frame_thr)"/>
+        <param     if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
+        <param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
+        <param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
+        <param name="OdomF2M/ScanMaxSize"        type="string" value="$(arg local_map_size)"/>
+        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/>
+        <param name="OdomLOAM/Resolution"        type="string" value="$(arg resolution)"/>
+        <param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
+        <param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
+        <param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
+      </node>
+
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
+        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
+        <param name="subscribe_depth"      type="bool" value="false"/>
+        <param name="subscribe_rgb"        type="bool" value="false"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync"          type="bool" value="false"/>
+        <param name="wait_for_transform_duration" type="double" value="0.2"/>
+
+        <remap from="scan_cloud" to="assembled_cloud"/>
+        <remap from="imu" to="$(arg imu_topic)/filtered"/>
+
+        <!-- RTAB-Map's parameters -->
+        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>
+        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
+        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
+        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
+        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
+        <param name="RGBD/AngularUpdate"             type="string" value="0.05"/>
+        <param name="RGBD/LinearUpdate"              type="string" value="0.05"/>
+        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
+        <param name="Mem/STMSize"                    type="string" value="30"/>
+        <param name="Mem/LaserScanNormalK"           type="string" value="20"/>
+
+        <param name="Reg/Strategy"                   type="string" value="1"/>
+        <param name="Grid/CellSize"                  type="string" value="$(arg resolution)"/>
+        <param name="Grid/RangeMax"                  type="string" value="$(arg grid_max_range)"/>
+        <param name="Grid/ClusterRadius"             type="string" value="1"/>
+        <param name="Grid/GroundIsObstacle"          type="string" value="$(arg ground_is_obstacle)"/>
+        <param name="Optimizer/GravitySigma"         type="string" value="0.3"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/VoxelSize"                  type="string" value="$(arg resolution)"/>
+        <param name="Icp/PointToPlaneK"              type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
+        <param name="Icp/PointToPlane"               type="string" value="true"/>
+        <param name="Icp/Iterations"                 type="string" value="$(arg iterations)"/>
+        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
+        <param name="Icp/MaxTranslation"             type="string" value="3"/>
+        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="$(eval resolution*10)"/>
+        <param name="Icp/PM"                         type="string" value="true"/>
+        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
+        <param name="Icp/CorrespondenceRatio"        type="string" value="$(arg loop_ratio)"/>
+      </node>
+
+      <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
+        <param name="frame_id" type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id" type="string" value="odom"/>
+        <param name="subscribe_odom_info" type="bool" value="true"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync" type="bool" value="false"/>
+        <remap from="scan_cloud" to="odom_filtered_input_scan"/>
+        <remap from="odom_info" to="odom_info"/>
+      </node>
+
+      <node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
+        <remap     if="$(arg deskewing)" from="cloud" to="odom_filtered_input_scan"/>
+        <remap unless="$(arg deskewing)" from="cloud" to="$(arg scan_topic)"/>
+        <remap from="odom"            to="odom"/>
+        <param     if="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="20" />
+        <param unless="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="10" />
+        <param name="fixed_frame_id"  type="string" value="" />
+        <param name="queue_size"      type="int"    value="$(arg queue_size)" />
+      </node>
+  </group>
+
+</launch>
@@ -0,0 +1,200 @@
+<?xml version="1.0"?>
+<!-- -->
+<launch>
+
+    <!--
+    Hand-held 3D lidar mapping example using a Velodyne PUCK, an external IMU and color camera (using D435i as example).
+    Prerequisities: rtabmap should be built with libpointmatcher
+    We use D435i imu only for lidar deskewing and icp_odometry guess in this example.
+    Example:
+     $ roslaunch rtabmap_ros test_velodyne_d435i_deskewing.launch
+     $ rosrun rviz rviz -f map
+     $ Show TF and /rtabmap/cloud_map topics
+    -->
+
+    <arg name="rtabmapviz"    default="true"/>
+    <arg name="scan_20_hz"    default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
+    <arg name="deskewing"     default="true"/>
+    <arg name="slerp"         default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
+    <arg name="scan_topic"    default="/velodyne_points"/>
+    <arg name="use_sim_time"  default="false"/>
+    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
+
+    <arg name="imu_topic"       default="/camera/imu"/>
+    <arg name="frame_id"        default="velodyne"/>       <!-- base frame of the robot: for this example, we use velodyne as base frame -->
+    <arg name="queue_size"      default="10"/>
+    <arg name="queue_size_odom" default="1"/>
+    <arg name="loop_ratio"      default="0.2"/>
+
+    <arg name="resolution"         default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor -->
+    <arg name="iterations"         default="10"/>
+
+    <!-- Grid parameters -->
+    <arg name="ground_is_obstacle" default="true"/>
+    <arg name="grid_max_range"     default="20"/>
+
+    <!-- For F2M Odometry -->
+    <arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car) -->
+    <arg name="local_map_size"    default="15000"/>
+    <arg name="key_frame_thr"     default="0.6"/>
+
+    <!-- For FLOAM Odometry -->
+    <arg name="floam"    default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
+    <arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings -->
+
+    <!-- Static transform between velodyne and D435i: TODO: Adjust with real position/orientation!!! -->
+    <node unless="$(arg use_sim_time)" pkg="tf" type="static_transform_publisher" name="velodyne_to_camera_tf" args="0.03 0.064 -0.055 0 -0.02 0 velodyne camera_link 100"/>
+
+    <!-- Velodyne sensor VLP16 -->
+    <include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
+       <arg     if="$(arg scan_20_hz)" name="rpm" value="1200"/>
+       <arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
+       <arg name="organize_cloud" value="true"/> <!-- should be organized for deskewing -->
+    </include>
+
+    <!-- D435i -->
+    <group unless="$(arg use_sim_time)">
+      <include file="$(find realsense2_camera)/launch/rs_camera.launch">
+        <arg name="unite_imu_method" value="copy"/>
+        <arg name="enable_gyro"      value="true"/>
+        <arg name="enable_accel"     value="true"/>
+      </include>
+    </group>
+
+    <!-- IMU orientation estimation and publish tf accordingly to os_sensor frame -->
+    <node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
+      <remap from="imu/data_raw" to="$(arg imu_topic)"/>
+      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
+    </node>
+    <node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
+      <param name="use_mag" value="false"/>
+      <param name="world_frame" value="enu"/>
+      <param name="publish_tf" value="false"/>
+    </node>
+    <node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
+      <remap from="imu/data" to="$(arg imu_topic)/filtered"/>
+      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
+      <param name="base_frame_id" value="$(arg frame_id)"/>
+    </node>
+
+    <!-- Lidar Deskewing -->
+    <node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
+      <param name="wait_for_transform" value="0.01"/>
+      <param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
+      <param name="slerp" value="$(arg slerp)"/>
+      <remap from="input_cloud" to="$(arg scan_topic)"/>
+    </node>
+
+    <arg     if="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)/deskewed"/>
+    <arg unless="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)"/>
+
+    <group ns="rtabmap">
+      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
+        <remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
+        <remap from="imu"        to="$(arg imu_topic)/filtered"/>
+        <param name="guess_frame_id"  type="string" value="$(arg frame_id)_stabilized"/>
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id"   type="string" value="odom"/>
+        <param name="queue_size"      type="int"    value="$(arg queue_size_odom)"/>
+        <param name="wait_imu_to_init" type="bool" value="true"/>
+        <param name="wait_for_transform_duration" type="double" value="0.2"/>
+        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
+        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/PointToPlane"        type="string" value="true"/>
+        <param name="Icp/Iterations"          type="string" value="$(arg iterations)"/>
+        <param     if="$(arg floam)" name="Icp/VoxelSize" type="string" value="0"/>
+        <param unless="$(arg floam)" name="Icp/VoxelSize" type="string" value="$(arg resolution)"/>
+        <param name="Icp/DownsamplingStep"    type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
+        <param name="Icp/Epsilon"             type="string" value="0.001"/>
+        <param     if="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="0"/>
+        <param unless="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
+        <param name="Icp/MaxTranslation"      type="string" value="2"/>
+        <param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
+        <param name="Icp/PM"                  type="string" value="true"/>
+        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
+        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
+        <param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp"  type="string" value="0.8"/>
+
+        <!-- Odom parameters -->
+        <param name="Odom/ScanKeyFrameThr"       type="string" value="$(arg key_frame_thr)"/>
+        <param     if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
+        <param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
+        <param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
+        <param name="OdomF2M/ScanMaxSize"        type="string" value="$(arg local_map_size)"/>
+        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/>
+        <param name="OdomLOAM/Resolution"        type="string" value="$(arg resolution)"/>
+        <param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
+        <param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
+        <param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
+      </node>
+
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
+        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
+        <param name="subscribe_depth"      type="bool" value="false"/>
+        <param name="subscribe_rgb"        type="bool" value="true"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync"          type="bool" value="true"/>
+        <param name="wait_for_transform_duration" type="double" value="0.2"/>
+
+        <remap from="scan_cloud" to="assembled_cloud"/>
+        <remap from="rgb/image" to="/camera/color/image_raw"/>
+        <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
+        <remap from="imu"        to="$(arg imu_topic)/filtered"/>
+
+        <!-- RTAB-Map's parameters -->
+        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>
+        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
+        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
+        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
+        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
+        <param name="RGBD/AngularUpdate"             type="string" value="0.05"/>
+        <param name="RGBD/LinearUpdate"              type="string" value="0.05"/>
+        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
+        <param name="Mem/STMSize"                    type="string" value="30"/>
+        <param name="Mem/LaserScanNormalK"           type="string" value="20"/>
+
+        <param name="Reg/Strategy"                   type="string" value="1"/>
+        <param name="Grid/CellSize"                  type="string" value="$(arg resolution)"/>
+        <param name="Grid/RangeMax"                  type="string" value="$(arg grid_max_range)"/>
+        <param name="Grid/ClusterRadius"             type="string" value="1"/>
+        <param name="Grid/GroundIsObstacle"          type="string" value="$(arg ground_is_obstacle)"/>
+        <param name="Optimizer/GravitySigma"         type="string" value="0.3"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/VoxelSize"                  type="string" value="$(arg resolution)"/>
+        <param name="Icp/PointToPlaneK"              type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
+        <param name="Icp/PointToPlane"               type="string" value="true"/>
+        <param name="Icp/Iterations"                 type="string" value="$(arg iterations)"/>
+        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
+        <param name="Icp/MaxTranslation"             type="string" value="3"/>
+        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="$(eval resolution*10)"/>
+        <param name="Icp/PM"                         type="string" value="true"/>
+        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
+        <param name="Icp/CorrespondenceRatio"        type="string" value="$(arg loop_ratio)"/>
+      </node>
+
+      <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
+        <param name="frame_id" type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id" type="string" value="odom"/>
+        <param name="subscribe_odom_info" type="bool" value="true"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync" type="bool" value="false"/>
+        <remap from="scan_cloud" to="odom_filtered_input_scan"/>
+        <remap from="odom_info" to="odom_info"/>
+      </node>
+
+      <node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
+        <remap from="cloud"           to="$(arg scan_topic_deskewed)"/>
+        <remap from="odom"            to="odom"/>
+        <param     if="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="20" />
+        <param unless="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="10" />
+        <param name="fixed_frame_id"  type="string" value="" />
+        <param name="queue_size"      type="int"    value="$(arg queue_size)" />
+      </node>
+  </group>
+
+</launch>
@@ -0,0 +1,185 @@
+<?xml version="1.0"?>
+<!-- -->
+<launch>
+
+    <!--
+    Hand-held 3D lidar mapping example using only a Velodyne PUCK and external odometry (using t265 as example).
+    Prerequisities: rtabmap should be built with libpointmatcher
+    We use T265 only for lidar deskewing and icp_odometry guess in this example.
+    Example:
+     $ roslaunch rtabmap_ros test_velodyne_t265_deskewing.launch
+     $ rosrun rviz rviz -f map
+     $ Show TF and /rtabmap/cloud_map topics
+    -->
+
+    <arg name="rtabmapviz"    default="true"/>
+    <arg name="scan_20_hz"    default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
+    <arg name="deskewing"     default="true"/>
+    <arg name="slerp"         default="false"/> <!-- If true, a slerp between the first and last time will be used to deskew each point, which is faster than using tf for every point but less accurate -->
+    <arg name="scan_topic"    default="/velodyne_points"/>
+    <arg name="use_sim_time"  default="false"/>
+    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
+
+    <arg name="odom_frame_id"   default="t265_odom_frame"/> <!-- input odometry: here we use T265 odometry, but it could be wheel odometry -->
+    <arg name="frame_id"        default="velodyne"/>       <!-- base frame of the robot: for this example, we use velodyne as base frame -->
+    <arg name="queue_size"      default="10"/>
+    <arg name="queue_size_odom" default="1"/>
+    <arg name="loop_ratio"      default="0.2"/>
+
+    <arg name="resolution"         default="0.05"/> <!-- set 0.05-0.3 for indoor, set 0.3-0.5 for outdoor -->
+    <arg name="iterations"         default="10"/>
+
+    <!-- Grid parameters -->
+    <arg name="ground_is_obstacle" default="true"/>
+    <arg name="grid_max_range"     default="20"/>
+
+    <!-- For F2M Odometry -->
+    <arg name="ground_normals_up" default="false"/> <!-- set to true when velodyne is always horizontal to ground (ground robot, car) -->
+    <arg name="local_map_size"    default="15000"/>
+    <arg name="key_frame_thr"     default="0.6"/>
+
+    <!-- For FLOAM Odometry -->
+    <arg name="floam"    default="false"/> <!-- RTAB-Map should be built with FLOAM http://official-rtab-map-forum.206.s1.nabble.com/icp-odometry-with-LOAM-crash-tp8261p8563.html -->
+    <arg name="floam_sensor" default="0"/> <!-- 0=16 rings (VLP16), 1=32 rings, 2=64 rings -->
+
+    <!-- Static transform between velodyne and T265: TODO: Adjust with real position/orientation!!! -->
+    <node unless="$(arg use_sim_time)" pkg="tf" type="static_transform_publisher" name="T265_to_velodyne_tf" args="-0.01 0 0.055 0 0 0 t265_link velodyne 100"/>
+
+    <!-- Velodyne sensor VLP16 -->
+    <include unless="$(arg use_sim_time)" file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
+       <arg     if="$(arg scan_20_hz)" name="rpm" value="1200"/>
+       <arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
+       <arg name="organize_cloud" value="true"/> <!-- should be organized for deskewing -->
+    </include>
+
+    <!-- T265 -->
+    <group unless="$(arg use_sim_time)" ns="t265">
+      <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
+        <arg name="device_type"           value="t265"/>
+        <arg name="serial_no"             value=""/>
+        <arg name="tf_prefix"             value="t265"/>
+        <arg name="initial_reset"         value="false"/>
+        <arg name="enable_fisheye1"       value="false"/>
+        <arg name="enable_fisheye2"       value="false"/>
+        <arg name="topic_odom_in"         value=""/>
+        <arg name="calib_odom_file"       value=""/>
+        <arg name="enable_pose"           value="true"/>
+      </include>
+    </group>
+
+    <!-- Lidar Deskewing -->
+    <node if="$(arg deskewing)" pkg="nodelet" type="nodelet" name="lidar_deskewing" args="standalone rtabmap_ros/lidar_deskewing" output="screen">
+      <param name="wait_for_transform" value="0.01"/>
+      <param name="fixed_frame_id" value="$(arg odom_frame_id)"/>
+      <param name="slerp" value="$(arg slerp)"/>
+      <remap from="input_cloud" to="$(arg scan_topic)"/>
+    </node>
+
+    <arg     if="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)/deskewed"/>
+    <arg unless="$(arg deskewing)" name="scan_topic_deskewed" default="$(arg scan_topic)"/>
+
+    <group ns="rtabmap">
+      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
+        <remap from="scan_cloud" to="$(arg scan_topic_deskewed)"/>
+        <param name="guess_frame_id"  type="string" value="$(arg odom_frame_id)"/>
+        <param name="frame_id"        type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id"   type="string" value="odom"/>
+        <param name="queue_size"      type="int"    value="$(arg queue_size_odom)"/>
+        <param name="wait_for_transform_duration" type="double" value="0.2"/>
+        <param     if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
+        <param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/PointToPlane"        type="string" value="true"/>
+        <param name="Icp/Iterations"          type="string" value="$(arg iterations)"/>
+        <param     if="$(arg floam)" name="Icp/VoxelSize" type="string" value="0"/>
+        <param unless="$(arg floam)" name="Icp/VoxelSize" type="string" value="$(arg resolution)"/>
+        <param name="Icp/DownsamplingStep"    type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
+        <param name="Icp/Epsilon"             type="string" value="0.001"/>
+        <param     if="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="0"/>
+        <param unless="$(arg floam)" name="Icp/PointToPlaneK" type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"  type="string" value="0"/>
+        <param name="Icp/MaxTranslation"      type="string" value="2"/>
+        <param name="Icp/MaxCorrespondenceDistance" type="string" value="$(eval resolution*10)"/>
+        <param name="Icp/PM"                  type="string" value="true"/>
+        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
+        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
+        <param if="$(arg ground_normals_up)" name="Icp/PointToPlaneGroundNormalsUp"  type="string" value="0.8"/>
+
+        <!-- Odom parameters -->
+        <param name="Odom/ScanKeyFrameThr"       type="string" value="$(arg key_frame_thr)"/>
+        <param     if="$(arg floam)" name="Odom/Strategy" type="string" value="11"/>
+        <param unless="$(arg floam)" name="Odom/Strategy" type="string" value="0"/>
+        <param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg resolution)"/>
+        <param name="OdomF2M/ScanMaxSize"        type="string" value="$(arg local_map_size)"/>
+        <param name="OdomLOAM/Sensor"            type="string" value="$(arg floam_sensor)"/>
+        <param name="OdomLOAM/Resolution"        type="string" value="$(arg resolution)"/>
+        <param if="$(eval not deskewing and scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.05"/>
+        <param if="$(eval not deskewing and not scan_20_hz)" name="OdomLOAM/ScanPeriod" type="string" value="0.1"/>
+        <param if="$(arg deskewing)" name="OdomLOAM/ScanPeriod" type="string" value="0"/>
+      </node>
+
+      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
+        <param name="frame_id"             type="string" value="$(arg frame_id)"/>
+        <param name="subscribe_depth"      type="bool" value="false"/>
+        <param name="subscribe_rgb"        type="bool" value="false"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync"          type="bool" value="false"/>
+        <param name="wait_for_transform_duration" type="double" value="0.2"/>
+
+        <remap from="scan_cloud" to="assembled_cloud"/>
+
+        <!-- RTAB-Map's parameters -->
+        <param name="Rtabmap/DetectionRate"          type="string" value="1"/>
+        <param name="RGBD/NeighborLinkRefining"      type="string" value="false"/>
+        <param name="RGBD/ProximityBySpace"          type="string" value="true"/>
+        <param name="RGBD/ProximityMaxGraphDepth"    type="string" value="0"/>
+        <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
+        <param name="RGBD/AngularUpdate"             type="string" value="0.05"/>
+        <param name="RGBD/LinearUpdate"              type="string" value="0.05"/>
+        <param name="Mem/NotLinkedNodesKept"         type="string" value="false"/>
+        <param name="Mem/STMSize"                    type="string" value="30"/>
+        <param name="Mem/LaserScanNormalK"           type="string" value="20"/>
+
+        <param name="Reg/Strategy"                   type="string" value="1"/>
+        <param name="Grid/CellSize"                  type="string" value="$(arg resolution)"/>
+        <param name="Grid/RangeMax"                  type="string" value="$(arg grid_max_range)"/>
+        <param name="Grid/ClusterRadius"             type="string" value="1"/>
+        <param name="Grid/GroundIsObstacle"          type="string" value="$(arg ground_is_obstacle)"/>
+        <param name="Optimizer/GravitySigma"         type="string" value="0.3"/>
+
+        <!-- ICP parameters -->
+        <param name="Icp/VoxelSize"                  type="string" value="$(arg resolution)"/>
+        <param name="Icp/PointToPlaneK"              type="string" value="20"/>
+        <param name="Icp/PointToPlaneRadius"         type="string" value="0"/>
+        <param name="Icp/PointToPlane"               type="string" value="true"/>
+        <param name="Icp/Iterations"                 type="string" value="$(arg iterations)"/>
+        <param name="Icp/Epsilon"                    type="string" value="0.001"/>
+        <param name="Icp/MaxTranslation"             type="string" value="3"/>
+        <param name="Icp/MaxCorrespondenceDistance"  type="string" value="$(eval resolution*10)"/>
+        <param name="Icp/PM"                         type="string" value="true"/>
+        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
+        <param name="Icp/CorrespondenceRatio"        type="string" value="$(arg loop_ratio)"/>
+      </node>
+
+      <node if="$(arg rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
+        <param name="frame_id" type="string" value="$(arg frame_id)"/>
+        <param name="odom_frame_id" type="string" value="odom"/>
+        <param name="subscribe_odom_info" type="bool" value="true"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+        <param name="approx_sync" type="bool" value="false"/>
+        <remap from="scan_cloud" to="odom_filtered_input_scan"/>
+        <remap from="odom_info" to="odom_info"/>
+      </node>
+
+      <node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
+        <remap from="cloud"           to="$(arg scan_topic_deskewed)"/>
+        <remap from="odom"            to="odom"/>
+        <param     if="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="20" />
+        <param unless="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="10" />
+        <param name="fixed_frame_id"  type="string" value="" />
+        <param name="queue_size"      type="int"    value="$(arg queue_size)" />
+      </node>
+  </group>
+
+</launch>
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>rtabmap_examples</name>
+  <version>0.1.0</version>
+  <description>RTAB-Map's example launch files.</description>
+  <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
+  <author>Mathieu Labbe</author>
+  <license>BSD</license>
+  <url type="bugtracker">https://github.com/introlab/rtabmap_ros/issues</url>
+  <url type="repository">https://github.com/introlab/rtabmap_ros</url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <exec_depend>rtabmap_costmap_plugins</exec_depend>
+  <exec_depend>rtabmap_msgs</exec_depend>
+  <exec_depend>rtabmap_ros</exec_depend>
+  <exec_depend>rtabmap_rviz_plugins</exec_depend>
+  <exec_depend>rtabmap_viz</exec_depend>
+
+</package>
@@ -0,0 +1,244 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 the Universite de Sherbrooke 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 HOLDER 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.
+*/
+
+#include <ros/ros.h>
+#include <ros/publisher.h>
+#include <ros/subscriber.h>
+
+#include <rtabmap_msgs/MapData.h>
+#include <rtabmap_msgs/Info.h>
+#include <rtabmap_ros/MsgConversion.h>
+#include <rtabmap_msgs/AddLink.h>
+#include <rtabmap_msgs/GetMap.h>
+
+#include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/core/VWDictionary.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/RegistrationVis.h>
+#include <rtabmap/utilite/UStl.h>
+
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+
+/*
+ * Test:
+ * $ roslaunch rtabmap_ros demo_robot_mapping.launch
+ * Disable internal loop closure detection, in rtabmapviz->Preferences:
+ *    ->Vocabulary, set Max words to -1 (loop closure detection disabled)
+ *    ->Proximity Detection, uncheck proximity detection by space
+ * $ rosrun rtabmap_ros external_loop_detection_example
+ * $ rosbag play --clock demo_mapping.bag
+ */
+
+typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::MapData, rtabmap_msgs::Info> MyInfoMapSyncPolicy;
+
+ros::ServiceClient addLinkSrv;
+ros::ServiceClient getMapSrv;
+
+// This is used to keep in cache the old data of the map
+std::map<int, rtabmap::SensorData> localData;
+
+// In this example, we use rtabmap for our loop closure detector
+rtabmap::Rtabmap loopClosureDetector;
+
+rtabmap::RegistrationVis reg;
+
+bool g_localizationMode = false;
+
+void mapDataCallback(const rtabmap_msgs::MapDataConstPtr & mapDataMsg, const rtabmap_msgs::InfoConstPtr & infoMsg)
+{
+	ROS_INFO("Received map data!");
+
+	rtabmap::Statistics stats;
+	rtabmap_ros::infoFromROS(*infoMsg, stats);
+
+	bool smallMovement = (bool)uValue(stats.data(), rtabmap::Statistics::kMemorySmall_movement(), 0.0f);
+	bool fastMovement = (bool)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
+
+	if(smallMovement || fastMovement)
+	{
+		// The signature has been ignored from rtabmap, don't process it
+		return;
+	}
+
+	rtabmap::Transform mapToOdom;
+	std::map<int, rtabmap::Transform> poses;
+	std::multimap<int, rtabmap::Link> links;
+	std::map<int, rtabmap::Signature> signatures;
+	rtabmap_ros::mapDataFromROS(*mapDataMsg, poses, links, signatures, mapToOdom);
+
+	if(!signatures.empty() &&
+		signatures.rbegin()->second.sensorData().isValid() &&
+		localData.find(signatures.rbegin()->first) == localData.end())
+	{
+		int id = signatures.rbegin()->first;
+		const rtabmap::SensorData & s =  signatures.rbegin()->second.sensorData();
+		cv::Mat rgb;
+		//rtabmap::LaserScan scan;
+		s.uncompressDataConst(&rgb, 0/*, &scan*/);
+		//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(scan, scan.localTransform());
+
+		if(loopClosureDetector.process(rgb, id))
+		{
+			if(loopClosureDetector.getLoopClosureId()>0)
+			{
+				int fromId = id;
+				int toId = loopClosureDetector.getLoopClosureId();
+				ROS_INFO("Detected loop closure between %d and %d", fromId, toId);
+				if(localData.find(toId) != localData.end())
+				{
+					//Compute transformation
+					rtabmap::RegistrationInfo regInfo;
+					rtabmap::SensorData tmpFrom = s;
+					rtabmap::SensorData tmpTo = localData.at(toId);
+					tmpFrom.uncompressData();
+					tmpTo.uncompressData();
+					rtabmap::Transform t = reg.computeTransformation(tmpFrom, tmpTo, rtabmap::Transform(), &regInfo);
+
+					if(!t.isNull())
+					{
+						rtabmap::Link link(fromId, toId, rtabmap::Link::kUserClosure, t, regInfo.covariance.inv());
+						rtabmap_msgs::AddLinkRequest req;
+						rtabmap_ros::linkToROS(link, req.link);
+						rtabmap_msgs::AddLinkResponse res;
+						if(!addLinkSrv.call(req, res))
+						{
+							ROS_ERROR("Failed to call %s service", addLinkSrv.getService().c_str());
+						}
+					}
+					else
+					{
+						ROS_WARN("Could not compute transformation between %d and %d: %s", fromId, toId, regInfo.rejectedMsg.c_str());
+					}
+				}
+				else
+				{
+					ROS_WARN("Could not compute transformation between %d and %d because node data %d is not in cache.", fromId, toId, toId);
+				}
+			}
+			if(!g_localizationMode)
+			{
+				localData.insert(std::make_pair(id, s));
+			}
+		}
+	}
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "external_loop_detection_example");
+
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+
+	ros::NodeHandle nh;
+	ros::NodeHandle pnh("~");
+
+	rtabmap::ParametersMap params;
+	params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemGenerateIds(), "false")); // use provided ids
+	params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDEnabled(), "false")); // BOW-only mode
+	loopClosureDetector.init(params, "");
+
+	pnh.param("localization", g_localizationMode, g_localizationMode);
+
+	// service to add link
+	addLinkSrv = nh.serviceClient<rtabmap_msgs::AddLink>("/rtabmap/add_link");
+
+	// service to get map for initialization
+	getMapSrv = nh.serviceClient<rtabmap_msgs::GetMap>("/rtabmap/get_map_data");
+
+	rtabmap_msgs::GetMap::Request mapReq;
+	mapReq.global = true;
+	mapReq.graphOnly = false;
+	mapReq.optimized = false;
+	rtabmap_msgs::GetMap::Response mapRes;
+	ros::Rate rate(1);
+	while(!getMapSrv.exists() && nh.ok())
+	{
+		ROS_INFO("Waiting for service \"%s\" to be available. If rtabmap is already started, "
+				"make sure the service name is the same or remap it.", getMapSrv.getService().c_str());
+		rate.sleep();
+		ros::spinOnce();
+	}
+	if(!nh.ok())
+	{
+		return 0;
+	}
+	ROS_INFO("Calling \"%s\" service to get data already in the map.", getMapSrv.getService().c_str());
+	if(getMapSrv.call(mapReq, mapRes) && !mapRes.data.nodes.empty())
+	{
+		ROS_INFO("Adding %d nodes to memory...", (int)mapRes.data.nodes.size());
+		std::map<int, rtabmap::Transform> poses;
+		std::multimap<int, rtabmap::Link> links;
+		rtabmap::Transform mapToOdom;
+		rtabmap_ros::mapGraphFromROS(mapRes.data.graph, poses, links, mapToOdom);
+		int addedNodes = 0;
+		for(size_t i=0; i<mapRes.data.nodes.size(); ++i)
+		{
+			rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(mapRes.data.nodes.at(i));
+			rtabmap::SensorData compressedData = s.sensorData();
+			s.sensorData().uncompressData();
+			if(loopClosureDetector.process(s.sensorData(), rtabmap::Transform()))
+			{
+				localData.insert(std::make_pair(
+						loopClosureDetector.getStatistics().getLastSignatureData().id(),
+						compressedData));
+				++addedNodes;
+			}
+		}
+		ROS_INFO("Added %d/%d nodes to memory! Vocabulary size=%d",
+				addedNodes,
+				(int)mapRes.data.nodes.size(),
+				(int)loopClosureDetector.getMemory()->getVWDictionary()->getVisualWords().size());
+
+	}
+
+	if(g_localizationMode)
+	{
+		params.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false")); // localization mode
+		loopClosureDetector.parseParameters(params);
+		ROS_INFO("Set detector in localization mode");
+	}
+
+	// subscription
+	message_filters::Subscriber<rtabmap_msgs::Info> infoTopic;
+	message_filters::Subscriber<rtabmap_msgs::MapData> mapDataTopic;
+	infoTopic.subscribe(nh, "/rtabmap/info", 1);
+	mapDataTopic.subscribe(nh, "/rtabmap/mapData", 1);
+	message_filters::Synchronizer<MyInfoMapSyncPolicy> infoMapSync(
+			MyInfoMapSyncPolicy(10),
+			mapDataTopic,
+			infoTopic);
+	infoMapSync.registerCallback(&mapDataCallback);
+	ROS_INFO("Subscribed to %s and %s", mapDataTopic.getTopic().c_str(), infoTopic.getTopic().c_str());
+
+	ros::spin();
+
+	return 0;
+}