i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'devel' of github.com:introlab/rtabmap_ros

Browse Source
This commit is contained in:
matlabbe
2016-07-17 22:34:49 -04:00
parent 951229ba67 7268d75ae1
commit 446345447f
50 changed files with 2193 additions and 1262 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+22 -17
View File
@@ -18,7 +18,7 @@ find_package(rviz)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.11.7 REQUIRED)
find_package(RTABMap 0.11.8 REQUIRED)
find_package(OpenCV REQUIRED)
@@ -149,6 +149,8 @@ SET(Libraries
)
SET(rtabmap_ros_lib_src
src/nodelets/rgbd_odometry.cpp
src/nodelets/stereo_odometry.cpp
src/nodelets/data_throttle.cpp
src/nodelets/stereo_throttle.cpp
src/nodelets/data_odom_sync.cpp
@@ -157,8 +159,8 @@ SET(rtabmap_ros_lib_src
src/nodelets/disparity_to_depth.cpp
src/nodelets/obstacles_detection.cpp
src/nodelets/point_cloud_aggregator.cpp
src/nodelets/OdometryROS.cpp
src/MsgConversion.cpp
src/OdometryROS.cpp
src/MapsManager.cpp
)
@@ -176,6 +178,19 @@ SET(rtabmap_ros_lib_src
)
ENDIF(costmap_2d_FOUND)
# If octomap is found, add definition
IF(octomap_ros_FOUND)
MESSAGE(STATUS "WITH octomap")
include_directories(
${octomap_ros_INCLUDE_DIRS}
)
SET(Libraries
${octomap_ros_LIBRARIES}
${Libraries}
)
ADD_DEFINITIONS("-DWITH_OCTOMAP_ROS")
ENDIF(octomap_ros_FOUND)
IF(QT4_FOUND OR Qt5_FOUND)
SET(Libraries
${Libraries}
@@ -244,27 +259,14 @@ IF(Qt5_FOUND)
ENDIF(Qt5_FOUND)
add_dependencies(rtabmap_ros ${${PROJECT_NAME}_EXPORTED_TARGETS})
# If octomap is found, add definition
IF(octomap_ros_FOUND)
MESSAGE(STATUS "WITH octomap")
include_directories(
${octomap_ros_INCLUDE_DIRS}
)
SET(Libraries
${octomap_ros_LIBRARIES}
${Libraries}
)
add_definitions(-DWITH_OCTOMAP)
ENDIF(octomap_ros_FOUND)
add_executable(rtabmap src/CoreNode.cpp src/CoreWrapper.cpp)
target_link_libraries(rtabmap rtabmap_ros ${Libraries})
add_executable(rgbd_odometry src/RGBDOdometryNode.cpp)
target_link_libraries(rgbd_odometry rtabmap_ros ${Libraries})
target_link_libraries(rgbd_odometry ${Libraries})
add_executable(stereo_odometry src/StereoOdometryNode.cpp)
target_link_libraries(stereo_odometry rtabmap_ros ${Libraries})
target_link_libraries(stereo_odometry ${Libraries})
add_executable(map_optimizer src/MapOptimizerNode.cpp)
target_link_libraries(map_optimizer rtabmap_ros ${Libraries})
@@ -276,6 +278,9 @@ add_executable(camera src/CameraNode.cpp)
add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS})
target_link_libraries(camera ${Libraries})
add_executable(stereo_camera src/StereoCameraNode.cpp)
target_link_libraries(stereo_camera rtabmap_ros ${Libraries})
IF(RTABMAP_GUI)
add_executable(rtabmapviz src/GuiNode.cpp src/GuiWrapper.cpp src/PreferencesDialogROS.cpp)
target_link_libraries(rtabmapviz rtabmap_ros ${QT_LIBRARIES} ${Libraries})
LICENSE
+1 -1
View File
@@ -1,4 +1,4 @@
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
include/rtabmap_ros/MsgConversion.h
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
launch/config/rgbd_gui.ini
+120 -32
View File
@@ -6,17 +6,12 @@ General\loggerPauseLevel=3
General\loggerType=1
General\loggerPrintTime=true
General\verticalLayoutUsed=true
General\imageFlipped=false
General\imageRejectedShown=true
General\imageHighestHypShown=true
General\beep=false
General\keypointsOpacity=16
General\voxelSize=0
General\decimation=16
MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\x80\0\0\x2\x92\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\0\0\0\0(\0\0\x2\x92\0\0\0\xe0\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x2\x92\xfc\x2\0\0\0\x2\xfc\0\0\0(\0\0\x2\x92\0\0\x1\x46\0\xff\xff\xff\xfc\x1\0\0\0\x2\xfc\0\0\0\0\0\0\x1\\\0\0\0q\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\x1\0\0\0(\0\0\x1\x1b\0\0\0g\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\x1I\0\0\x1q\0\0\0\xd9\0\xff\xff\xff\xfc\0\0\x1\x62\0\0\x3\x9e\0\0\0\xc8\0\xff\xff\xff\xfa\0\0\0\x1\x2\0\0\0\x2\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\0\0\0\0\0\xff\xff\xff\xff\0\0\0g\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\0(\0\0\x2\x92\0\0\0\xe1\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\xf7\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\x87\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\x87\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\0g\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\x87\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\0`\0\xff\xff\xff\0\0\0\0\0\0\x2\x92\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\x1\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)"
MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\x1V\0\0\0\xa1\0\0\x6\x65\0\0\x3\x94\0\0\x1^\0\0\0\xbd\0\0\x6]\0\0\x3\x8c\0\0\0\0\0\0)
MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\x80\0\0\x2\x92\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\0\0\0\0(\0\0\x2\x92\0\0\0\xe0\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x2\xa8\xfc\x2\0\0\0\x2\xfc\0\0\0(\0\0\x2\xa8\0\0\0\xe1\0\xff\xff\xff\xfc\x1\0\0\0\x2\xfc\0\0\0\0\0\0\x1\\\0\0\0O\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\x1\0\0\0(\0\0\x1%\0\0\0\x19\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\x1S\0\0\x1}\0\0\0=\0\xff\xff\xff\xfc\0\0\x1\x62\0\0\x3\x9e\0\0\0\xc8\0\xff\xff\xff\xfa\0\0\0\x1\x2\0\0\0\x2\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\0\0\0\0\0\xff\xff\xff\xff\0\0\0g\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\0(\0\0\x2\x92\0\0\0\xe1\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\xf7\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\x87\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\x87\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\x87\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\0m\0\xff\xff\xff\0\0\0\0\0\0\x2\xa8\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\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\xc4\0\0\0\xc4\0\0\x5\xd7\0\0\x3\xc3\0\0\0\xce\0\0\0\xea\0\0\x5\xcd\0\0\x3\xb9\0\0\0\0\0\0)
General\showClouds0=true
General\voxelSize0=0
General\decimation0=4
General\maxDepth0=4
General\showScans0=true
@@ -25,7 +20,6 @@ General\ptSize0=1
General\opacityScan0=1
General\ptSizeScan0=1
General\showClouds1=true
General\voxelSize1=0
General\decimation1=4
General\maxDepth1=0
General\showScans1=true
@@ -33,45 +27,25 @@ General\opacity1=1
General\ptSize1=2
General\opacityScan1=1
General\ptSizeScan1=1
General\showClouds2=true
General\voxelSize2=0.01
General\decimation2=1
General\maxDepth2=4
General\showScans2=true
General\meshing0=false
General\cloudFiltering=true
General\cloudFiltering=false
General\cloudFilteringRadius=0.2
General\cloudFilteringAngle=20
General\meshNormalKSearch0=20
General\meshGP3Radius0=0.04
General\meshSmoothing0=false
General\meshSmoothingRadius0=0.04
General\meshNormalKSearch1=20
General\meshGP3Radius1=0.04
General\meshSmoothing1=true
General\meshSmoothingRadius1=0.04
General\odomQualityThr=50
General\meshing=false
General\meshGP3Radius=0.04
General\meshNormalKSearch=20
General\meshSmoothing=false
General\meshSmoothingRadius=0.04
General\gridMapShown=false
General\gridMapResolution=0.05
General\gridMapFillEmptySpace=true
General\gridMapOccupancyFrom3DCloud=false
General\gridMapFillEmptyRadius=0
General\gridMapOpacity=0.75
General\posteriorGraphView=true
General\showGraphs=true
PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\x1\xde\0\0\0n\0\0\x5\xe8\0\0\x3\x82\0\0\x1\xde\0\0\0n\0\0\x5\xe8\0\0\x3\x82\0\0\0\0\0\0)
PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\x1\xde\0\0\0R\0\0\x5\xe8\0\0\x3\x66\0\0\x1\xde\0\0\0R\0\0\x5\xe8\0\0\x3\x66\0\0\0\0\0\0)
AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\0\0\0\0\x18\0\0\x2_\0\0\x1\xf2\0\0\0\0\0\0\0\x18\0\0\x2_\0\0\x1\xf2\0\0\0\0\0\0)
widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xbf\xf0\0\x13\0\0\0\0\xbe\xb6\0\0\0\0\0\0>\xa4\0\0\0\0\0\0)
widget_cloudViewer\camera_focal=@Variant(\0\0\0T>\xa4\0\0\0\0\0\0\xbe\xa1\0\0\0\0\0\0>t\0\0\0\0\0\0)
widget_cloudViewer\camera_focal=@Variant(\0\0\0T>\xa4\0\0\0\0\0\0\xbe\xa1\0\0\0\0\0\0>\xa4\0\0\0\0\0\0)
widget_cloudViewer\camera_up=@Variant(\0\0\0T\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0?\xf0\0\0\0\0\0\0)
widget_cloudViewer\grid=false
widget_cloudViewer\grid_cell_count=50
widget_cloudViewer\grid_cell_size=@Variant(\0\0\0\x87?\x80\0\0)
widget_cloudViewer\grid_cell_size=1
widget_cloudViewer\trajectory_shown=true
widget_cloudViewer\trajectory_size=100
widget_cloudViewer\camera_target_locked=false
@@ -110,3 +84,117 @@ PostProcessingDialog\iterations=1
PostProcessingDialog\reextract_features=false
PostProcessingDialog\refine_neigbors=false
PostProcessingDialog\refine_lc=false
MainWindow\maximized=false
MainWindow\status_bar=false
widget_cloudViewer\frustum_shown=false
widget_cloudViewer\frustum_scale=@Variant(\0\0\0\x87?\0\0\0)
widget_cloudViewer\frustum_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0)
widget_cloudViewer\rendering_rate=5
imageView_source\alpha=50
imageView_source\graphics_view=false
imageView_source\graphics_view_scale=true
imageView_loopClosure\alpha=50
imageView_loopClosure\graphics_view=false
imageView_loopClosure\graphics_view_scale=true
imageView_odometry\alpha=200
imageView_odometry\graphics_view=false
imageView_odometry\graphics_view_scale=true
ExportCloudsDialog\pipeline=0
ExportCloudsDialog\normals_k=10
ExportCloudsDialog\regenerate_min_depth=0
ExportCloudsDialog\filtering=false
ExportCloudsDialog\filtering_radius=0.02
ExportCloudsDialog\filtering_min_neighbors=2
ExportCloudsDialog\subtract=false
ExportCloudsDialog\subtract_point_radius=0.02
ExportCloudsDialog\subtract_point_angle=0
ExportCloudsDialog\subtract_min_neighbors=5
ExportCloudsDialog\mls_polygonial_order=2
ExportCloudsDialog\mls_upsampling_method=0
ExportCloudsDialog\mls_upsampling_radius=0.01
ExportCloudsDialog\mls_upsampling_step=0
ExportCloudsDialog\mls_point_density=0
ExportCloudsDialog\mls_dilation_voxel_size=0.01
ExportCloudsDialog\mls_dilation_iterations=0
ExportCloudsDialog\mesh_mu=2.5
ExportCloudsDialog\mesh_decimation_factor=0
ExportCloudsDialog\mesh_texture=false
ExportCloudsDialog\mesh_angle_tolerance=15
ExportCloudsDialog\mesh_quad=false
ExportCloudsDialog\mesh_triangle_size=2
ExportScansDialog\binary=true
ExportScansDialog\normals_k=20
ExportScansDialog\regenerate=false
ExportScansDialog\regenerate_decimation=1
ExportScansDialog\filtering=false
ExportScansDialog\filtering_radius=0.02
ExportScansDialog\filtering_min_neighbors=2
ExportScansDialog\assemble=true
ExportScansDialog\assemble_voxel=0.01
PostProcessingDialog\sba=false
PostProcessingDialog\sba_iterations=20
PostProcessingDialog\sba_epsilon=0
PostProcessingDialog\sba_type=1
PostProcessingDialog\sba_variance=1
graphicsView_graphView\node_radius=0.00999999977648258
graphicsView_graphView\link_width=0
graphicsView_graphView\node_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0)
graphicsView_graphView\current_goal_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0)
graphicsView_graphView\neighbor_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\xff\xff\0\0)
graphicsView_graphView\global_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
graphicsView_graphView\local_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xff\xff\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)
graphicsView_graphView\neighbor_merged_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\xaa\xaa\0\0\0\0)
graphicsView_graphView\rejected_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\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\global_path_color=@Variant(\0\0\0\x43\x1\xff\xff\x80\x80\0\0\x80\x80\0\0)
graphicsView_graphView\gt_color=@Variant(\0\0\0\x43\x1\xff\xff\xa0\xa0\xa0\xa0\xa4\xa4\0\0)
graphicsView_graphView\intra_session_color=@Variant(\0\0\0\x43\x1\xff\xff\xff\xff\0\0\0\0\0\0)
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\grid_visible=true
graphicsView_graphView\origin_visible=true
graphicsView_graphView\referential_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\graph_visible=true
graphicsView_graphView\global_path_visible=true
graphicsView_graphView\local_path_visible=true
graphicsView_graphView\gt_graph_visible=true
General\cloudsKept=true
General\loggerPrintThreadId=false
General\notifyNewGlobalPath=false
General\minDepth0=0
General\showFeatures0=false
General\downsamplingScan0=1
General\voxelSizeScan0=0
General\ptSizeFeatures0=3
General\minDepth1=0
General\showFeatures1=true
General\downsamplingScan1=1
General\voxelSizeScan1=0
General\ptSizeFeatures1=3
General\cloudVoxel=0
General\cloudNoiseRadius=0
General\cloudNoiseMinNeighbors=5
General\showLabels=false
General\noFiltering=true
General\subtractFiltering=false
General\subtractFilteringMinPts=5
General\subtractFilteringRadius=0.02
General\subtractFilteringAngle=0
General\normalKSearch=10
General\gridMapEroded=false
General\projMapFrame=true
General\projMaxGroundAngle=30
General\projMaxGroundHeight=0
General\projMinClusterSize=20
General\projMaxObstaclesHeight=0
General\projFlatObstaclesDetected=true
General\meshing_angle=15
General\meshing_quad=false
General\meshing_triangle_size=2
Figures\counts=
Figures\curves=
launch/demo/demo_stereo_outdoor.launch
+1
View File
@@ -55,6 +55,7 @@
<param name="Vis/MaxDepth" type="string" value="10"/>
<param name="Vis/MinInliers" type="string" value="10"/>
<param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
<param name="OdomF2M/MaxSize" type="string" value="1000"/>
<param name="GFTT/MinDistance" type="string" value="10"/>
<param name="GFTT/QualityLevel" type="string" value="0.00001"/>
</node>
launch/rgbd_mapping.launch
+28 -95
View File
@@ -1,5 +1,7 @@
<launch>
<!-- Backward compatibility launch file, use rtabmap.launch instead -->
<!-- Your RGB-D sensor should be already started with "depth_registration:=true".
Examples:
$ roslaunch freenect_launch freenect.launch depth_registration:=true
@@ -17,17 +19,15 @@
<arg name="rviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
<arg name="frame_id" default="camera_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="time_threshold" default="0"/> <!-- (ms) If not 0 ms, memory management is used to keep processing time on this fixed limit. -->
<arg name="optimize_from_last_node" default="false"/> <!-- Optimize the map from the last node. Should be true on multi-session mapping and when time threshold is set -->
<arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
<arg name="approx_sync" default="true"/> <!-- if timestamps of the input topics are not synchronized -->
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="depth_registered_topic" default="/camera/depth_registered/image_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<arg name="compressed" default="false"/>
<arg name="convert_depth_to_mm" default="true"/>
<arg name="subscribe_scan" default="false"/> <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
<arg name="scan_topic" default="/scan"/>
@@ -41,102 +41,35 @@
<arg name="namespace" default="rtabmap"/>
<arg name="wait_for_transform" default="0.2"/>
<!-- Nodes -->
<group ns="$(arg namespace)">
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="rtabmapviz" value="$(arg rtabmapviz)" />
<arg name="rviz" value="$(arg rviz)" />
<arg name="localization" value="$(arg localization)"/>
<arg name="gui_cfg" value="$(arg rtabmapviz_cfg)" />
<arg name="rviz_cfg" value="$(arg rviz_cfg)" />
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="compressed in:=$(arg rgb_topic) raw out:=$(arg rgb_topic)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_registered_topic) raw out:=$(arg depth_registered_topic)" />
<arg name="frame_id" value="$(arg frame_id)"/>
<arg name="namespace" value="$(arg namespace)"/>
<arg name="database_path" value="$(arg database_path)"/>
<arg name="wait_for_transform" value="$(arg wait_for_transform)"/>
<arg name="rtabmap_args" value="$(arg rtabmap_args)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="approx_sync" value="$(arg approx_sync)"/>
<!-- Odometry -->
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<arg name="rgb_topic" value="$(arg rgb_topic)" />
<arg name="depth_topic" value="$(arg depth_registered_topic)" />
<arg name="camera_info_topic" value="$(arg camera_info_topic)" />
<arg name="compressed" value="$(arg compressed)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<arg name="subscribe_scan" value="$(arg subscribe_scan)"/>
<arg name="scan_topic" value="$(arg scan_topic)"/>
<param name="Odom/FillInfoData" type="string" value="true"/>
</node>
<arg name="subscribe_scan_cloud" value="$(arg subscribe_scan_cloud)"/>
<arg name="scan_cloud_topic" value="$(arg scan_cloud_topic)"/>
<!-- Visual SLAM (robot side) -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
<param name="Rtabmap/TimeThr" type="string" value="$(arg time_threshold)"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/>
<param name="Mem/SaveDepth16Format" type="string" value="$(arg convert_depth_to_mm)"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
<!-- when 2D scan is set -->
<param if="$(arg subscribe_scan)" name="Optimizer/Slam2D" type="string" value="true"/>
<param if="$(arg subscribe_scan)" name="Icp/CorrespondenceRatio" type="string" value="0.25"/>
<param if="$(arg subscribe_scan)" name="Reg/Strategy" type="string" value="1"/>
<param if="$(arg subscribe_scan)" name="Reg/Force3DoF" type="string" value="true"/>
<!-- when 3D scan is set -->
<param if="$(arg subscribe_scan_cloud)" name="Reg/Strategy" type="string" value="1"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="$(arg rgb_topic)"/>
<remap from="depth/image_in" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info_in" to="$(arg camera_info_topic)"/>
<remap if="$(arg visual_odometry)" from="odom_in" to="rtabmap/odom"/>
<remap unless="$(arg visual_odometry)" from="odom_in" to="$(arg odom_topic)"/>
<remap from="rgb/image_out" to="data_odom_sync/image"/>
<remap from="depth/image_out" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
<remap from="odom_out" to="odom_sync"/>
</node>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="data_odom_sync/image"/>
<remap from="depth/image" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="decimation" type="double" value="2"/>
<param name="voxel_size" type="double" value="0.02"/>
</node>
<arg name="visual_odometry" value="$(arg visual_odometry)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="odom_args" value="$(arg rtabmap_args)"/>
</include>
</launch>
launch/rtabmap.launch
+11 -9
View File
@@ -2,7 +2,7 @@
<launch>
<!-- Convenience launch file to launch odometry, rtabmap and rtabmapviz nodes at once -->
<!-- For rgbd:=true
<!-- For stereo:=false
Your RGB-D sensor should be already started with "depth_registration:=true".
Examples:
$ roslaunch freenect_launch freenect.launch depth_registration:=true
@@ -15,7 +15,6 @@
$ roslaunch rtabmap_ros bumblebee.launch -->
<!-- Choose between RGB-D and stereo -->
<arg name="rgbd" default="true"/>
<arg name="stereo" default="false"/>
<!-- Choose visualization -->
@@ -37,6 +36,7 @@
<arg name="wait_for_transform" default="0.2"/>
<arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
<arg name="approx_sync" default="true"/> <!-- if timestamps of the input topics are not synchronized -->
<!-- RGB-D related topics -->
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
@@ -49,7 +49,6 @@
<arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" /> <!-- using grayscale image for efficiency -->
<arg name="left_camera_info_topic" default="$(arg stereo_namespace)/left/camera_info" />
<arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
<arg name="approx_sync" default="false"/> <!-- if timestamps of the stereo images are not synchronized -->
<arg name="compressed" default="false"/> <!-- If you want to subscribe to compressed image topics -->
<!-- For depth_topic, "compressedDepth" image_transport is used. -->
@@ -80,7 +79,7 @@
<group ns="$(arg namespace)">
<!-- RGB-D Odometry -->
<group if="$(arg rgbd)">
<group unless="$(arg stereo)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="compressedDepth in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
@@ -91,6 +90,7 @@
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
</node>
@@ -101,7 +101,7 @@
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="compressed in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
@@ -118,14 +118,15 @@
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="$(arg rgbd)"/>
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="stereo_approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
@@ -150,7 +151,8 @@
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="$(arg rgbd)"/>
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
@@ -189,7 +191,7 @@
<param name="decimation" type="double" value="2"/>
<param name="voxel_size" type="double" value="0.02"/>
<param if="$(arg stereo)" name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
</node>
</launch>
launch/stereo_mapping.launch
+33 -92
View File
@@ -1,5 +1,7 @@
<launch>
<!-- Backward compatibility launch file, use "rtabmap.launch rgbd:=false stereo:=true" instead -->
<!-- Your camera should be calibrated and publishing rectified left and right
images + corresponding camera_info msgs. You can use stereo_image_proc for image rectification.
Example:
@@ -17,20 +19,17 @@
<arg name="rviz_cfg" default="-d $(find rtabmap_ros)/launch/config/rgbd.rviz" />
<arg name="frame_id" default="base_link"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="time_threshold" default="0"/> <!-- (ms) If not 0 ms, memory management is used to keep processing time on this fixed limit. -->
<arg name="optimize_from_last_node" default="false"/> <!-- Optimize the map from the last node. Should be true on multi-session mapping and when time threshold is set -->
<arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="rtabmap_args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="launch_prefix" default=""/>
<arg name="approx_sync" default="false"/> <!-- if timestamps of the input topics are not synchronized -->
<arg name="stereo_namespace" default="/stereo_camera"/>
<arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect_color" />
<arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" /> <!-- using grayscale image for efficiency -->
<arg name="left_camera_info_topic" default="$(arg stereo_namespace)/left/camera_info" />
<arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
<arg name="approximate_sync" default="false"/> <!-- if timestamps of the stereo images are not synchronized -->
<arg name="compressed" default="false"/>
<arg name="convert_depth_to_mm" default="true"/>
<arg name="subscribe_scan" default="false"/> <!-- Assuming 2D scan if set, rtabmap will do 3DoF mapping instead of 6DoF -->
<arg name="scan_topic" default="/scan"/>
@@ -44,98 +43,40 @@
<arg name="namespace" default="rtabmap"/>
<arg name="wait_for_transform" default="0.2"/>
<!-- Nodes -->
<group ns="$(arg namespace)">
<include file="$(find rtabmap_ros)/launch/rtabmap.launch">
<arg name="rgbd" value="false"/>
<arg name="stereo" value="true"/>
<arg name="rtabmapviz" value="$(arg rtabmapviz)" />
<arg name="rviz" value="$(arg rviz)" />
<arg name="localization" value="$(arg localization)"/>
<arg name="gui_cfg" value="$(arg rtabmapviz_cfg)" />
<arg name="rviz_cfg" value="$(arg rviz_cfg)" />
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="compressed in:=$(arg left_image_topic) raw out:=$(arg left_image_topic)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic)" />
<arg name="frame_id" value="$(arg frame_id)"/>
<arg name="namespace" value="$(arg namespace)"/>
<arg name="database_path" value="$(arg database_path)"/>
<arg name="wait_for_transform" value="$(arg wait_for_transform)"/>
<arg name="rtabmap_args" value="$(arg rtabmap_args)"/>
<arg name="launch_prefix" value="$(arg launch_prefix)"/>
<arg name="approx_sync" value="$(arg approx_sync)"/>
<!-- Odometry -->
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<remap from="left/image_rect" to="$(arg left_image_topic)"/>
<remap from="right/image_rect" to="$(arg right_image_topic)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<arg name="stereo_namespace" value="$(arg stereo_namespace)"/>
<arg name="left_image_topic" value="$(arg left_image_topic)" />
<arg name="right_image_topic" value="$(arg right_image_topic)" />
<arg name="left_camera_info_topic" value="$(arg left_camera_info_topic)" />
<arg name="right_camera_info_topic" value="$(arg right_camera_info_topic)" />
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approximate_sync)"/>
<arg name="compressed" value="$(arg compressed)"/>
<param name="Odom/FillInfoData" type="string" value="true"/>
</node>
<arg name="subscribe_scan" value="$(arg subscribe_scan)"/>
<arg name="scan_topic" value="$(arg scan_topic)"/>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="stereo_approx_sync" type="bool" value="$(arg approximate_sync)"/>
<arg name="subscribe_scan_cloud" value="$(arg subscribe_scan_cloud)"/>
<arg name="scan_cloud_topic" value="$(arg scan_cloud_topic)"/>
<remap from="left/image_rect" to="$(arg left_image_topic)"/>
<remap from="right/image_rect" to="$(arg right_image_topic)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
<param name="Rtabmap/TimeThr" type="string" value="$(arg time_threshold)"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="$(arg optimize_from_last_node)"/>
<param name="Mem/SaveDepth16Format" type="string" value="$(arg convert_depth_to_mm)"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
<!-- when 2D scan is set -->
<param if="$(arg subscribe_scan)" name="Optimizer/Slam2D" type="string" value="true"/>
<param if="$(arg subscribe_scan)" name="Icp/CorrespondenceRatio" type="string" value="0.25"/>
<param if="$(arg subscribe_scan)" name="Reg/Strategy" type="string" value="1"/>
<param if="$(arg subscribe_scan)" name="Reg/Force3DoF" type="string" value="true"/>
<!-- when 3D scan is set -->
<param if="$(arg subscribe_scan_cloud)" name="Reg/Strategy" type="string" value="1"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="$(arg rtabmapviz_cfg)" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_stereo" type="bool" value="true"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<remap from="left/image_rect" to="$(arg left_image_topic)"/>
<remap from="right/image_rect" to="$(arg right_image_topic)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="$(arg rviz_cfg)"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="left/image" to="$(arg left_image_topic)"/>
<remap from="right/image" to="$(arg right_image_topic)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="cloud" to="voxel_cloud" />
<param name="decimation" type="double" value="2"/>
<param name="voxel_size" type="double" value="0.02"/>
<param name="approx_sync" type="bool" value="$(arg approximate_sync)"/>
</node>
<arg name="visual_odometry" value="$(arg visual_odometry)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="odom_args" value="$(arg rtabmap_args)"/>
</include>
</launch>
launch/tests/sensor_fusion.launch
+145
View File
@@ -0,0 +1,145 @@
<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>
launch/tests/sensor_fusion_kinect_brick.launch
+37
View File
@@ -0,0 +1,37 @@
<launch>
<arg name="localization" default="false"/>
<!-- 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="6xDEo7"/>
<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>
nodelet_plugins.xml
+17
View File
@@ -1,4 +1,21 @@
<library path="lib/librtabmap_ros">
<class name="rtabmap_ros/rgbd_odometry"
type="rtabmap_ros::RGBDOdometry"
base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
</description>
</class>
<class name="rtabmap_ros/stereo_odometry"
type="rtabmap_ros::StereoOdometry"
base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
</description>
</class>
<class name="rtabmap_ros/data_throttle"
type="rtabmap_ros::DataThrottleNodelet"
base_class_type="nodelet::Nodelet">
package.xml
+1 -3
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap_ros</name>
<version>0.11.7</version>
<version>0.11.8</version>
<description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
@@ -39,7 +39,6 @@
<build_depend>move_base_msgs</build_depend>
<build_depend>costmap_2d</build_depend>
<build_depend>octomap_ros</build_depend>
<build_depend>octomap</build_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>roscpp</run_depend>
@@ -69,7 +68,6 @@
<run_depend>move_base_msgs</run_depend>
<run_depend>costmap_2d</run_depend>
<run_depend>octomap_ros</run_depend>
<run_depend>octomap</run_depend>
<build_depend>libpcl-all-dev</build_depend>
src/CameraNode.cpp
+2 -2
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -207,7 +207,7 @@ public:
ROS_ERROR("Path \"%s\" does not exist (or you don't have the permissions to read)... falling back to usb device...", path.c_str());
}
//usb device
camera_ = new rtabmap::CameraVideo(deviceId, frameRate);
camera_ = new rtabmap::CameraVideo(deviceId, false, frameRate);
}
cameraThread_ = new rtabmap::CameraThread(camera_);
init();
src/CoreNode.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/CoreWrapper.cpp
+137 -62
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -51,13 +51,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d_surface.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/OdometryEvent.h>
#include <rtabmap/core/Version.h>
#include <pcl_conversions/pcl_conversions.h>
#include <laser_geometry/laser_geometry.h>
#ifdef WITH_OCTOMAP
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
#include <octomap_msgs/conversions.h>
#include <rtabmap/core/OctoMap.h>
#endif
#endif
#define BAD_COVARIANCE 9999
@@ -98,14 +102,20 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
mapToOdom_(rtabmap::Transform::getIdentity()),
mapsManager_(true),
depthSync_(0),
depthExactSync_(0),
depthScanSync_(0),
depthScan3dSync_(0),
stereoScanSync_(0),
stereoScan3dSync_(0),
stereoApproxSync_(0),
stereoExactSync_(0),
depth2Sync_(0),
depthTFSync_(0),
depthTFExactSync_(0),
depthScanTFSync_(0),
depthScan3dTFSync_(0),
stereoScanTFSync_(0),
stereoScan3dTFSync_(0),
stereoApproxTFSync_(0),
stereoExactTFSync_(0),
transformThread_(0),
@@ -126,8 +136,9 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
int queueSize = 10;
bool publishTf = true;
double tfDelay = 0.05; // 20 Hz
double tfTolerance = 0.1; // 100 ms
std::string tfPrefix = "";
bool stereoApproxSync = false;
bool approxSync = true;
// ROS related parameters (private)
pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
@@ -166,11 +177,22 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
pnh.param("depth_cameras", depthCameras, depthCameras);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("stereo_approx_sync", stereoApproxSync, stereoApproxSync);
if(pnh.hasParam("stereo_approx_sync") && !pnh.hasParam("approx_sync"))
{
ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
"to \"approx_sync\"! Your value is still copied to "
"corresponding parameter.");
pnh.param("stereo_approx_sync", approxSync, approxSync);
}
else
{
pnh.param("approx_sync", approxSync, approxSync);
}
pnh.param("publish_tf", publishTf, publishTf);
pnh.param("tf_delay", tfDelay, tfDelay);
pnh.param("tf_prefix", tfPrefix, tfPrefix);
pnh.param("tf_tolerance", tfTolerance, tfTolerance);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_);
pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
@@ -219,7 +241,9 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
ROS_INFO("rtabmap: queue_size = %d", queueSize);
ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
ROS_INFO("rtabmap: tf_tolerance = %f", tfTolerance);
ROS_INFO("rtabmap: depth_cameras = %d", depthCameras);
ROS_INFO("rtabmap: approx_sync = %s", approxSync?"true":"false");
infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
@@ -408,9 +432,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
cancelGoalSrv_ = nh.advertiseService("cancel_goal", &CoreWrapper::cancelGoalCallback, this);
setLabelSrv_ = nh.advertiseService("set_label", &CoreWrapper::setLabelCallback, this);
listLabelsSrv_ = nh.advertiseService("list_labels", &CoreWrapper::listLabelsCallback, this);
#ifdef WITH_OCTOMAP
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
octomapBinarySrv_ = nh.advertiseService("octomap_binary", &CoreWrapper::octomapBinaryCallback, this);
octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this);
#endif
#endif
//private services
setLogDebugSrv_ = pnh.advertiseService("log_debug", &CoreWrapper::setLogDebug, this);
@@ -418,13 +444,13 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart, const ParametersMap & parameters)
setLogWarnSrv_ = pnh.advertiseService("log_warning", &CoreWrapper::setLogWarn, this);
setLogErrorSrv_ = pnh.advertiseService("log_error", &CoreWrapper::setLogError, this);
setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, stereoApproxSync, depthCameras);
setupCallbacks(subscribeDepth, subscribeScan2d, subscribeScan3d, subscribeStereo, queueSize, approxSync, depthCameras);
int optimizeIterations = 0;
Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations);
if(publishTf && optimizeIterations != 0)
{
transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay));
transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay, tfTolerance));
}
else if(publishTf)
{
@@ -443,10 +469,16 @@ CoreWrapper::~CoreWrapper()
if(depthSync_)
delete depthSync_;
if(depthExactSync_)
delete depthExactSync_;
if(depthScanSync_)
delete depthScanSync_;
if(depthScan3dSync_)
delete depthScan3dSync_;
if(stereoScanSync_)
delete stereoScanSync_;
if(stereoScan3dSync_)
delete stereoScan3dSync_;
if(stereoApproxSync_)
delete stereoApproxSync_;
if(stereoExactSync_)
@@ -455,10 +487,16 @@ CoreWrapper::~CoreWrapper()
delete depth2Sync_;
if(depthTFSync_)
delete depthTFSync_;
if(depthTFExactSync_)
delete depthTFExactSync_;
if(depthScanTFSync_)
delete depthScanTFSync_;
if(depthScan3dTFSync_)
delete depthScan3dTFSync_;
if(stereoScanTFSync_)
delete stereoScanTFSync_;
if(stereoScan3dTFSync_)
delete stereoScan3dTFSync_;
if(stereoApproxTFSync_)
delete stereoApproxTFSync_;
if(stereoExactTFSync_)
@@ -527,7 +565,7 @@ void CoreWrapper::saveParameters(const std::string & configFile)
}
}
void CoreWrapper::publishLoop(double tfDelay)
void CoreWrapper::publishLoop(double tfDelay, double tfTolerance)
{
if(tfDelay == 0)
return;
@@ -537,7 +575,7 @@ void CoreWrapper::publishLoop(double tfDelay)
if(!odomFrameId_.empty())
{
mapToOdomMutex_.lock();
ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfDelay);
ros::Time tfExpiration = ros::Time::now() + ros::Duration(tfTolerance);
geometry_msgs::TransformStamped msg;
msg.child_frame_id = odomFrameId_;
msg.header.frame_id = mapFrameId_;
@@ -817,8 +855,19 @@ void CoreWrapper::commonDepthCallback(
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight);
UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight);
UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
imageMsgs[i]->width,
imageHeight,
imageMsgs[i]->height).c_str());
UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
depthMsgs[i]->width,
imageHeight,
depthMsgs[i]->height).c_str());
Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp);
if(localTransform.isNull())
@@ -993,7 +1042,9 @@ void CoreWrapper::commonDepthCallback(
if(scanCloudNormalK_ > 0)
{
//compute normals
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal = util3d::computeNormals(pclScan, scanCloudNormalK_);
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
}
else
@@ -1156,7 +1207,9 @@ void CoreWrapper::commonStereoCallback(
if(scanCloudNormalK_ > 0)
{
//compute normals
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal = util3d::computeNormals(pclScan, scanCloudNormalK_);
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanCloudNormalK_);
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScanFromPointCloud(*pclScanNormal);
}
else
@@ -1440,6 +1493,8 @@ void CoreWrapper::process(
if(rtabmap_.isIDsGenerated() || data.id() > 0)
{
double timeRtabmap = 0.0;
double timeUpdateMaps = 0.0;
double timePublishMaps = 0.0;
if(rtabmap_.process(data, odom, OdometryEvent::generateCovarianceMatrix(odomRotationalVariance, odomTransitionalVariance)))
{
timeRtabmap = timer.ticks();
@@ -1473,8 +1528,11 @@ void CoreWrapper::process(
false,
false,
false,
false,
tmpSignature);
timeUpdateMaps = timer.ticks();
mapsManager_.publishMaps(filteredPoses, stamp, mapFrameId_);
// update goal if planning is enabled
@@ -1545,17 +1603,20 @@ void CoreWrapper::process(
}
}
}
timePublishMaps = timer.ticks();
}
}
else
{
timeRtabmap = timer.ticks();
}
ROS_INFO("rtabmap: Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Pub=%.4fs (local map=%d, WM=%d)",
ROS_INFO("rtabmap: Rate=%.2fs, Limit=%.3fs, RTAB-Map=%.4fs, Maps update=%.4fs pub=%.4fs (local map=%d, WM=%d)",
rate_>0?1.0f/rate_:0,
rtabmap_.getTimeThreshold()/1000.0f,
timeRtabmap,
timer.ticks(),
timeUpdateMaps,
timePublishMaps,
(int)rtabmap_.getLocalOptimizedPoses().size(),
rtabmap_.getWMSize()+rtabmap_.getSTMSize());
}
@@ -1934,6 +1995,7 @@ bool CoreWrapper::getProjMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::
false,
true,
false,
false,
false);
if(filteredPoses.size())
{
@@ -1978,6 +2040,7 @@ bool CoreWrapper::getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::
false,
false,
true,
false,
false);
if(filteredPoses.size())
{
@@ -2091,6 +2154,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
false,
false,
false,
false,
signatures);
}
else
@@ -2545,7 +2609,8 @@ void CoreWrapper::publishGlobalPath(const ros::Time & stamp)
}
}
#ifdef WITH_OCTOMAP
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
bool CoreWrapper::octomapBinaryCallback(
octomap_msgs::GetOctomap::Request &req,
octomap_msgs::GetOctomap::Response &res)
@@ -2555,14 +2620,10 @@ bool CoreWrapper::octomapBinaryCallback(
res.map.header.stamp = ros::Time::now();
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false, false);
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true);
octomap::OcTree * octree = mapsManager_.createOctomap(poses);
bool success = octree != 0 && octree->size() && octomap_msgs::binaryMapToMsg(*octree, res.map);
if(octree)
{
delete octree;
}
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
bool success = octomap->octree()->size() && octomap_msgs::binaryMapToMsg(*octomap->octree(), res.map);
return success;
}
@@ -2575,17 +2636,14 @@ bool CoreWrapper::octomapFullCallback(
res.map.header.stamp = ros::Time::now();
std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), true, false, false, false);
poses = mapsManager_.updateMapCaches(poses, rtabmap_.getMemory(), false, false, false, false, true);
octomap::OcTree * octree = mapsManager_.createOctomap(poses);
bool success = octree != 0 && octree->size() && octomap_msgs::fullMapToMsg(*octree, res.map);
if(octree)
{
delete octree;
}
const rtabmap::OctoMap * octomap = mapsManager_.getOctomap();
bool success = octomap->octree()->size() && octomap_msgs::fullMapToMsg(*octomap->octree(), res.map);
return success;
}
#endif
#endif
/**
* exclusive callbacks:
@@ -2604,7 +2662,7 @@ void CoreWrapper::setupCallbacks(
bool subscribeScan3d,
bool subscribeStereo,
int queueSize,
bool stereoApproxSync,
bool approxSync,
int depthCameras)
{
ros::NodeHandle nh; // public
@@ -2649,7 +2707,6 @@ void CoreWrapper::setupCallbacks(
odomSub_.subscribe(nh, "odom", 1);
if(subscribeScan2d)
{
ROS_INFO("Registering Depth+LaserScan callback...");
scanSub_.subscribe(nh, "scan", 1);
depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
MyDepthScanSyncPolicy(queueSize),
@@ -2670,7 +2727,6 @@ void CoreWrapper::setupCallbacks(
}
else if(subscribeScan3d)
{
ROS_INFO("Registering Depth+LaserScan3d callback...");
scan3dSub_.subscribe(nh, "scan_cloud", 1);
depthScan3dSync_ = new message_filters::Synchronizer<MyDepthScan3dSyncPolicy>(
MyDepthScan3dSyncPolicy(queueSize),
@@ -2693,7 +2749,6 @@ void CoreWrapper::setupCallbacks(
{
if(depthCameras > 1)
{
ROS_INFO("Registering Depth2 callback...");
depth2Sync_ = new message_filters::Synchronizer<MyDepth2SyncPolicy>(
MyDepth2SyncPolicy(queueSize),
odomSub_,
@@ -2717,17 +2772,29 @@ void CoreWrapper::setupCallbacks(
}
else
{
ROS_INFO("Registering Depth callback...");
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(
MyDepthSyncPolicy(queueSize),
*imageSubs_[0],
odomSub_,
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
if(approxSync)
{
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(
MyDepthSyncPolicy(queueSize),
*imageSubs_[0],
odomSub_,
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
}
else
{
depthExactSync_ = new message_filters::Synchronizer<MyDepthExactSyncPolicy>(
MyDepthExactSyncPolicy(queueSize),
*imageSubs_[0],
odomSub_,
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthExactSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
}
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str(),
@@ -2776,15 +2843,27 @@ void CoreWrapper::setupCallbacks(
}
else //!subscribeLaserScan
{
depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
MyDepthTFSyncPolicy(queueSize),
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s",
if(approxSync)
{
depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
MyDepthTFSyncPolicy(queueSize),
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
}
else
{
depthTFExactSync_ = new message_filters::Synchronizer<MyDepthTFExactSyncPolicy>(
MyDepthTFExactSyncPolicy(queueSize),
*imageSubs_[0],
*imageDepthSubs_[0],
*cameraInfoSubs_[0]);
depthTFExactSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
}
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
imageSubs_[0]->getTopic().c_str(),
imageDepthSubs_[0]->getTopic().c_str(),
cameraInfoSubs_[0]->getTopic().c_str());
@@ -2856,9 +2935,8 @@ void CoreWrapper::setupCallbacks(
}
else //!subscribeLaserScan
{
if(stereoApproxSync)
if(approxSync)
{
ROS_INFO("Registering Stereo Approx callback...");
stereoApproxSync_ = new message_filters::Synchronizer<MyStereoApproxSyncPolicy>(
MyStereoApproxSyncPolicy(queueSize),
imageRectLeft_,
@@ -2870,7 +2948,6 @@ void CoreWrapper::setupCallbacks(
}
else
{
ROS_INFO("Registering Stereo Exact callback...");
stereoExactSync_ = new message_filters::Synchronizer<MyStereoExactSyncPolicy>(
MyStereoExactSyncPolicy(queueSize),
imageRectLeft_,
@@ -2881,8 +2958,9 @@ void CoreWrapper::setupCallbacks(
stereoExactSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
}
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s",
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
@@ -2895,7 +2973,6 @@ void CoreWrapper::setupCallbacks(
// use odom from TF, so subscribe to sensors only
if(subscribeScan2d)
{
ROS_INFO("Registering Stereo+LaserScan2d+OdomTF callback...");
scanSub_.subscribe(nh, "scan", 1);
stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
MyStereoScanTFSyncPolicy(queueSize),
@@ -2916,7 +2993,6 @@ void CoreWrapper::setupCallbacks(
}
else if(subscribeScan3d)
{
ROS_INFO("Registering Stereo+LaserScan3d+OdomTF callback...");
scan3dSub_.subscribe(nh, "scan_cloud", 1);
stereoScan3dTFSync_ = new message_filters::Synchronizer<MyStereoScan3dTFSyncPolicy>(
MyStereoScan3dTFSyncPolicy(queueSize),
@@ -2937,9 +3013,8 @@ void CoreWrapper::setupCallbacks(
}
else //!subscribeLaserScan
{
if(stereoApproxSync)
if(approxSync)
{
ROS_INFO("Registering Stereo+OdomTF Approx callback...");
stereoApproxTFSync_ = new message_filters::Synchronizer<MyStereoApproxTFSyncPolicy>(
MyStereoApproxTFSyncPolicy(queueSize),
imageRectLeft_,
@@ -2950,7 +3025,6 @@ void CoreWrapper::setupCallbacks(
}
else
{
ROS_INFO("Registering Stereo+OdomTF Exact callback...");
stereoExactTFSync_ = new message_filters::Synchronizer<MyStereoExactTFSyncPolicy>(
MyStereoExactTFSyncPolicy(queueSize),
imageRectLeft_,
@@ -2960,8 +3034,9 @@ void CoreWrapper::setupCallbacks(
stereoExactTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4));
}
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
src/CoreWrapper.h
+17 -6
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -65,7 +65,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#ifdef WITH_OCTOMAP
#ifdef WITH_OCTOMAP_ROS
#include <octomap_msgs/GetOctomap.h>
#endif
@@ -90,7 +90,7 @@ private:
bool subscribeScan3d,
bool subscribeStereo,
int queueSize,
bool stereoApproxSync,
bool approxSync,
int depthCameras);
void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
@@ -234,7 +234,7 @@ private:
bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res);
bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res);
#ifdef WITH_OCTOMAP
#ifdef WITH_OCTOMAP_ROS
bool octomapBinaryCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res);
bool octomapFullCallback(octomap_msgs::GetOctomap::Request &req, octomap_msgs::GetOctomap::Response &res);
#endif
@@ -242,7 +242,7 @@ private:
rtabmap::ParametersMap loadParameters(const std::string & configFile);
void saveParameters(const std::string & configFile);
void publishLoop(double tfDelay);
void publishLoop(double tfDelay, double tfTolerance);
void publishStats(const ros::Time & stamp);
void publishCurrentGoal(const ros::Time & stamp);
@@ -338,6 +338,12 @@ private:
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthSyncPolicy;
message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
typedef message_filters::sync_policies::ExactTime<
sensor_msgs::Image,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthExactSyncPolicy;
message_filters::Synchronizer<MyDepthExactSyncPolicy> * depthExactSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
@@ -403,6 +409,11 @@ private:
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
typedef message_filters::sync_policies::ExactTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthTFExactSyncPolicy;
message_filters::Synchronizer<MyDepthTFExactSyncPolicy> * depthTFExactSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
@@ -457,7 +468,7 @@ private:
ros::ServiceServer cancelGoalSrv_;
ros::ServiceServer setLabelSrv_;
ros::ServiceServer listLabelsSrv_;
#ifdef WITH_OCTOMAP
#ifdef WITH_OCTOMAP_ROS
ros::ServiceServer octomapBinarySrv_;
ros::ServiceServer octomapFullSrv_;
#endif
src/DbPlayerNode.cpp
+20 -7
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -41,8 +41,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_ros/SetGoal.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/DBReader.h>
#include <rtabmap/core/OdometryEvent.h>
#include <cmath>
bool paused = false;
@@ -118,19 +122,24 @@ int main(int argc, char** argv)
ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
ROS_INFO("scan_frame_id = %s", scanFrameId.c_str());
ROS_INFO("database = %s", databasePath.c_str());
ROS_INFO("rate = %f", rate);
ROS_INFO("publish_tf = %s", publishTf?"true":"false");
rtabmap::DBReader reader(databasePath, rate);
ROS_INFO("start_id = %d", startId);
if(databasePath.empty())
{
ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
return -1;
}
databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
if(databasePath.size() && databasePath.at(0) != '/')
{
databasePath = UDirectory::currentDir(true) + databasePath;
}
ROS_INFO("database = %s", databasePath.c_str());
if(!reader.init(startId))
rtabmap::DBReader reader(databasePath, rate, false, false, false, startId);
if(!reader.init())
{
ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());
return -1;
@@ -154,7 +163,9 @@ int main(int argc, char** argv)
tf2_ros::TransformBroadcaster tfBroadcaster;
UTimer timer;
rtabmap::OdometryEvent odom = reader.getNextData();
rtabmap::CameraInfo info;
rtabmap::SensorData data = reader.takeImage(&info);
rtabmap::OdometryEvent odom(data, info.odomPose, info.odomCovariance);
double acquisitionTime = timer.ticks();
while(ros::ok() && odom.data().id())
{
@@ -479,7 +490,9 @@ int main(int argc, char** argv)
}
timer.restart();
odom = reader.getNextData();
info = rtabmap::CameraInfo();
data = reader.takeImage(&info);
odom = rtabmap::OdometryEvent(data, info.odomPose, info.odomCovariance);
acquisitionTime = timer.ticks();
}
src/GuiNode.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/GuiWrapper.cpp
+204 -176
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -529,70 +529,74 @@ void GuiWrapper::commonDepthCallback(
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
UASSERT(imageMsgs.size()>0 &&
imageMsgs.size() == depthMsgs.size() &&
imageMsgs.size() == cameraInfoMsgs.size());
std_msgs::Header odomHeader;
if(odomMsg.get())
{
odomHeader = odomMsg->header;
}
else
{
if(scan2dMsg.get())
{
odomHeader = scan2dMsg->header;
}
else if(scan3dMsg.get())
{
odomHeader = scan3dMsg->header;
}
else if(cameraInfoMsgs.size() && cameraInfoMsgs[0].get())
{
odomHeader = cameraInfoMsgs[0]->header;
}
else if(depthMsgs.size() && depthMsgs[0].get())
{
odomHeader = depthMsgs[0]->header;
}
else if(imageMsgs.size() && imageMsgs[0].get())
{
odomHeader = imageMsgs[0]->header;
}
odomHeader.frame_id = odomFrameId_;
}
Transform odomT = getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get())
{
UASSERT(odomMsg->pose.covariance.size() == 36);
if(!(odomMsg->pose.covariance[0] == 0 &&
odomMsg->pose.covariance[7] == 0 &&
odomMsg->pose.covariance[14] == 0 &&
odomMsg->pose.covariance[21] == 0 &&
odomMsg->pose.covariance[28] == 0 &&
odomMsg->pose.covariance[35] == 0))
{
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone();
}
}
if(odomHeader.frame_id.empty())
{
ROS_ERROR("Odometry frame not set!?");
return;
}
cv::Mat rgb;
cv::Mat depth;
std::vector<CameraModel> cameraModels;
cv::Mat scan;
rtabmap::OdometryInfo info;
bool ignoreData = false;
if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
!mainWindow_->isProcessingOdometry() &&
!mainWindow_->isProcessingStatistics())
{
lastOdomInfoUpdateTime_ = UTimer::now();
UASSERT(imageMsgs.size()>0 &&
imageMsgs.size() == depthMsgs.size() &&
imageMsgs.size() == cameraInfoMsgs.size());
std_msgs::Header odomHeader;
if(odomMsg.get())
{
odomHeader = odomMsg->header;
}
else
{
if(scan2dMsg.get())
{
odomHeader = scan2dMsg->header;
}
else if(scan3dMsg.get())
{
odomHeader = scan3dMsg->header;
}
else if(cameraInfoMsgs.size() && cameraInfoMsgs[0].get())
{
odomHeader = cameraInfoMsgs[0]->header;
}
else if(depthMsgs.size() && depthMsgs[0].get())
{
odomHeader = depthMsgs[0]->header;
}
else if(imageMsgs.size() && imageMsgs[0].get())
{
odomHeader = imageMsgs[0]->header;
}
odomHeader.frame_id = odomFrameId_;
}
Transform odomT = getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get())
{
UASSERT(odomMsg->pose.covariance.size() == 36);
if(!(odomMsg->pose.covariance[0] == 0 &&
odomMsg->pose.covariance[7] == 0 &&
odomMsg->pose.covariance[14] == 0 &&
odomMsg->pose.covariance[21] == 0 &&
odomMsg->pose.covariance[28] == 0 &&
odomMsg->pose.covariance[35] == 0))
{
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone();
}
}
if(odomHeader.frame_id.empty())
{
ROS_ERROR("Odometry frame not set!?");
return;
}
cv::Mat rgb;
cv::Mat depth;
std::vector<CameraModel> cameraModels;
if(imageMsgs[0].get() && depthMsgs[0].get())
{
int imageWidth = imageMsgs[0]->width;
@@ -686,7 +690,6 @@ void GuiWrapper::commonDepthCallback(
}
}
cv::Mat scan;
if(scan2dMsg.get() != 0)
{
// make sure the frame of the laser is updated too
@@ -726,28 +729,38 @@ void GuiWrapper::commonDepthCallback(
scan = util3d::laserScanFromPointCloud(*pclScan);
}
rtabmap::OdometryInfo info;
if(odomInfoMsg.get())
{
info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
}
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
scan2dMsg.get()?(int)scan2dMsg->range_max:0,
rgb,
depth,
cameraModels,
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
covariance,
info);
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent));
ignoreData = false;
}
else if(odomInfoMsg.get())
{
info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData();
ignoreData = true;
}
else
{
// don't update GUI odom stuff if we don't use visual odometry
return;
}
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
scan2dMsg.get()?(int)scan2dMsg->range_max:0,
rgb,
depth,
cameraModels,
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
covariance,
info);
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
}
void GuiWrapper::commonStereoCallback(
@@ -760,6 +773,91 @@ void GuiWrapper::commonStereoCallback(
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
UASSERT(leftImageMsg.get() && rightImageMsg.get());
UASSERT(leftCamInfoMsg.get() && rightCamInfoMsg.get());
if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
return;
}
std_msgs::Header odomHeader;
if(odomMsg.get())
{
odomHeader = odomMsg->header;
}
else
{
if(scan2dMsg.get())
{
odomHeader = scan2dMsg->header;
}
else if(scan3dMsg.get())
{
odomHeader = scan3dMsg->header;
}
else
{
odomHeader = leftCamInfoMsg->header;
}
odomHeader.frame_id = odomFrameId_;
}
Transform odomT = getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get())
{
UASSERT(odomMsg->pose.covariance.size() == 36);
if(!(odomMsg->pose.covariance[0] == 0 &&
odomMsg->pose.covariance[7] == 0 &&
odomMsg->pose.covariance[14] == 0 &&
odomMsg->pose.covariance[21] == 0 &&
odomMsg->pose.covariance[28] == 0 &&
odomMsg->pose.covariance[35] == 0))
{
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone();
}
}
if(odomHeader.frame_id.empty())
{
ROS_ERROR("Odometry frame not set!?");
return;
}
Transform localTransform = getTransform(frameId_, leftCamInfoMsg->header.frame_id, leftCamInfoMsg->header.stamp);
if(localTransform.isNull())
{
return;
}
// sync with odometry stamp
if(odomHeader.stamp != leftCamInfoMsg->header.stamp)
{
if(!odomT.isNull())
{
Transform sensorT = getTransform(odomHeader.frame_id, frameId_, leftCamInfoMsg->header.stamp);
if(sensorT.isNull())
{
return;
}
localTransform = odomT.inverse() * sensorT * localTransform;
}
}
cv::Mat left;
cv::Mat right;
cv::Mat scan;
rtabmap::StereoCameraModel stereoModel;
rtabmap::OdometryInfo info;
bool ignoreData = false;
// limit 10 Hz max
if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
!mainWindow_->isProcessingOdometry() &&
@@ -767,85 +865,7 @@ void GuiWrapper::commonStereoCallback(
{
lastOdomInfoUpdateTime_ = UTimer::now();
UASSERT(leftImageMsg.get() && rightImageMsg.get());
UASSERT(leftCamInfoMsg.get() && rightCamInfoMsg.get());
if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
return;
}
std_msgs::Header odomHeader;
if(odomMsg.get())
{
odomHeader = odomMsg->header;
}
else
{
if(scan2dMsg.get())
{
odomHeader = scan2dMsg->header;
}
else if(scan3dMsg.get())
{
odomHeader = scan3dMsg->header;
}
else
{
odomHeader = leftCamInfoMsg->header;
}
odomHeader.frame_id = odomFrameId_;
}
Transform odomT = getTransform(odomHeader.frame_id, frameId_, odomHeader.stamp);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(odomMsg.get())
{
UASSERT(odomMsg->pose.covariance.size() == 36);
if(!(odomMsg->pose.covariance[0] == 0 &&
odomMsg->pose.covariance[7] == 0 &&
odomMsg->pose.covariance[14] == 0 &&
odomMsg->pose.covariance[21] == 0 &&
odomMsg->pose.covariance[28] == 0 &&
odomMsg->pose.covariance[35] == 0))
{
covariance = cv::Mat(6,6,CV_64FC1,(void*)odomMsg->pose.covariance.data()).clone();
}
}
if(odomHeader.frame_id.empty())
{
ROS_ERROR("Odometry frame not set!?");
return;
}
Transform localTransform = getTransform(frameId_, leftCamInfoMsg->header.frame_id, leftCamInfoMsg->header.stamp);
if(localTransform.isNull())
{
return;
}
// sync with odometry stamp
if(odomHeader.stamp != leftCamInfoMsg->header.stamp)
{
if(!odomT.isNull())
{
Transform sensorT = getTransform(odomHeader.frame_id, frameId_, leftCamInfoMsg->header.stamp);
if(sensorT.isNull())
{
return;
}
localTransform = odomT.inverse() * sensorT * localTransform;
}
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
if(stereoModel.baseline() > 10.0)
{
@@ -862,7 +882,6 @@ void GuiWrapper::commonStereoCallback(
// left
cv_bridge::CvImageConstPtr ptrImage;
cv::Mat left;
if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
@@ -874,9 +893,8 @@ void GuiWrapper::commonStereoCallback(
}
// right
cv::Mat right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image;
right = cv_bridge::toCvCopy(rightImageMsg, "mono8")->image;
cv::Mat scan;
if(scan2dMsg.get() != 0)
{
// make sure the frame of the laser is updated too
@@ -916,28 +934,38 @@ void GuiWrapper::commonStereoCallback(
scan = util3d::laserScanFromPointCloud(*pclScan);
}
rtabmap::OdometryInfo info;
if(odomInfoMsg.get())
{
info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
}
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
scan2dMsg.get()?(int)scan2dMsg->range_max:0,
left,
right,
stereoModel,
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
covariance,
info);
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent));
ignoreData = false;
}
else if(odomInfoMsg.get())
{
info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg).copyWithoutData();
ignoreData = true;
}
else
{
// don't update GUI odom stuff if we don't use visual odometry
return;
}
rtabmap::OdometryEvent odomEvent(
rtabmap::SensorData(
scan,
scan2dMsg.get()?(int)scan2dMsg->ranges.size():0,
scan2dMsg.get()?(int)scan2dMsg->range_max:0,
left,
right,
stereoModel,
odomHeader.seq,
rtabmap_ros::timestampFromROS(odomHeader.stamp)),
odomMsg.get()?rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose):odomT,
covariance,
info);
QMetaObject::invokeMethod(mainWindow_, "processOdometry", Q_ARG(rtabmap::OdometryEvent, odomEvent), Q_ARG(bool, ignoreData));
}
// With odom msg
src/GuiWrapper.h
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/MapAssemblerNode.cpp
+2 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -101,6 +101,7 @@ public:
false,
false,
false,
false,
nodes_);
mapsManager_.publishMaps(poses, msg->header.stamp, msg->header.frame_id);
src/MapOptimizerNode.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/MapsManager.cpp
+337 -72
View File
@@ -1,9 +1,29 @@
/*
* MapsManager.cpp
*
* Created on: 2015-05-14
* Author: mathieu
*/
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 "MapsManager.h"
@@ -17,14 +37,18 @@
#include <rtabmap/core/util2d.h>
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/Graph.h>
#include <rtabmap/core/Version.h>
#include <nav_msgs/OccupancyGrid.h>
#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#ifdef WITH_OCTOMAP
#include <octomap/octomap.h>
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
#include <octomap_msgs/conversions.h>
#include <rtabmap/core/OctoMap.h>
#endif
#endif
using namespace rtabmap;
@@ -48,6 +72,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
projMaxObstaclesHeight_(2.0), // meters (<=0 disabled)
projMaxGroundHeight_(0.0), // meters (<=0 disabled, only works if proj_detect_flat_obstacles is true)
projDetectFlatObstacles_(false),
projMapFrame_(false),
gridCellSize_(0.05), // meters
gridSize_(0), // meters
gridEroded_(false),
@@ -56,7 +81,10 @@ MapsManager::MapsManager(bool usePublicNamespace) :
mapFilterRadius_(0.0),
mapFilterAngle_(30.0), // degrees
mapCacheCleanup_(true),
negativePosesIgnored(false)
negativePosesIgnored_(false),
octomap_(0),
octomapTreeDepth_(16),
octomapGroundIsObstacle_(false)
{
ros::NodeHandle nh;
@@ -102,6 +130,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
}
pnh.param("proj_max_ground_height", projMaxGroundHeight_, projMaxGroundHeight_);
pnh.param("proj_detect_flat_obstacles", projDetectFlatObstacles_, projDetectFlatObstacles_);
pnh.param("proj_map_frame", projMapFrame_, projMapFrame_);
// common grid map stuff
pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m
@@ -118,7 +147,25 @@ MapsManager::MapsManager(bool usePublicNamespace) :
pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_);
pnh.param("map_negative_poses_ignored", negativePosesIgnored, negativePosesIgnored);
pnh.param("map_negative_poses_ignored", negativePosesIgnored_, negativePosesIgnored_);
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
octomap_ = new OctoMap(gridCellSize_);
pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_);
if(octomapTreeDepth_ > 16)
{
ROS_WARN("octomap_tree_depth maximum is 16");
octomapTreeDepth_ = 16;
}
else if(octomapTreeDepth_ < 0)
{
ROS_WARN("octomap_tree_depth cannot be negative, set to 16 instead");
octomapTreeDepth_ = 16;
}
pnh.param("octomap_ground_is_obstacle", octomapGroundIsObstacle_, octomapGroundIsObstacle_);
#endif
#endif
// If true, the last message published on
// the map topics will be saved and sent to new subscribers when they
@@ -133,6 +180,15 @@ MapsManager::MapsManager(bool usePublicNamespace) :
projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
scanMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
octoMapPubBin_ = nh.advertise<octomap_msgs::Octomap>("octomap_binary", 1, latch);
octoMapPubFull_ = nh.advertise<octomap_msgs::Octomap>("octomap_full", 1, latch);
octoMapCloud_ = nh.advertise<sensor_msgs::PointCloud2>("octomap_cloud", 1, latch);
octoMapEmptySpace_ = nh.advertise<sensor_msgs::PointCloud2>("octomap_empty_space", 1, latch);
octoMapProj_ = nh.advertise<nav_msgs::OccupancyGrid>("octomap_proj", 1, latch);
#endif
#endif
}
else
{
@@ -140,11 +196,30 @@ MapsManager::MapsManager(bool usePublicNamespace) :
projMapPub_ = pnh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
gridMapPub_ = pnh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
scanMapPub_ = pnh.advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
octoMapPubBin_ = pnh.advertise<octomap_msgs::Octomap>("octomap_binary", 1, latch);
octoMapPubFull_ = pnh.advertise<octomap_msgs::Octomap>("octomap_full", 1, latch);
octoMapCloud_ = pnh.advertise<sensor_msgs::PointCloud2>("octomap_cloud", 1, latch);
octoMapEmptySpace_ = pnh.advertise<sensor_msgs::PointCloud2>("octomap_cloud_ground", 1, latch);
octoMapProj_ = pnh.advertise<nav_msgs::OccupancyGrid>("octomap_proj", 1, latch);
#endif
#endif
}
}
MapsManager::~MapsManager() {
clear();
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
if(octomap_)
{
delete octomap_;
octomap_ = 0;
}
#endif
#endif
}
void MapsManager::clear()
@@ -153,6 +228,11 @@ void MapsManager::clear()
cameraModels_.clear();
projMaps_.clear();
gridMaps_.clear();
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
octomap_->clear();
#endif
#endif
}
bool MapsManager::hasSubscribers() const
@@ -160,7 +240,12 @@ bool MapsManager::hasSubscribers() const
return cloudMapPub_.getNumSubscribers() != 0 ||
projMapPub_.getNumSubscribers() != 0 ||
gridMapPub_.getNumSubscribers() != 0 ||
scanMapPub_.getNumSubscribers() != 0;
scanMapPub_.getNumSubscribers() != 0 ||
octoMapPubBin_.getNumSubscribers() != 0 ||
octoMapPubFull_.getNumSubscribers() != 0 ||
octoMapCloud_.getNumSubscribers() != 0 ||
octoMapEmptySpace_.getNumSubscribers() != 0 ||
octoMapProj_.getNumSubscribers() != 0;
}
std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Transform> & poses)
@@ -181,6 +266,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
bool updateProj,
bool updateGrid,
bool updateScan,
bool updateOctomap,
const std::map<int, rtabmap::Signature> & signatures)
{
if(!updateCloud && !updateProj && !updateGrid && !updateScan)
@@ -190,8 +276,22 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
updateProj = projMapPub_.getNumSubscribers() != 0;
updateGrid = gridMapPub_.getNumSubscribers() != 0;
updateScan = scanMapPub_.getNumSubscribers() != 0;
updateOctomap =
octoMapPubBin_.getNumSubscribers() != 0 ||
octoMapPubFull_.getNumSubscribers() != 0 ||
octoMapCloud_.getNumSubscribers() != 0 ||
octoMapEmptySpace_.getNumSubscribers() != 0 ||
octoMapProj_.getNumSubscribers() != 0;
}
#ifndef WITH_OCTOMAP_ROS
updateOctomap = false;
#endif
#ifndef RTABMAP_OCTOMAP
updateOctomap = false;
#endif
UDEBUG("Updating map caches...");
if(!memory && signatures.size() == 0)
@@ -203,7 +303,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
std::map<int, rtabmap::Transform> filteredPoses;
// update cache
if(updateCloud || updateProj || updateGrid || updateScan)
if(updateCloud || updateProj || updateGrid || updateScan || updateOctomap)
{
// filter nodes
if(mapFilterRadius_ > 0.0)
@@ -229,7 +329,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
filteredPoses = poses;
}
if(negativePosesIgnored)
if(negativePosesIgnored_)
{
for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end();)
{
@@ -244,6 +344,31 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
}
}
bool longUpdate = false;
if(filteredPoses.size() > 20)
{
if(updateCloud && clouds_.size() < 5)
{
ROS_WARN("Many clouds should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-clouds_.size()));
longUpdate = true;
}
else if(updateProj && projMaps_.size() < 5)
{
ROS_WARN("Many occupancy grid map from projections should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-projMaps_.size()));
longUpdate = true;
}
else if(updateGrid && gridMaps_.size() < 5)
{
ROS_WARN("Many occupancy grid map from laser scans should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-gridMaps_.size()));
longUpdate = true;
}
else if(updateScan && scans_.size() < 5)
{
ROS_WARN("Many scans should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-scans_.size()));
longUpdate = true;
}
}
for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
{
if(!iter->second.isNull())
@@ -254,6 +379,18 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
bool gridRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first));
bool scanRequired = updateScan && (iter->first < 0 || !uContains(scans_, iter->first));
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
if(!rgbDepthRequired)
{
rgbDepthRequired = updateOctomap &&
(iter->first < 0 ||
octomap_->addedNodes().empty() ||
iter->first > octomap_->addedNodes().rbegin()->first);
}
#endif
#endif
if(rgbDepthRequired ||
depthRequired ||
scanRequired ||
@@ -373,9 +510,9 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
uInsert(cameraModels_, std::make_pair(iter->first, models));
}
if(depthRequired)
if(depthRequired || updateOctomap)
{
UDEBUG("Creating proj map for %d...", iter->first);
UDEBUG("Creating proj map / octomap for %d...", iter->first);
cv::Mat ground, obstacles;
if(cloudRGB.get())
{
@@ -393,12 +530,63 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
// add pose rotation without yaw
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0));
cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0));
util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_);
pcl::IndicesPtr groundIndices, obstaclesIndices;
util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
cloudClipped,
groundIndices,
obstaclesIndices,
20,
projMaxGroundAngle_*M_PI/180.0,
gridCellSize_*2.0f,
projMinClusterSize_,
projDetectFlatObstacles_,
projMaxGroundHeight_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
if(groundIndices->size())
{
pcl::copyPointCloud(*cloudClipped, *groundIndices, *groundCloud);
}
if(obstaclesIndices->size())
{
pcl::copyPointCloud(*cloudClipped, *obstaclesIndices, *obstaclesCloud);
}
if(updateProj)
{
util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
groundCloud,
obstaclesCloud,
ground,
obstacles,
gridCellSize_);
uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
}
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
if(updateOctomap)
{
Transform tinv = Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0).inverse();
groundCloud = util3d::transformPointCloud(groundCloud, tinv);
obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv);
if(octomapGroundIsObstacle_)
{
*obstaclesCloud += *groundCloud;
groundCloud->clear();
}
octomap_->addToCache(iter->first, groundCloud, obstaclesCloud);
}
#endif
#endif
}
}
else if(cloudXYZ.get())
else if(updateProj && cloudXYZ.get())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudClipped = cloudXYZ;
if(cloudClipped->size() && projMaxObstaclesHeight_ > 0)
@@ -410,13 +598,30 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
// add pose rotation without yaw
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0));
cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0));
UDEBUG("util3d::occupancy2DFromCloud3D()");
util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_, projDetectFlatObstacles_, projMaxGroundHeight_);
pcl::IndicesPtr groundIndices, obstaclesIndices;
util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
cloudClipped,
groundIndices,
obstaclesIndices,
20,
projMaxGroundAngle_*M_PI/180.0,
gridCellSize_*2.0f,
projMinClusterSize_,
projDetectFlatObstacles_,
projMaxGroundHeight_);
util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(
cloudClipped,
groundIndices,
obstaclesIndices,
ground,
obstacles,
gridCellSize_);
uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
}
}
uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
}
if(scanRequired || gridRequired)
@@ -477,6 +682,17 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
}
}
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
if(updateOctomap)
{
UTimer time;
octomap_->update(filteredPoses);
ROS_INFO("Octomap update time = %fs", time.ticks());
}
#endif
#endif
// cleanup not used nodes
UDEBUG("Cleanup not used nodes");
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=clouds_.begin();
@@ -539,6 +755,11 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
++iter;
}
}
if(longUpdate)
{
ROS_WARN("Map(s) updated!");
}
}
return filteredPoses;
@@ -654,6 +875,101 @@ void MapsManager::publishMaps(
clouds_.clear();
cameraModels_.clear();
}
#ifdef WITH_OCTOMAP_ROS
#ifdef RTABMAP_OCTOMAP
if(octoMapPubBin_.getNumSubscribers() ||
octoMapPubFull_.getNumSubscribers() ||
octoMapCloud_.getNumSubscribers() ||
octoMapEmptySpace_.getNumSubscribers() ||
octoMapProj_.getNumSubscribers())
{
if(octoMapPubBin_.getNumSubscribers())
{
octomap_msgs::Octomap msg;
octomap_msgs::binaryMapToMsg(*octomap_->octree(), msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapPubBin_.publish(msg);
}
if(octoMapPubFull_.getNumSubscribers())
{
octomap_msgs::Octomap msg;
octomap_msgs::fullMapToMsg(*octomap_->octree(), msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapPubFull_.publish(msg);
}
if(octoMapCloud_.getNumSubscribers() || octoMapEmptySpace_.getNumSubscribers())
{
sensor_msgs::PointCloud2 msg;
pcl::IndicesPtr obstacles(new std::vector<int>);
pcl::IndicesPtr ground(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacles.get(), ground.get());
if(octoMapCloud_.getNumSubscribers())
{
pcl::PointCloud<pcl::PointXYZRGB> cloudObstacles;
pcl::copyPointCloud(*cloud, *obstacles, cloudObstacles);
pcl::toROSMsg(cloudObstacles, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapCloud_.publish(msg);
}
if(octoMapEmptySpace_.getNumSubscribers())
{
pcl::PointCloud<pcl::PointXYZRGB> cloudGround;
pcl::copyPointCloud(*cloud, *ground, cloudGround);
pcl::toROSMsg(cloudGround, msg);
msg.header.frame_id = mapFrameId;
msg.header.stamp = stamp;
octoMapEmptySpace_.publish(msg);
}
}
if(octoMapProj_.getNumSubscribers())
{
// create the projection map
float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
cv::Mat pixels = octomap_->createProjectionMap(xMin, yMin, gridCellSize, gridSize_);
if(!pixels.empty())
{
//init
nav_msgs::OccupancyGrid map;
map.info.resolution = gridCellSize;
map.info.origin.position.x = 0.0;
map.info.origin.position.y = 0.0;
map.info.origin.position.z = 0.0;
map.info.origin.orientation.x = 0.0;
map.info.origin.orientation.y = 0.0;
map.info.origin.orientation.z = 0.0;
map.info.origin.orientation.w = 1.0;
map.info.width = pixels.cols;
map.info.height = pixels.rows;
map.info.origin.position.x = xMin;
map.info.origin.position.y = yMin;
map.data.resize(map.info.width * map.info.height);
memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
map.header.frame_id = mapFrameId;
map.header.stamp = stamp;
octoMapProj_.publish(map);
}
else if(poses.size())
{
ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size());
}
}
}
else
{
octomap_->clear();
}
#endif
#endif
if(scanMapPub_.getNumSubscribers())
{
@@ -820,54 +1136,3 @@ cv::Mat MapsManager::generateGridMap(
return map;
}
#ifdef WITH_OCTOMAP
// returned OcTree must be deleted
// RTAB-Map optimizes the graph at almost each iteration, an octomap cannot
// be updated online. Only available on service. To have an "online" octomap published as a topic,
// you may want to subscribe an octomap_server to /rtabmap/cloud topic.
//
octomap::OcTree * MapsManager::createOctomap(const std::map<int, Transform> & poses)
{
octomap::OcTree * octree = new octomap::OcTree(gridCellSize_);
UTimer time;
for(std::map<int, Transform>::const_iterator posesIter = poses.begin(); posesIter!=poses.end(); ++posesIter)
{
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator cloudsIter = clouds_.find(posesIter->first);
if(cloudsIter != clouds_.end() && cloudsIter->second->size())
{
octomap::Pointcloud * scan = new octomap::Pointcloud();
//octomap::pointcloudPCLToOctomap(*cloudsIter->second, *scan); // Not anymore in Indigo!
scan->reserve(cloudsIter->second->size());
for(pcl::PointCloud<pcl::PointXYZRGB>::const_iterator it = cloudsIter->second->begin();
it != cloudsIter->second->end();
++it)
{
// Check if the point is invalid
if(pcl::isFinite(*it))
{
scan->push_back(it->x, it->y, it->z);
}
}
float x,y,z, r,p,w;
posesIter->second.getTranslationAndEulerAngles(x,y,z,r,p,w);
octomap::ScanNode node(scan, octomap::pose6d(x,y,z, r,p,w), posesIter->first);
octree->insertPointCloud(node, cloudMaxDepth_, true, true);
ROS_INFO("inserted %d pt=%d (%fs)", posesIter->first, (int)scan->size(), time.ticks());
}
}
octree->updateInnerOccupancy();
ROS_INFO("updated inner occupancy (%fs)", time.ticks());
// clear memory if no one subscribed
if(mapCacheCleanup_ && cloudMapPub_.getNumSubscribers() == 0)
{
clouds_.clear();
cameraModels_.clear();
}
return octree;
}
#endif
src/MapsManager.h
+39 -14
View File
@@ -1,9 +1,29 @@
/*
* MapsManager.h
*
* Created on: 2015-05-14
* Author: mathieu
*/
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.
*/
#ifndef MAPSMANAGER_H_
#define MAPSMANAGER_H_
@@ -14,12 +34,8 @@
#include <ros/time.h>
#include <ros/publisher.h>
namespace octomap{
class OcTree;
}
namespace rtabmap {
class OctoMap;
class Memory;
} // namespace rtabmap
@@ -41,6 +57,7 @@ public:
bool updateProj,
bool updateGrid,
bool updateScan,
bool updateOctomap,
const std::map<int, rtabmap::Signature> & signatures = std::map<int, rtabmap::Signature>());
void publishMaps(
@@ -60,9 +77,7 @@ public:
float & yMin,
float & gridCellSize);
#ifdef WITH_OCTOMAP
octomap::OcTree * createOctomap(const std::map<int, rtabmap::Transform> & poses);
#endif
rtabmap::OctoMap * getOctomap() const {return octomap_;}
private:
// mapping stuff
@@ -84,6 +99,7 @@ private:
double projMaxObstaclesHeight_;
double projMaxGroundHeight_;
bool projDetectFlatObstacles_;
bool projMapFrame_;
double gridCellSize_;
double gridSize_;
bool gridEroded_;
@@ -92,18 +108,27 @@ private:
double mapFilterRadius_;
double mapFilterAngle_;
bool mapCacheCleanup_;
bool negativePosesIgnored;
bool negativePosesIgnored_;
ros::Publisher cloudMapPub_;
ros::Publisher projMapPub_;
ros::Publisher gridMapPub_;
ros::Publisher scanMapPub_;
ros::Publisher octoMapPubBin_;
ros::Publisher octoMapPubFull_;
ros::Publisher octoMapCloud_;
ros::Publisher octoMapEmptySpace_;
ros::Publisher octoMapProj_;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds_;
std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > scans_;
std::map<int, std::vector<rtabmap::CameraModel> > cameraModels_;
std::map<int, std::pair<cv::Mat, cv::Mat> > projMaps_; // <ground, obstacles>
std::map<int, std::pair<cv::Mat, cv::Mat> > gridMaps_; // <ground, obstacles>
rtabmap::OctoMap * octomap_;
int octomapTreeDepth_;
bool octomapGroundIsObstacle_;
};
#endif /* MAPSMANAGER_H_ */
src/MsgConversion.cpp
+29 -9
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -147,6 +147,7 @@ void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
stat.setRefImageId(info.refId);
stat.setLoopClosureId(info.loopClosureId);
stat.setProximityDetectionId(info.proximityDetectionId);
stat.setStamp(info.header.stamp.toSec());
stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform));
@@ -345,19 +346,38 @@ void cameraModelToROS(
const rtabmap::CameraModel & model,
sensor_msgs::CameraInfo & camInfo)
{
UASSERT(model.isValidForRectification());
camInfo.D = std::vector<double>(model.D_raw().cols);
memcpy(camInfo.D.data(), model.D_raw().data, model.D_raw().cols*sizeof(double));
UASSERT(model.K_raw().total() == 9);
memcpy(camInfo.K.elems, model.K_raw().data, 9*sizeof(double));
UASSERT(model.K_raw().empty() || model.K_raw().total() == 9);
if(model.K_raw().empty())
{
memset(camInfo.K.elems, 0.0, 9*sizeof(double));
}
else
{
memcpy(camInfo.K.elems, model.K_raw().data, 9*sizeof(double));
}
UASSERT(model.R().total() == 9);
memcpy(camInfo.R.elems, model.R().data, 9*sizeof(double));
UASSERT(model.R().empty() || model.R().total() == 9);
if(model.R().empty())
{
memset(camInfo.R.elems, 0.0, 9*sizeof(double));
}
else
{
memcpy(camInfo.R.elems, model.R().data, 9*sizeof(double));
}
UASSERT(model.P().total() == 12);
memcpy(camInfo.P.elems, model.P().data, 12*sizeof(double));
UASSERT(model.P().empty() || model.P().total() == 12);
if(model.P().empty())
{
memset(camInfo.P.elems, 0.0, 12*sizeof(double));
}
else
{
memcpy(camInfo.P.elems, model.P().data, 12*sizeof(double));
}
if(camInfo.D.size() > 5)
{
src/OdomMsgToTFNode.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/PreferencesDialogROS.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/PreferencesDialogROS.h
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/RGBDOdometryNode.cpp
+40 -344
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -25,358 +25,54 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "OdometryROS.h"
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <image_geometry/stereo_camera_model.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include "rtabmap_ros/MsgConversion.h"
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util2d.h>
#include "ros/ros.h"
#include "nodelet/loader.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/Parameters.h>
using namespace rtabmap;
class RGBDOdometry : public rtabmap_ros::OdometryROS
{
public:
RGBDOdometry(int argc, char * argv[]) :
rtabmap_ros::OdometryROS(argc, argv),
sync_(0),
sync2_(0),
queueSize_(5)
{
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
int depthCameras = 1;
pnh.param("queue_size", queueSize_, queueSize_);
pnh.param("depth_cameras", depthCameras, depthCameras);
if(depthCameras <= 0)
{
depthCameras = 1;
}
if(depthCameras > 2)
{
ROS_FATAL("Only 2 cameras maximum supported yet.");
}
if(depthCameras == 2)
{
ros::NodeHandle rgb0_nh(nh, "rgb0");
ros::NodeHandle depth0_nh(nh, "depth0");
ros::NodeHandle rgb0_pnh(pnh, "rgb0");
ros::NodeHandle depth0_pnh(pnh, "depth0");
image_transport::ImageTransport rgb0_it(rgb0_nh);
image_transport::ImageTransport depth0_it(depth0_nh);
image_transport::TransportHints hintsRgb0("raw", ros::TransportHints(), rgb0_pnh);
image_transport::TransportHints hintsDepth0("raw", ros::TransportHints(), depth0_pnh);
image_mono_sub_.subscribe(rgb0_it, rgb0_nh.resolveName("image"), 1, hintsRgb0);
image_depth_sub_.subscribe(depth0_it, depth0_nh.resolveName("image"), 1, hintsDepth0);
info_sub_.subscribe(rgb0_nh, "camera_info", 1);
ros::NodeHandle rgb1_nh(nh, "rgb1");
ros::NodeHandle depth1_nh(nh, "depth1");
ros::NodeHandle rgb1_pnh(pnh, "rgb1");
ros::NodeHandle depth1_pnh(pnh, "depth1");
image_transport::ImageTransport rgb1_it(rgb1_nh);
image_transport::ImageTransport depth1_it(depth1_nh);
image_transport::TransportHints hintsRgb1("raw", ros::TransportHints(), rgb1_pnh);
image_transport::TransportHints hintsDepth1("raw", ros::TransportHints(), depth1_pnh);
image_mono2_sub_.subscribe(rgb1_it, rgb1_nh.resolveName("image"), 1, hintsRgb1);
image_depth2_sub_.subscribe(depth1_it, depth1_nh.resolveName("image"), 1, hintsDepth1);
info2_sub_.subscribe(rgb1_nh, "camera_info", 1);
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str(),
image_mono2_sub_.getTopic().c_str(),
image_depth2_sub_.getTopic().c_str(),
info2_sub_.getTopic().c_str());
sync2_ = new message_filters::Synchronizer<MySync2Policy>(
MySync2Policy(queueSize_),
image_mono_sub_,
image_depth_sub_,
info_sub_,
image_mono2_sub_,
image_depth2_sub_,
info2_sub_);
sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
}
else
{
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info", 1);
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str());
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
}
virtual ~RGBDOdometry()
{
if(sync_)
{
delete sync_;
}
if(sync2_)
{
delete sync2_;
}
}
void callback(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
if(!this->isPaused())
{
if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 "
"recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s",
image->encoding.c_str(), depth->encoding.c_str());
return;
}
ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);
if(localTransform.isNull())
{
return;
}
if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
{
rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
rtabmap::SensorData data(
ptrImage->image,
ptrDepth->image,
rtabmapModel,
0,
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, stamp);
}
}
}
void callback2(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo,
const sensor_msgs::ImageConstPtr& image2,
const sensor_msgs::ImageConstPtr& depth2,
const sensor_msgs::CameraInfoConstPtr& cameraInfo2)
{
if(!this->isPaused())
{
std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfoConstPtr> infoMsgs;
imageMsgs.push_back(image);
imageMsgs.push_back(image2);
depthMsgs.push_back(depth);
depthMsgs.push_back(depth2);
infoMsgs.push_back(cameraInfo);
infoMsgs.push_back(cameraInfo2);
ros::Time higherStamp;
int imageWidth = imageMsgs[0]->width;
int imageHeight = imageMsgs[0]->height;
int cameraCount = imageMsgs.size();
cv::Mat rgb;
cv::Mat depth;
pcl::PointCloud<pcl::PointXYZ> scanCloud;
std::vector<CameraModel> cameraModels;
for(unsigned int i=0; i<imageMsgs.size(); ++i)
{
if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight);
UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight);
ros::Time stamp = imageMsgs[i]->header.stamp>depthMsgs[i]->header.stamp?imageMsgs[i]->header.stamp:depthMsgs[i]->header.stamp;
if(i == 0)
{
higherStamp = stamp;
}
else if(stamp > higherStamp)
{
higherStamp = stamp;
}
Transform localTransform = getTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp);
if(localTransform.isNull())
{
return;
}
cv_bridge::CvImageConstPtr ptrImage;
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
}
else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
}
else
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
cv::Mat subDepth = ptrDepth->image;
// initialize
if(rgb.empty())
{
rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
}
if(depth.empty())
{
depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
}
if(ptrImage->image.type() == rgb.type())
{
ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
ROS_ERROR("Some RGB images are not the same type!");
return;
}
if(subDepth.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
ROS_ERROR("Some Depth images are not the same type!");
return;
}
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform));
}
rtabmap::SensorData data(
rgb,
depth,
cameraModels,
0,
rtabmap_ros::timestampFromROS(higherStamp));
this->processData(data, higherStamp);
}
}
protected:
virtual void flushCallbacks()
{
// flush callbacks
if(sync_)
{
delete sync_;
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
if(sync2_)
{
delete sync2_;
sync2_ = new message_filters::Synchronizer<MySync2Policy>(
MySync2Policy(queueSize_),
image_mono_sub_,
image_depth_sub_,
info_sub_,
image_mono2_sub_,
image_depth2_sub_,
info2_sub_);
sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
}
}
private:
image_transport::SubscriberFilter image_mono_sub_;
image_transport::SubscriberFilter image_depth_sub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
image_transport::SubscriberFilter image_mono2_sub_;
image_transport::SubscriberFilter image_depth2_sub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> info2_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> * sync_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySync2Policy;
message_filters::Synchronizer<MySync2Policy> * sync2_;
int queueSize_;
};
int main(int argc, char *argv[])
int main(int argc, char **argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
ros::init(argc, argv, "rgbd_odometry");
// process "--params" argument
rtabmap_ros::OdometryROS::processArguments(argc, argv);
nodelet::V_string nargv;
for(int i=1;i<argc;++i)
{
if(strcmp(argv[i], "--params") == 0)
{
rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(false);
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
{
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
std::cout <<
str <<
std::setw(60 - str.size()) <<
" [" <<
rtabmap::Parameters::getDescription(iter->first).c_str() <<
"]" <<
std::endl;
}
ROS_WARN("Node will now exit after showing default odometry parameters because "
"argument \"--params\" is detected!");
exit(0);
}
else if(strcmp(argv[i], "--udebug") == 0)
{
ULogger::setLevel(ULogger::kDebug);
}
else if(strcmp(argv[i], "--uinfo") == 0)
{
ULogger::setLevel(ULogger::kInfo);
}
nargv.push_back(argv[i]);
}
RGBDOdometry odom(argc, argv);
nodelet::Loader nodelet;
nodelet::M_string remap(ros::names::getRemappings());
std::string nodelet_name = ros::this_node::getName();
nodelet.load(nodelet_name, "rtabmap_ros/rgbd_odometry", remap, nargv);
ros::spin();
return 0;
}
src/StereoCameraNode.cpp
+106
View File
@@ -0,0 +1,106 @@
/*
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 <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/core/CameraStereo.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap_ros/MsgConversion.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/CameraInfo.h>
#include <cv_bridge/cv_bridge.h>
int main(int argc, char** argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kInfo);
ros::init(argc, argv, "uvc_stereo_camera");
ros::NodeHandle pnh("~");
double rate = 0.0;
std::string id = "camera";
std::string frameId = "camera_link";
double scale = 1.0;
pnh.param("rate", rate, rate);
pnh.param("camera_id", id, id);
pnh.param("frame_id", frameId, frameId);
pnh.param("scale", scale, scale);
rtabmap::CameraStereoVideo camera(0, false, rate);
if(camera.init(UDirectory::homeDir() + "/.ros/camera_info", id))
{
ros::NodeHandle nh;
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
image_transport::ImageTransport left_it(left_nh);
image_transport::ImageTransport right_it(right_nh);
image_transport::Publisher imageLeftPub = left_it.advertise(left_nh.resolveName("image_raw"), 1);
image_transport::Publisher imageRightPub = right_it.advertise(right_nh.resolveName("image_raw"), 1);
ros::Publisher infoLeftPub = left_nh.advertise<sensor_msgs::CameraInfo>(left_nh.resolveName("camera_info"), 1);
ros::Publisher infoRightPub = right_nh.advertise<sensor_msgs::CameraInfo>(right_nh.resolveName("camera_info"), 1);
while(ros::ok())
{
rtabmap::SensorData data = camera.takeImage();
ros::Time currentTime = ros::Time::now();
cv_bridge::CvImage imageLeft;
imageLeft.header.frame_id = frameId;
imageLeft.header.stamp = currentTime;
imageLeft.encoding = "bgr8";
cv::resize(data.imageRaw(), imageLeft.image, cv::Size(0,0), scale, scale, CV_INTER_AREA);
imageLeftPub.publish(imageLeft.toImageMsg());
cv_bridge::CvImage imageRight;
imageRight.header = imageLeft.header;
imageRight.encoding = "mono8";
cv::resize(data.rightRaw(), imageRight.image, cv::Size(0,0), scale, scale, CV_INTER_AREA);
imageRightPub.publish(imageRight.toImageMsg());
sensor_msgs::CameraInfo infoLeft, infoRight;
rtabmap_ros::cameraModelToROS(data.stereoCameraModel().left().scaled(scale), infoLeft);
rtabmap_ros::cameraModelToROS(data.stereoCameraModel().right().scaled(scale), infoRight);
infoLeft.header = imageLeft.header;
infoRight.header = imageLeft.header;
infoLeftPub.publish(infoLeft);
infoRightPub.publish(infoRight);
ros::spinOnce();
}
}
else
{
ROS_ERROR("Could not initialize the camera!");
}
return 0;
}
src/StereoOdometryNode.cpp
+41 -196
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -25,210 +25,55 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "OdometryROS.h"
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <image_geometry/stereo_camera_model.h>
#include <cv_bridge/cv_bridge.h>
#include "rtabmap_ros/MsgConversion.h"
#include "ros/ros.h"
#include "nodelet/loader.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/core/Parameters.h>
using namespace rtabmap;
class StereoOdometry : public rtabmap_ros::OdometryROS
{
public:
StereoOdometry(int argc, char * argv[]) :
rtabmap_ros::OdometryROS(argc, argv, true),
approxSync_(0),
exactSync_(0),
queueSize_(5)
{
ros::NodeHandle nh;
ros::NodeHandle pnh("~");
bool approxSync = false;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize_, queueSize_);
ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
ros::NodeHandle left_pnh(pnh, "left");
ros::NodeHandle right_pnh(pnh, "right");
image_transport::ImageTransport left_it(left_nh);
image_transport::ImageTransport right_it(right_nh);
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str());
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
}
virtual ~StereoOdometry()
{
if(approxSync_)
{
delete approxSync_;
}
if(exactSync_)
{
delete exactSync_;
}
}
void callback(
const sensor_msgs::ImageConstPtr& imageRectLeft,
const sensor_msgs::ImageConstPtr& imageRectRight,
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
{
if(!this->isPaused())
{
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended)");
return;
}
ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp);
if(localTransform.isNull())
{
return;
}
ros::WallTime time = ros::WallTime::now();
int quality = -1;
if(imageRectLeft->data.size() && imageRectRight->data.size())
{
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform);
if(stereoModel.baseline() <= 0)
{
ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
return;
}
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
{
ROS_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
stereoModel.baseline());
shown = true;
}
}
cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8");
cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8");
UTimer stepTimer;
//
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
rtabmap::SensorData data(
ptrImageLeft->image,
ptrImageRight->image,
stereoModel,
0,
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, stamp);
}
else
{
ROS_WARN("Odom: input images empty?!?");
}
}
}
protected:
virtual void flushCallbacks()
{
//flush callbacks
if(approxSync_)
{
delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
if(exactSync_)
{
delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
}
private:
image_transport::SubscriberFilter imageRectLeft_;
image_transport::SubscriberFilter imageRectRight_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
int queueSize_;
};
int main(int argc, char *argv[])
int main(int argc, char **argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
//pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
ros::init(argc, argv, "stereo_odometry");
// process "--params" argument
rtabmap_ros::OdometryROS::processArguments(argc, argv, true);
nodelet::V_string nargv;
for(int i=1;i<argc;++i)
{
if(strcmp(argv[i], "--params") == 0)
{
rtabmap::ParametersMap parametersOdom = rtabmap::Parameters::getDefaultOdometryParameters(true);
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
{
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
std::cout <<
str <<
std::setw(60 - str.size()) <<
" [" <<
rtabmap::Parameters::getDescription(iter->first).c_str() <<
"]" <<
std::endl;
}
ROS_WARN("Node will now exit after showing default odometry parameters because "
"argument \"--params\" is detected!");
exit(0);
}
else if(strcmp(argv[i], "--udebug") == 0)
{
ULogger::setLevel(ULogger::kDebug);
}
else if(strcmp(argv[i], "--uinfo") == 0)
{
ULogger::setLevel(ULogger::kInfo);
}
nargv.push_back(argv[i]);
StereoOdometry odom(argc, argv);
}
nodelet::Loader nodelet;
nodelet::M_string remap(ros::names::getRemappings());
std::string nodelet_name = ros::this_node::getName();
nodelet.load(nodelet_name, "rtabmap_ros/stereo_odometry", remap, nargv);
ros::spin();
return 0;
}
src/OdometryROS.cpp → src/nodelets/OdometryROS.cpp
+96 -83
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -55,7 +55,7 @@ using namespace rtabmap;
namespace rtabmap_ros {
OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
OdometryROS::OdometryROS(bool stereo) :
odometry_(0),
frameId_("base_link"),
odomFrameId_("odom"),
@@ -64,19 +64,39 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
waitForTransform_(true),
waitForTransformDuration_(0.1), // 100 ms
publishNullWhenLost_(true),
guessFromTf_(false),
paused_(false),
resetCountdown_(0),
resetCurrentCount_(0)
resetCurrentCount_(0),
stereo_(stereo)
{
ros::NodeHandle nh;
}
OdometryROS::~OdometryROS()
{
ros::NodeHandle & pnh = getPrivateNodeHandle();
if(pnh.ok())
{
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
pnh.deleteParam(iter->first);
}
}
delete odometry_;
}
void OdometryROS::onInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
odomInfoPub_ = nh.advertise<rtabmap_ros::OdomInfo>("odom_info", 1);
odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
ros::NodeHandle pnh("~");
Transform initialPose = Transform::getIdentity();
std::string initialPoseStr;
std::string tfPrefix;
@@ -91,6 +111,13 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
pnh.param("config_path", configPath, configPath);
pnh.param("publish_null_when_lost", publishNullWhenLost_, publishNullWhenLost_);
pnh.param("guess_from_tf", guessFromTf_, guessFromTf_);
if(publishTf_ && guessFromTf_)
{
NODELET_WARN( "\"publish_tf\" and \"guess_from_tf\" cannot be used at the same time. \"guess_from_tf\" is disabled.");
guessFromTf_ = false;
}
configPath = uReplaceChar(configPath, '~', UDirectory::homeDir());
if(configPath.size() && configPath.at(0) != '/')
@@ -125,19 +152,19 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
}
else
{
ROS_ERROR("Wrong initial_pose format: %s (should be \"x y z roll pitch yaw\" with angle in radians). "
NODELET_ERROR( "Wrong initial_pose format: %s (should be \"x y z roll pitch yaw\" with angle in radians). "
"Identity will be used...", initialPoseStr.c_str());
}
}
//parameters
parameters_ = Parameters::getDefaultOdometryParameters(stereo);
parameters_ = Parameters::getDefaultOdometryParameters(stereo_);
if(!configPath.empty())
{
if(UFile::exists(configPath.c_str()))
{
ROS_INFO("Odometry: Loading parameters from %s", configPath.c_str());
NODELET_INFO( "Odometry: Loading parameters from %s", configPath.c_str());
rtabmap::ParametersMap allParameters;
Parameters::readINI(configPath.c_str(), allParameters);
// only update odometry parameters
@@ -152,7 +179,7 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
}
else
{
ROS_ERROR("Config file \"%s\" not found!", configPath.c_str());
NODELET_ERROR( "Config file \"%s\" not found!", configPath.c_str());
}
}
for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
@@ -163,39 +190,46 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
double vDouble;
if(pnh.getParam(iter->first, vStr))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
iter->second = vStr;
}
else if(pnh.getParam(iter->first, vBool))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
iter->second = uBool2Str(vBool);
}
else if(pnh.getParam(iter->first, vDouble))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
iter->second = uNumber2Str(vDouble);
}
else if(pnh.getParam(iter->first, vInt))
{
ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
NODELET_INFO( "Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
iter->second = uNumber2Str(vInt);
}
if(iter->first.compare(Parameters::kVisMinInliers()) == 0 && atoi(iter->second.c_str()) < 8)
{
ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
NODELET_WARN( "Parameter min_inliers must be >= 8, setting to 8...");
iter->second = uNumber2Str(8);
}
}
rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv);
std::vector<std::string> argList = getMyArgv();
char * argv[argList.size()];
for(unsigned int i=0; i<argList.size(); ++i)
{
argv[i] = &argList[i].at(0);
}
rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argList.size(), argv);
for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
rtabmap::ParametersMap::iterator jter = parameters_.find(iter->first);
if(jter!=parameters_.end())
{
ROS_INFO("Update odometry parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
NODELET_INFO( "Update odometry parameter \"%s\"=\"%s\" from arguments", iter->first.c_str(), iter->second.c_str());
jter->second = iter->second;
}
}
@@ -212,19 +246,19 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
{
// can be migrated
parameters_.at(iter->second.second)= vStr;
ROS_WARN("Odometry: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
NODELET_WARN( "Odometry: Parameter name changed: \"%s\" -> \"%s\". Please update your launch file accordingly. Value \"%s\" is still set to the new parameter name.",
iter->first.c_str(), iter->second.second.c_str(), vStr.c_str());
}
else
{
if(iter->second.second.empty())
{
ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore!",
NODELET_ERROR( "Odometry: Parameter \"%s\" doesn't exist anymore!",
iter->first.c_str());
}
else
{
ROS_ERROR("Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
NODELET_ERROR( "Odometry: Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
iter->first.c_str(), iter->second.second.c_str());
}
}
@@ -248,50 +282,8 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
setLogInfoSrv_ = pnh.advertiseService("log_info", &OdometryROS::setLogInfo, this);
setLogWarnSrv_ = pnh.advertiseService("log_warning", &OdometryROS::setLogWarn, this);
setLogErrorSrv_ = pnh.advertiseService("log_error", &OdometryROS::setLogError, this);
}
OdometryROS::~OdometryROS()
{
ros::NodeHandle pnh("~");
for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
{
pnh.deleteParam(iter->first);
}
delete odometry_;
}
void OdometryROS::processArguments(int argc, char * argv[], bool stereo)
{
for(int i=1;i<argc;++i)
{
if(strcmp(argv[i], "--params") == 0)
{
rtabmap::ParametersMap parametersOdom = Parameters::getDefaultOdometryParameters(stereo);
for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
{
std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
std::cout <<
str <<
std::setw(60 - str.size()) <<
" [" <<
rtabmap::Parameters::getDescription(iter->first).c_str() <<
"]" <<
std::endl;
}
ROS_WARN("Node will now exit after showing default odometry parameters because "
"argument \"--params\" is detected!");
exit(0);
}
else if(strcmp(argv[i], "--udebug") == 0)
{
ULogger::setLevel(ULogger::kDebug);
}
else if(strcmp(argv[i], "--uinfo") == 0)
{
ULogger::setLevel(ULogger::kInfo);
}
}
onOdomInit();
}
Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
@@ -305,7 +297,7 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::
//if(!tfBuffer_.canTransform(fromFrameId, toFrameId, stamp, ros::Duration(1)))
if(!tfListener_.waitForTransform(fromFrameId, toFrameId, stamp, ros::Duration(waitForTransformDuration_)))
{
ROS_WARN("odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)!",
NODELET_WARN( "odometry: Could not get transform from %s to %s (stamp=%f) after %f seconds (\"wait_for_transform_duration\"=%f)!",
fromFrameId.c_str(), toFrameId.c_str(), stamp.toSec(), waitForTransformDuration_, waitForTransformDuration_);
return transform;
}
@@ -317,14 +309,14 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
NODELET_WARN( "%s",ex.what());
}
return transform;
}
void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
{
if(odometry_->getPose().isNull() &&
if(odometry_->getPose().isIdentity() &&
!groundTruthFrameId_.empty())
{
// sync with the first value of the ground truth
@@ -334,18 +326,39 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
return;
}
ROS_INFO("Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
initialPose.prettyPrint().c_str(),
groundTruthFrameId_.c_str(),
frameId_.c_str());
odometry_->reset(initialPose);
}
Transform guess;
if(guessFromTf_)
{
Transform previousPose = this->getTransform(odomFrameId_, frameId_, ros::Time(odometry_->previousStamp()));
Transform pose = this->getTransform(odomFrameId_, frameId_, stamp);
if(!previousPose.isNull() && !pose.isNull())
{
guess = previousPose.inverse() * pose;
/*if(!odometry_->previousVelocityTransform().isNull())
{
float dt = rtabmap_ros::timestampFromROS(stamp) - odometry_->previousStamp();
float vx,vy,vz, vroll,vpitch,vyaw;
odometry_->previousVelocityTransform().getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
Transform motionGuess(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
NODELET_WARN( "P Guess %s", motionGuess.prettyPrint().c_str());
}
NODELET_WARN( "TF Guess %s", guess.prettyPrint().c_str());*/
}
}
// process data
ros::WallTime time = ros::WallTime::now();
rtabmap::OdometryInfo info;
SensorData dataCpy = data;
rtabmap::Transform pose = odometry_->process(dataCpy, &info);
rtabmap::Transform pose = odometry_->process(dataCpy, guess, &info);
if(!pose.isNull())
{
resetCurrentCount_ = resetCountdown_;
@@ -474,7 +487,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
}
else if(publishNullWhenLost_)
{
//ROS_WARN("Odometry lost!");
//NODELET_WARN( "Odometry lost!");
//send null pose to notify that odometry is lost
nav_msgs::Odometry odom;
@@ -500,7 +513,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
if(pose.isNull() && resetCurrentCount_ > 0)
{
ROS_WARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
NODELET_WARN( "Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", resetCurrentCount_);
--resetCurrentCount_;
if(resetCurrentCount_ == 0)
@@ -509,12 +522,12 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
Transform tfPose = this->getTransform(odomFrameId_, frameId_, stamp);
if(tfPose.isNull())
{
ROS_WARN("Odometry automatically reset to latest computed pose!");
NODELET_WARN( "Odometry automatically reset to latest computed pose!");
odometry_->reset(odometry_->getPose());
}
else
{
ROS_WARN("Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
NODELET_WARN( "Odometry automatically reset to latest odometry pose available from TF (%s->%s)!",
odomFrameId_.c_str(), frameId_.c_str());
odometry_->reset(tfPose);
}
@@ -531,7 +544,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odomInfoPub_.publish(infoMsg);
}
ROS_INFO("Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
NODELET_INFO( "Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
}
bool OdometryROS::isOdometryF2M() const
@@ -541,7 +554,7 @@ bool OdometryROS::isOdometryF2M() const
bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: reset odom!");
NODELET_INFO( "visual_odometry: reset odom!");
odometry_->reset();
this->flushCallbacks();
return true;
@@ -550,7 +563,7 @@ bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
bool OdometryROS::resetToPose(rtabmap_ros::ResetPose::Request& req, rtabmap_ros::ResetPose::Response&)
{
Transform pose(req.x, req.y, req.z, req.roll, req.pitch, req.yaw);
ROS_INFO("visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
NODELET_INFO( "visual_odometry: reset odom to pose %s!", pose.prettyPrint().c_str());
odometry_->reset(pose);
this->flushCallbacks();
return true;
@@ -560,12 +573,12 @@ bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
if(paused_)
{
ROS_WARN("visual_odometry: Already paused!");
NODELET_WARN( "visual_odometry: Already paused!");
}
else
{
paused_ = true;
ROS_INFO("visual_odometry: paused!");
NODELET_INFO( "visual_odometry: paused!");
}
return true;
}
@@ -574,37 +587,37 @@ bool OdometryROS::resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
if(!paused_)
{
ROS_WARN("visual_odometry: Already running!");
NODELET_WARN( "visual_odometry: Already running!");
}
else
{
paused_ = false;
ROS_INFO("visual_odometry: resumed!");
NODELET_INFO( "visual_odometry: resumed!");
}
return true;
}
bool OdometryROS::setLogDebug(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: Set log level to Debug");
NODELET_INFO( "visual_odometry: Set log level to Debug");
ULogger::setLevel(ULogger::kDebug);
return true;
}
bool OdometryROS::setLogInfo(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: Set log level to Info");
NODELET_INFO( "visual_odometry: Set log level to Info");
ULogger::setLevel(ULogger::kInfo);
return true;
}
bool OdometryROS::setLogWarn(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: Set log level to Warning");
NODELET_INFO( "visual_odometry: Set log level to Warning");
ULogger::setLevel(ULogger::kWarning);
return true;
}
bool OdometryROS::setLogError(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
ROS_INFO("visual_odometry: Set log level to Error");
NODELET_INFO( "visual_odometry: Set log level to Error");
ULogger::setLevel(ULogger::kError);
return true;
}
src/OdometryROS.h → src/nodelets/OdometryROS.h
+11 -5
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define ODOMETRYROS_H_
#include <ros/ros.h>
#include <nodelet/nodelet.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf/transform_listener.h>
@@ -46,14 +47,13 @@ class Odometry;
namespace rtabmap_ros {
class OdometryROS
class OdometryROS : public nodelet::Nodelet
{
public:
static void processArguments(int argc, char * argv[], bool stereo = false);
public:
OdometryROS(int argc, char * argv[], bool stereo = false);
OdometryROS(bool stereo);
virtual ~OdometryROS();
void processData(const rtabmap::SensorData & data, const ros::Time & stamp);
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -76,6 +76,10 @@ public:
protected:
virtual void flushCallbacks() = 0;
private:
virtual void onInit();
virtual void onOdomInit() = 0;
private:
rtabmap::Odometry * odometry_;
@@ -87,6 +91,7 @@ private:
bool waitForTransform_;
double waitForTransformDuration_;
bool publishNullWhenLost_;
bool guessFromTf_;
rtabmap::ParametersMap parameters_;
ros::Publisher odomPub_;
@@ -107,6 +112,7 @@ private:
bool paused_;
int resetCountdown_;
int resetCurrentCount_;
bool stereo_;
};
}
src/nodelets/data_odom_sync.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/nodelets/data_throttle.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/nodelets/disparity_to_depth.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/nodelets/obstacles_detection.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/nodelets/point_cloud_xyz.cpp
+8 -5
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -214,12 +214,15 @@ private:
model.fromCameraInfo(*cameraInfo);
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromDepth(
cv::Mat(imageDepthPtr->image, roi),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows),
rtabmap::CameraModel m(
model.fx(),
model.fy(),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
pclCloud = rtabmap::util3d::cloudFromDepth(
cv::Mat(imageDepthPtr->image, roi),
m,
decimation_);
processAndPublish(pclCloud, depth->header);
src/nodelets/point_cloud_xyzrgb.cpp
+8 -5
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -240,13 +240,16 @@ private:
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
cv::Rect roi = rtabmap::Feature2D::computeRoi(imageDepthPtr->image, roiRatios_);
rtabmap::CameraModel m(
model.fx(),
model.fy(),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
pclCloud = rtabmap::util3d::cloudFromDepthRGB(
cv::Mat(imagePtr->image, roi),
cv::Mat(imageDepthPtr->image, roi),
model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows),
model.fx(),
model.fy(),
m,
decimation_);
src/nodelets/rgbd_odometry.cpp
+419
View File
@@ -0,0 +1,419 @@
/*
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 "OdometryROS.h"
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <image_geometry/stereo_camera_model.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include "rtabmap_ros/MsgConversion.h"
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
using namespace rtabmap;
namespace rtabmap_ros
{
class RGBDOdometry : public rtabmap_ros::OdometryROS
{
public:
RGBDOdometry() :
OdometryROS(false),
approxSync_(0),
exactSync_(0),
sync2_(0),
queueSize_(5)
{
}
virtual ~RGBDOdometry()
{
if(approxSync_)
{
delete approxSync_;
}
if(exactSync_)
{
delete exactSync_;
}
if(sync2_)
{
delete sync2_;
}
}
private:
virtual void onOdomInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
int depthCameras = 1;
bool approxSync = true;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize_, queueSize_);
pnh.param("depth_cameras", depthCameras, depthCameras);
if(depthCameras <= 0)
{
depthCameras = 1;
}
if(depthCameras > 2)
{
NODELET_FATAL("Only 2 cameras maximum supported yet.");
}
if(depthCameras == 2)
{
ros::NodeHandle rgb0_nh(nh, "rgb0");
ros::NodeHandle depth0_nh(nh, "depth0");
ros::NodeHandle rgb0_pnh(pnh, "rgb0");
ros::NodeHandle depth0_pnh(pnh, "depth0");
image_transport::ImageTransport rgb0_it(rgb0_nh);
image_transport::ImageTransport depth0_it(depth0_nh);
image_transport::TransportHints hintsRgb0("raw", ros::TransportHints(), rgb0_pnh);
image_transport::TransportHints hintsDepth0("raw", ros::TransportHints(), depth0_pnh);
image_mono_sub_.subscribe(rgb0_it, rgb0_nh.resolveName("image"), 1, hintsRgb0);
image_depth_sub_.subscribe(depth0_it, depth0_nh.resolveName("image"), 1, hintsDepth0);
info_sub_.subscribe(rgb0_nh, "camera_info", 1);
ros::NodeHandle rgb1_nh(nh, "rgb1");
ros::NodeHandle depth1_nh(nh, "depth1");
ros::NodeHandle rgb1_pnh(pnh, "rgb1");
ros::NodeHandle depth1_pnh(pnh, "depth1");
image_transport::ImageTransport rgb1_it(rgb1_nh);
image_transport::ImageTransport depth1_it(depth1_nh);
image_transport::TransportHints hintsRgb1("raw", ros::TransportHints(), rgb1_pnh);
image_transport::TransportHints hintsDepth1("raw", ros::TransportHints(), depth1_pnh);
image_mono2_sub_.subscribe(rgb1_it, rgb1_nh.resolveName("image"), 1, hintsRgb1);
image_depth2_sub_.subscribe(depth1_it, depth1_nh.resolveName("image"), 1, hintsDepth1);
info2_sub_.subscribe(rgb1_nh, "camera_info", 1);
NODELET_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str(),
image_mono2_sub_.getTopic().c_str(),
image_depth2_sub_.getTopic().c_str(),
info2_sub_.getTopic().c_str());
sync2_ = new message_filters::Synchronizer<MySync2Policy>(
MySync2Policy(queueSize_),
image_mono_sub_,
image_depth_sub_,
info_sub_,
image_mono2_sub_,
image_depth2_sub_,
info2_sub_);
sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
}
else
{
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info", 1);
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
NODELET_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str());
}
}
void callback(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
if(!this->isPaused())
{
if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0 ||
depth->encoding.compare(sensor_msgs::image_encodings::MONO16)==0))
{
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 "
"recommended) and image_depth=16UC1,32FC1,mono16. Types detected: %s %s",
image->encoding.c_str(), depth->encoding.c_str());
return;
}
ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
Transform localTransform = getTransform(this->frameId(), image->header.frame_id, stamp);
if(localTransform.isNull())
{
return;
}
if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
{
rtabmap::CameraModel rtabmapModel = rtabmap_ros::cameraModelFromROS(*cameraInfo, localTransform);
cv_bridge::CvImagePtr ptrImage = cv_bridge::toCvCopy(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
rtabmap::SensorData data(
ptrImage->image,
ptrDepth->image,
rtabmapModel,
0,
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, stamp);
}
}
}
void callback2(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo,
const sensor_msgs::ImageConstPtr& image2,
const sensor_msgs::ImageConstPtr& depth2,
const sensor_msgs::CameraInfoConstPtr& cameraInfo2)
{
if(!this->isPaused())
{
std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfoConstPtr> infoMsgs;
imageMsgs.push_back(image);
imageMsgs.push_back(image2);
depthMsgs.push_back(depth);
depthMsgs.push_back(depth2);
infoMsgs.push_back(cameraInfo);
infoMsgs.push_back(cameraInfo2);
ros::Time higherStamp;
int imageWidth = imageMsgs[0]->width;
int imageHeight = imageMsgs[0]->height;
int cameraCount = imageMsgs.size();
cv::Mat rgb;
cv::Mat depth;
pcl::PointCloud<pcl::PointXYZ> scanCloud;
std::vector<CameraModel> cameraModels;
for(unsigned int i=0; i<imageMsgs.size(); ++i)
{
if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
UASSERT_MSG(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
imageMsgs[i]->width,
imageHeight,
imageMsgs[i]->height).c_str());
UASSERT_MSG(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight,
uFormat("imageWidth=%d vs %d imageHeight=%d vs %d",
imageWidth,
depthMsgs[i]->width,
imageHeight,
depthMsgs[i]->height).c_str());
ros::Time stamp = imageMsgs[i]->header.stamp>depthMsgs[i]->header.stamp?imageMsgs[i]->header.stamp:depthMsgs[i]->header.stamp;
if(i == 0)
{
higherStamp = stamp;
}
else if(stamp > higherStamp)
{
higherStamp = stamp;
}
Transform localTransform = getTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp);
if(localTransform.isNull())
{
return;
}
cv_bridge::CvImageConstPtr ptrImage;
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
}
else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
}
else
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
cv::Mat subDepth = ptrDepth->image;
// initialize
if(rgb.empty())
{
rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
}
if(depth.empty())
{
depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
}
if(ptrImage->image.type() == rgb.type())
{
ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
NODELET_ERROR("Some RGB images are not the same type!");
return;
}
if(subDepth.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
NODELET_ERROR("Some Depth images are not the same type!");
return;
}
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*infoMsgs[i], localTransform));
}
rtabmap::SensorData data(
rgb,
depth,
cameraModels,
0,
rtabmap_ros::timestampFromROS(higherStamp));
this->processData(data, higherStamp);
}
}
protected:
virtual void flushCallbacks()
{
// flush callbacks
if(approxSync_)
{
delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
approxSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
if(exactSync_)
{
delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), image_mono_sub_, image_depth_sub_, info_sub_);
exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
if(sync2_)
{
delete sync2_;
sync2_ = new message_filters::Synchronizer<MySync2Policy>(
MySync2Policy(queueSize_),
image_mono_sub_,
image_depth_sub_,
info_sub_,
image_mono2_sub_,
image_depth2_sub_,
info2_sub_);
sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
}
}
private:
image_transport::SubscriberFilter image_mono_sub_;
image_transport::SubscriberFilter image_depth_sub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
image_transport::SubscriberFilter image_mono2_sub_;
image_transport::SubscriberFilter image_depth2_sub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> info2_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySync2Policy;
message_filters::Synchronizer<MySync2Policy> * sync2_;
int queueSize_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::RGBDOdometry, nodelet::Nodelet);
}
src/nodelets/stereo_odometry.cpp
+235
View File
@@ -0,0 +1,235 @@
/*
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 "OdometryROS.h"
#include "pluginlib/class_list_macros.h"
#include "nodelet/nodelet.h"
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <image_geometry/stereo_camera_model.h>
#include <cv_bridge/cv_bridge.h>
#include "rtabmap_ros/MsgConversion.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UTimer.h>
using namespace rtabmap;
namespace rtabmap_ros
{
class StereoOdometry : public rtabmap_ros::OdometryROS
{
public:
StereoOdometry() :
rtabmap_ros::OdometryROS(true),
approxSync_(0),
exactSync_(0),
queueSize_(5)
{
}
virtual ~StereoOdometry()
{
if(approxSync_)
{
delete approxSync_;
}
if(exactSync_)
{
delete exactSync_;
}
}
private:
virtual void onOdomInit()
{
ros::NodeHandle & nh = getNodeHandle();
ros::NodeHandle & pnh = getPrivateNodeHandle();
bool approxSync = false;
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("queue_size", queueSize_, queueSize_);
NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
ros::NodeHandle left_nh(nh, "left");
ros::NodeHandle right_nh(nh, "right");
ros::NodeHandle left_pnh(pnh, "left");
ros::NodeHandle right_pnh(pnh, "right");
image_transport::ImageTransport left_it(left_nh);
image_transport::ImageTransport right_it(right_nh);
image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
if(approxSync)
{
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
else
{
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
NODELET_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str());
}
void callback(
const sensor_msgs::ImageConstPtr& imageRectLeft,
const sensor_msgs::ImageConstPtr& imageRectRight,
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
{
if(!this->isPaused())
{
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
{
NODELET_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended)");
return;
}
ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
Transform localTransform = getTransform(this->frameId(), imageRectLeft->header.frame_id, stamp);
if(localTransform.isNull())
{
return;
}
ros::WallTime time = ros::WallTime::now();
int quality = -1;
if(imageRectLeft->data.size() && imageRectRight->data.size())
{
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform);
if(stereoModel.baseline() <= 0)
{
NODELET_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
return;
}
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
{
NODELET_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
stereoModel.baseline());
shown = true;
}
}
cv_bridge::CvImagePtr ptrImageLeft = cv_bridge::toCvCopy(imageRectLeft, "mono8");
cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8");
UTimer stepTimer;
//
UDEBUG("localTransform = %s", localTransform.prettyPrint().c_str());
rtabmap::SensorData data(
ptrImageLeft->image,
ptrImageRight->image,
stereoModel,
0,
rtabmap_ros::timestampFromROS(stamp));
this->processData(data, stamp);
}
else
{
NODELET_WARN("Odom: input images empty?!?");
}
}
}
protected:
virtual void flushCallbacks()
{
//flush callbacks
if(approxSync_)
{
delete approxSync_;
approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
if(exactSync_)
{
delete exactSync_;
exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize_), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
}
}
private:
image_transport::SubscriberFilter imageRectLeft_;
image_transport::SubscriberFilter imageRectRight_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
int queueSize_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoOdometry, nodelet::Nodelet);
}
src/nodelets/stereo_throttle.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/rviz/InfoDisplay.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/rviz/InfoDisplay.h
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/rviz/MapCloudDisplay.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/rviz/MapCloudDisplay.h
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/rviz/MapGraphDisplay.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/rviz/MapGraphDisplay.h
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/rviz/OrbitOrientedViewController.cpp
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
src/rviz/OrbitOrientedViewController.h
+1 -1
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without