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

Updated ros-pkg for RTAB-Map 0.8.0

Browse Source 
Moved all nodelets and rviz plugins in "rtabmap_ros" namespace instead of "rtabmap"
Refactored rtabmap_ros messages (added convenient conversion methods in rtabmap_ros/MsgConversion.h)
Added noise filtering parameters for map_assembler node
Added variance parameter for map_optimizer node
Odometry nodes publish covariance matrices in odometry messages. Publish rtambap_ros::OdomInfo topic too.
This commit is contained in:
Mathieu Labbe
2014-12-14 16:44:13 -05:00
parent c91586ea57
commit dfe5cff0c0
74 changed files with 915 additions and 1088 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+4 -7
View File
@@ -13,7 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.7 REQUIRED)
find_package(RTABMap 0.8 REQUIRED)
#Qt stuff
FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
@@ -38,11 +38,12 @@ link_directories( ${OGRE_LIBRARY_DIRS} )
add_message_files(
FILES
Info.msg
InfoEx.msg
KeyPoint.msg
MapData.msg
Graph.msg
NodeData.msg
Bytes.msg
Link.msg
OdomInfo.msg
)
## Generate services in the 'srv' folder
@@ -146,10 +147,6 @@ target_link_libraries(rtabmap rtabmap_ros ${Libraries})
add_executable(rgbd_odometry src/RGBDOdometryNode.cpp)
target_link_libraries(rgbd_odometry rtabmap_ros ${Libraries})
#deprecated node
add_executable(visual_odometry src/VisualOdometryDEPRECATED.cpp)
target_link_libraries(visual_odometry ${Libraries})
add_executable(stereo_odometry src/StereoOdometryNode.cpp)
target_link_libraries(stereo_odometry rtabmap_ros ${Libraries})
cfg/Camera.cfg
+2 -2
View File
@@ -1,5 +1,5 @@
#!/usr/bin/env python
PACKAGE = "rtabmap"
PACKAGE = "rtabmap_ros"
from dynamic_reconfigure.parameter_generator_catkin import *
@@ -12,4 +12,4 @@ gen.add("height", int_t, 0, "Image height", 480, 0, 1080)
gen.add("video_or_images_path", str_t, 0, "Video or images directory path", "")
gen.add("pause", bool_t, 0, "Pause", False)
exit(gen.generate(PACKAGE, "rtabmap", "Camera"))
exit(gen.generate(PACKAGE, "rtabmap_ros", "Camera"))
include/rtabmap_ros/MsgConversion.h
+42 -3
View File
@@ -29,12 +29,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define MSGCONVERSION_H_
#include <tf/LinearMath/Transform.h>
#include <geometry_msgs/Transform.h>
#include <geometry_msgs/Pose.h>
#include <rtabmap/core/Transform.h>
#include <opencv2/opencv.hpp>
namespace rtabmap {
#include <opencv2/opencv.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/Link.h>
#include <rtabmap/core/Signature.h>
#include <rtabmap/core/OdometryInfo.h>
#include <rtabmap_ros/Link.h>
#include <rtabmap_ros/KeyPoint.h>
#include <rtabmap_ros/MapData.h>
#include <rtabmap_ros/Graph.h>
#include <rtabmap_ros/NodeData.h>
#include <rtabmap_ros/OdomInfo.h>
namespace rtabmap_ros {
void transformToTF(const rtabmap::Transform & transform, tf::Transform & tfTransform);
rtabmap::Transform transformFromTF(const tf::Transform & transform);
@@ -49,6 +63,31 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg);
void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes);
cv::Mat compressedMatFromBytes(const std::vector<unsigned char> & bytes, bool copy = true);
rtabmap::Link linkFromROS(const rtabmap_ros::Link & msg);
void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg);
cv::KeyPoint keypointFromROS(const rtabmap_ros::KeyPoint & msg);
void keypointToROS(const cv::KeyPoint & kpt, rtabmap_ros::KeyPoint & msg);
void mapGraphFromROS(
const rtabmap_ros::Graph & msg,
std::map<int, rtabmap::Transform> & poses,
std::map<int, int> & mapIds,
std::multimap<int, rtabmap::Link> & links,
rtabmap::Transform & mapToOdom);
void mapGraphToROS(
const std::map<int, rtabmap::Transform> & poses,
const std::map<int, int> & mapIds,
const std::multimap<int, rtabmap::Link> & links,
const rtabmap::Transform & mapToOdom,
rtabmap_ros::Graph & msg);
rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg);
void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg);
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg);
void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg);
}
#endif /* MSGCONVERSION_H_ */
launch/azimut3/az3_mapping_client.launch
+1 -1
View File
@@ -21,7 +21,7 @@
<!-- Below, construct point cloud of the latest throttled data, disabled for bandwidth efficiency -->
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="/camera/data_throttled_image_relay"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth_relay"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
launch/azimut3/az3_mapping_client_nav.launch
+1 -1
View File
@@ -11,7 +11,7 @@
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/azimut3/config/azimut3_nav.rviz"/>
<!-- Below, construct point cloud of the latest throttled data, disabled for bandwidth efficiency -->
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap/point_cloud_xyzrgb">
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="rgb/image" to="/camera/data_throttled_image_relay"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth_relay"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
launch/azimut3/az3_mapping_robot.launch
+1 -1
View File
@@ -10,7 +10,7 @@
<!-- Throttling messages -->
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle camera_nodelet_manager" output="screen">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="max_rate" type="double" value="5.0"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
launch/azimut3/az3_mapping_robot_kinect_odom.launch
+1 -1
View File
@@ -10,7 +10,7 @@
<!-- Throttling messages -->
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle camera_nodelet_manager" output="screen">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="max_rate" type="double" value="5.0"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
launch/azimut3/az3_mapping_robot_kinect_scan.launch
+1 -1
View File
@@ -10,7 +10,7 @@
<!-- Throttling messages -->
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle camera_nodelet_manager" output="screen">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="max_rate" type="double" value="5.0"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
launch/azimut3/az3_mapping_robot_nav.launch
+2 -2
View File
@@ -47,7 +47,7 @@
<!-- Throttling messages -->
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle camera_nodelet_manager" output="screen">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="max_rate" type="double" value="5.0"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
@@ -60,7 +60,7 @@
</node>
<!-- for the planner -->
<node pkg="nodelet" type="nodelet" name="points_xyzrgb_planner" args="load rtabmap/point_cloud_xyzrgb camera_nodelet_manager">
<node pkg="nodelet" type="nodelet" name="points_xyzrgb_planner" args="load rtabmap_ros/point_cloud_xyzrgb camera_nodelet_manager">
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
launch/azimut3/az3_mapping_robot_stereo_nav.launch
+3 -3
View File
@@ -69,7 +69,7 @@
<node pkg="nodelet" type="nodelet" name="stereo_nodelet" args="manager"/>
<!-- HACK: the fps parameter on camera1394stereo_node doesn't work for my camera!?!?
Throttle camera images -->
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="load rtabmap/stereo_throttle stereo_nodelet">
<node pkg="nodelet" type="nodelet" name="stereo_throttle" args="load rtabmap_ros/stereo_throttle stereo_nodelet">
<remap from="left/image" to="left/image_raw"/>
<remap from="right/image" to="right/image_raw"/>
<remap from="left/camera_info" to="left/camera_info"/>
@@ -88,7 +88,7 @@
</node>
<!-- Create point cloud for the planner -->
<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap/point_cloud_xyz stereo_nodelet">
<node pkg="nodelet" type="nodelet" name="disparity2cloud" args="load rtabmap_ros/point_cloud_xyz stereo_nodelet">
<remap from="disparity/image" to="disparity"/>
<remap from="disparity/camera_info" to="right/camera_info_throttle"/>
<remap from="cloud" to="cloudXYZ"/>
@@ -97,7 +97,7 @@
<param name="decimation" type="int" value="4"/>
<param name="max_depth" type="double" value="4"/>
</node>
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap/obstacles_detection stereo_nodelet">
<node pkg="nodelet" type="nodelet" name="obstacles_detection" args="load rtabmap_ros/obstacles_detection stereo_nodelet">
<remap from="cloud" to="cloudXYZ"/>
<remap from="obstacles" to="/planner_cloud"/>
launch/azimut3/az3_remote_mapping_robot.launch
+1 -1
View File
@@ -10,7 +10,7 @@
<!-- Throttling messages -->
<group ns="camera">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle camera_nodelet_manager" output="screen">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle camera_nodelet_manager" output="screen">
<param name="max_rate" type="double" value="5.0"/>
<remap from="rgb/image_in" to="rgb/image_rect_color"/>
launch/azimut3/az3_remote_mapping_rviz.launch
+1 -1
View File
@@ -56,7 +56,7 @@
<!-- Below, construct point cloud of the latest throttled data -->
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="/camera/data_throttled_image_relay"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth_relay"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info_relay"/>
launch/config/demo_find_object.rviz
+3 -3
View File
@@ -272,7 +272,7 @@ Visualization Manager:
Value: true
Axis: Z
Channel Name: intensity
Class: rtabmap/MapCloud
Class: rtabmap_ros/MapCloud
Cloud decimation: 4
Cloud max depth (m): 3
Cloud voxel size (m): 0.02
@@ -298,7 +298,7 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rtabmap/Info
- Class: rtabmap_ros/Info
Enabled: true
Name: Info
Topic: /rtabmap/info
@@ -330,7 +330,7 @@ Visualization Manager:
Value: true
Views:
Current:
Class: rtabmap/OrbitOriented
Class: rtabmap_ros/OrbitOriented
Distance: 7.83194
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
launch/config/demo_robot_mapping.rviz
+3 -3
View File
@@ -138,7 +138,7 @@ Visualization Manager:
Value: true
Axis: Z
Channel Name: intensity
Class: rtabmap/MapCloud
Class: rtabmap_ros/MapCloud
Cloud decimation: 8
Cloud max depth (m): 4
Cloud voxel size (m): 0.01
@@ -165,7 +165,7 @@ Visualization Manager:
Use rainbow: true
Value: true
- Alpha: 1
Class: rtabmap/MapGraph
Class: rtabmap_ros/MapGraph
Color: 25; 255; 0
Enabled: true
Name: MapGraph
@@ -179,7 +179,7 @@ Visualization Manager:
Name: Map
Topic: /rtabmap/grid_map
Value: true
- Class: rtabmap/Info
- Class: rtabmap_ros/Info
Enabled: true
Name: Info
Topic: /rtabmap/info
launch/config/demo_stereo_outdoor.rviz
+4 -4
View File
@@ -125,7 +125,7 @@ Visualization Manager:
Value: true
Axis: Z
Channel Name: intensity
Class: rtabmap/MapCloud
Class: rtabmap_ros/MapCloud
Cloud decimation: 4
Cloud max depth (m): 4
Cloud voxel size (m): 0
@@ -152,7 +152,7 @@ Visualization Manager:
Use rainbow: true
Value: true
- Alpha: 1
Class: rtabmap/MapGraph
Class: rtabmap_ros/MapGraph
Color: 25; 255; 0
Enabled: true
Name: MapGraph
@@ -177,7 +177,7 @@ Visualization Manager:
Name: Map
Topic: /map
Value: false
- Class: rtabmap/Info
- Class: rtabmap_ros/Info
Enabled: true
Name: Info
Topic: /rtabmap/info
@@ -205,7 +205,7 @@ Visualization Manager:
Value: true
Views:
Current:
Class: rtabmap/OrbitOriented
Class: rtabmap_ros/OrbitOriented
Distance: 6.60197
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
launch/config/rgbd.rviz
+2 -2
View File
@@ -135,7 +135,7 @@ Visualization Manager:
Value: true
Axis: Z
Channel Name: intensity
Class: rtabmap/MapCloud
Class: rtabmap_ros/MapCloud
Cloud decimation: 4
Cloud max depth (m): 4
Cloud voxel size (m): 0.01
@@ -161,7 +161,7 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rtabmap/Info
- Class: rtabmap_ros/Info
Enabled: true
Name: Info
Topic: /rtabmap/info
launch/config/rgbdslam_datasets.rviz
+2 -2
View File
@@ -143,7 +143,7 @@ Visualization Manager:
Value: true
Axis: Z
Channel Name: intensity
Class: rtabmap/MapCloud
Class: rtabmap_ros/MapCloud
Cloud decimation: 4
Cloud max depth (m): 4
Cloud voxel size (m): 0.01
@@ -169,7 +169,7 @@ Visualization Manager:
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rtabmap/Info
- Class: rtabmap_ros/Info
Enabled: true
Name: Info
Topic: /rtabmap/info
launch/data_recorder.launch
+1 -1
View File
@@ -21,7 +21,7 @@
<!-- Throttling messages -->
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager"/>
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap/data_throttle standalone_nodelet" output="screen">
<node pkg="nodelet" type="nodelet" name="data_throttle" args="load rtabmap_ros/data_throttle standalone_nodelet" output="screen">
<param name="max_rate" type="double" value="10.0"/>
<remap from="rgb/image_in" to="/camera/rgb/image_rect_color"/>
launch/demo/demo_appearance_localization.launch
+2 -2
View File
@@ -27,8 +27,8 @@
<!-- visualization of the "infoEx" topic sent by rtabmap node -->
<node name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen" args="-d $(find rtabmap_ros)/launch/config/appearance_gui.ini">
<!-- This enables the GUI to pause a rtabmap/camera when action "pause" is checked. -->
<!-- NOTE: It is specific to rtabmap/camera. Action "pause" in the GUI will still pause the rtabmap node. -->
<!-- This enables the GUI to pause a rtabmap_ros/camera when action "pause" is checked. -->
<!-- NOTE: It is specific to rtabmap_ros/camera. Action "pause" in the GUI will still pause the rtabmap node. -->
<param name="camera_node_name" type="string" value="/camera"/>
</node>
launch/demo/demo_appearance_mapping.launch
+4 -4
View File
@@ -23,15 +23,15 @@
<param name="Mem/IncrementalMemory" type="string" value="true"/> <!-- true = SLAM mode -->
<param name="Mem/RehearsalIdUpdatedToNewOne" type="string" value="true"/> <!-- On merging, update to new ID-->
<param name="Mem/BadSignaturesIgnored" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
<param name="Kp/DetectorStrategy" type="string" value="2"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="3"/> <!-- kdTree -->
</node>
<!-- visualization of the "infoEx" topic sent by rtabmap node -->
<node name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen" args="-d $(find rtabmap_ros)/launch/config/appearance_gui.ini">
<!-- This enables the GUI to pause a rtabmap/camera when action "pause" is checked. -->
<!-- NOTE: It is specific to rtabmap/camera. Action "pause" in the GUI will still pause the rtabmap node. -->
<!-- This enables the GUI to pause a rtabmap_ros/camera when action "pause" is checked. -->
<!-- NOTE: It is specific to rtabmap_ros/camera. Action "pause" in the GUI will still pause the rtabmap node. -->
<param name="camera_node_name" type="string" value="/camera"/>
</node>
launch/demo/demo_find_object.launch
+1 -3
View File
@@ -42,8 +42,6 @@
<param name="Rtabmap/TimeThr" type="string" value="700"/>
<param name="Mem/RehearsalSimilarity" type="string" value="0.45"/>
<param name="Mem/RehearsedNodesKept" type="string" value="false"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
</node>
</group>
@@ -65,7 +63,7 @@
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_find_object.rviz" output="screen"/>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="/camera/data_throttled_image"/>
<remap from="depth/image" to="/camera/data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="/camera/data_throttled_camera_info"/>
launch/demo/demo_multi-session_mapping.launch
+1 -1
View File
@@ -26,7 +26,7 @@
<param name="queue_size" type="int" value="10"/>
<!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
<param name="RGBD/ScanMatchingSize" type="string" value="0"/>
<param name="RGBD/PoseScanMatching" type="string" value="false"/>
<param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/>
<param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="true"/>
launch/demo/demo_robot_localization.launch
-2
View File
@@ -41,8 +41,6 @@
<param name="LccIcp2/MaxFitness" type="string" value="10"/>
<param name="LccBow/MaxDepth" type="string" value="0.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
</node>
<!-- Visualisation (client side) -->
launch/demo/demo_robot_mapping.launch
-2
View File
@@ -35,8 +35,6 @@
<param name="LccIcp/Type" type="string" value="2"/> <!-- 0=No ICP, 1=ICP 3D, 2=ICP 2D -->
<param name="LccBow/MaxDepth" type="string" value="0.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
</node>
<!-- Visualisation (client side) -->
launch/demo/demo_robot_mapping_rviz.launch
+1 -5
View File
@@ -37,8 +37,6 @@
<param name="LccBow/MaxDepth" type="string" value="0.0"/> <!-- 3D visual words maximum depth 0=infinity -->
<param name="LccBow/InlierDistance" type="string" value="0.1"/> <!-- 3D visual words correspondence distance -->
<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> <!-- Optimize graph from initial node so /map -> /odom transform will be generated -->
<param name="Kp/DetectorStrategy" type="string" value="0"/> <!-- use SURF -->
<param name="Kp/NNStrategy" type="string" value="1"/> <!-- kdTree -->
</node>
<!-- Grid map assembler for rviz -->
@@ -50,9 +48,7 @@
<!-- Visualisation -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_robot_mapping.rviz" output="screen"/>
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="rgb/image" to="data_throttled_image"/>
<remap from="depth/image" to="data_throttled_image_depth"/>
<remap from="rgb/camera_info" to="data_throttled_camera_info"/>
launch/demo/demo_stereo_outdoor.launch
+1 -1
View File
@@ -48,7 +48,7 @@
<param name="Odom/MinInliers" type="string" value="10"/>
<param name="Odom/RoiRatios" type="string" value="0.03 0.03 0.04 0.04"/>
<param name="Odom/MaxDepth" type="string" value="10"/>
<param name="OdomBow/NNDR" type="string" value="0.8"/>
<param name="GFTT/MaxCorners" type="string" value="500"/>
<param name="GFTT/MinDistance" type="string" value="5"/>
</node>
launch/demo/demo_stereo_pr2.launch
+1 -1
View File
@@ -13,7 +13,7 @@
<group ns="/wide_stereo">
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="disparity" args="load stereo_image_proc/disparity standalone_nodelet"/>
<node pkg="nodelet" type="nodelet" name="disparity2depth" args="load rtabmap/disparity_to_depth standalone_nodelet"/>
<node pkg="nodelet" type="nodelet" name="disparity2depth" args="load rtabmap_ros/disparity_to_depth standalone_nodelet"/>
</group>
<!-- Odometry: Run the viso2_ros package -->
launch/rgbd_localization.launch
+17 -8
View File
@@ -3,17 +3,23 @@
<!-- RGB-D LOCALIZATION VERSION -->
<!-- ODOMETRY ARGUMENTS: "strategy", "feature", "nn" and "local_map":
-Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-Local map size: number of unique features to keep track
<!-- ODOMETRY MAIN ARGUMENTS:
-"strategy" : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-"feature" : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-"nn" : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-"max_depth" : Maximum features depth (m)
-"min_inliers" : Minimum visual correspondences to accept a transformation (m)
-"inlier_distance" : RANSAC maximum inliers distance (m)
-"local_map" : Local map size: number of unique features to keep track
-->
<arg name="strategy" default="0" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="max_depth" default="4.0" />
<arg name="min_inliers" default="20" />
<arg name="inlier_distance" default="0.02" />
<arg name="local_map" default="1000" />
<!-- TF FRAMES -->
@@ -32,7 +38,10 @@
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
</node>
launch/rgbd_localization_rviz.launch
+19 -10
View File
@@ -3,17 +3,23 @@
<!-- RGB-D LOCALIZATION VERSION -->
<!-- ODOMETRY ARGUMENTS: "strategy", "feature", "nn" and "local_map":
-Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-Local map size: number of unique features to keep track
<!-- ODOMETRY MAIN ARGUMENTS:
-"strategy" : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-"feature" : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-"nn" : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-"max_depth" : Maximum features depth (m)
-"min_inliers" : Minimum visual correspondences to accept a transformation (m)
-"inlier_distance" : RANSAC maximum inliers distance (m)
-"local_map" : Local map size: number of unique features to keep track
-->
<arg name="strategy" default="0" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="max_depth" default="4.0" />
<arg name="min_inliers" default="20" />
<arg name="inlier_distance" default="0.02" />
<arg name="local_map" default="1000" />
<!-- TF FRAMES -->
@@ -32,7 +38,10 @@
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
</node>
@@ -64,7 +73,7 @@
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap/data_odom_sync standalone_nodelet">
<node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="camera/rgb/image_rect_color"/>
<remap from="depth/image_in" to="camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="camera/depth_registered/camera_info"/>
@@ -77,7 +86,7 @@
<param name="queue_size" type="int" value="30"/>
</node>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">
<node 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"/>
launch/rgbd_mapping.launch
+17 -8
View File
@@ -5,17 +5,23 @@
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- ODOMETRY ARGUMENTS: "strategy", "feature", "nn" and "local_map":
-Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-Local map size: number of unique features to keep track
<!-- ODOMETRY MAIN ARGUMENTS:
-"strategy" : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-"feature" : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-"nn" : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-"max_depth" : Maximum features depth (m)
-"min_inliers" : Minimum visual correspondences to accept a transformation (m)
-"inlier_distance" : RANSAC maximum inliers distance (m)
-"local_map" : Local map size: number of unique features to keep track
-->
<arg name="strategy" default="0" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="max_depth" default="4.0" />
<arg name="min_inliers" default="20" />
<arg name="inlier_distance" default="0.02" />
<arg name="local_map" default="1000" />
<!-- TF FRAMES -->
@@ -32,7 +38,10 @@
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="frame_id" type="string" value="base_link"/>
launch/rgbd_mapping_rviz.launch
+19 -10
View File
@@ -5,17 +5,23 @@
<!-- WARNING : Database is automatically deleted on each startup -->
<!-- See "delete_db_on_start" option below... -->
<!-- ODOMETRY ARGUMENTS: "strategy", "feature", "nn" and "local_map":
-Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-Local map size: number of unique features to keep track
<!-- ODOMETRY MAIN ARGUMENTS:
-"strategy" : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
-"feature" : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
-"nn" : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE
Set to 1 for float descriptor like SIFT/SURF
Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK
-"max_depth" : Maximum features depth (m)
-"min_inliers" : Minimum visual correspondences to accept a transformation (m)
-"inlier_distance" : RANSAC maximum inliers distance (m)
-"local_map" : Local map size: number of unique features to keep track
-->
<arg name="strategy" default="0" />
<arg name="feature" default="6" />
<arg name="nn" default="3" />
<arg name="max_depth" default="4.0" />
<arg name="min_inliers" default="20" />
<arg name="inlier_distance" default="0.02" />
<arg name="local_map" default="1000" />
@@ -33,7 +39,10 @@
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="frame_id" type="string" value="base_link"/>
@@ -63,7 +72,7 @@
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap/data_odom_sync standalone_nodelet">
<node pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="camera/rgb/image_rect_color"/>
<remap from="depth/image_in" to="camera/depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="camera/depth_registered/camera_info"/>
@@ -76,7 +85,7 @@
<param name="queue_size" type="int" value="30"/>
</node>
<node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap/point_cloud_xyzrgb standalone_nodelet">
<node 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"/>
msg/Bytes.msg
-3
View File
@@ -1,3 +0,0 @@
uint8[] bytes
msg/Graph.msg
+23
View File
@@ -0,0 +1,23 @@
Header header
##
# /map to /odom transform
# Always identity when the graph is optimized from the latest pose.
##
geometry_msgs/Transform mapToOdom
##
# The nodes
# std::map<nodeId, mapId>
int32[] nodeIds
int32[] mapIds
# std::map<nodeId, Pose>
geometry_msgs/Pose[] poses
##
# The links
##
Link[] links
msg/Info.msg
+24 -2
View File
@@ -1,7 +1,6 @@
########################################
# If a loop is found with the current image ("refId"),
# "loopClosureId" is not null.
# RTAB-Map info with statistics
########################################
Header header
@@ -11,3 +10,26 @@ int32 loopClosureId
int32 localLoopClosureId
geometry_msgs/Transform loopClosureTransform
####
# For statistics...
####
# std::map<int, float> posterior;
int32[] posteriorKeys
float32[] posteriorValues
# std::map<int, float> likelihood;
int32[] likelihoodKeys
float32[] likelihoodValues
# std::map<int, float> rawLikelihood;
int32[] rawLikelihoodKeys
float32[] rawLikelihoodValues
# std::map<int, int> weights;
int32[] weightsKeys
int32[] weightsValues
# std::map<std::string, float> stats
string[] statsKeys
float32[] statsValues
msg/InfoEx.msg
-35
View File
@@ -1,35 +0,0 @@
########################################
# Extended info msg with statistics
########################################
Header header
int32 refId
int32 loopClosureId
int32 localLoopClosureId
geometry_msgs/Transform loopClosureTransform
####
# For statistics...
####
# std::map<int, float> posterior;
int32[] posteriorKeys
float32[] posteriorValues
# std::map<int, float> likelihood;
int32[] likelihoodKeys
float32[] likelihoodValues
# std::map<int, float> rawLikelihood;
int32[] rawLikelihoodKeys
float32[] rawLikelihoodValues
# std::map<int, int> weights;
int32[] weightsKeys
int32[] weightsValues
# std::map<std::string, float> stats
string[] statsKeys
float32[] statsValues
msg/Link.msg
+14
View File
@@ -0,0 +1,14 @@
#class rtabmap::Link
#{
# int from;
# int to;
# Type type;
# Transform transform;
# float variance;
#}
int32 fromId
int32 toId
int32 type
geometry_msgs/Transform transform
int32 variance
msg/MapData.msg
+1 -17
View File
@@ -5,23 +5,7 @@ Header header
# Graph stuff
##################
# /map to /odom transform
# Always identity when the graph is optimized from the latest pose.
geometry_msgs/Transform mapToOdom
# std::map<int, Pose> poses;
int32[] poseIDs
geometry_msgs/Pose[] poses
# std::multimap<int, Link> constraints;
int32[] constraintFromIDs
int32[] constraintToIDs
int32[] constraintTypes
geometry_msgs/Transform[] constraints
# Map ids std::map<nodeId, mapId>
int32[] mapIDs
int32[] maps
Graph graph
##################
# Point cloud stuff
msg/NodeData.msg
+12 -12
View File
@@ -6,28 +6,28 @@ int32 mapId
geometry_msgs/Pose pose
# compressed image in /camera_link frame
# use rtabmap::util3d::uncompressImage() from <rtabmap/core/util3d.h>
Bytes image
# use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h"
uint8[] image
# compressed depth image in /camera_link frame
# use rtabmap::util3d::uncompressImage() from <rtabmap/core/util3d.h>
Bytes depth
# use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h"
uint8[] depth
float32 fx
float32 fy
float32 cx
float32 cy
# compressed 2D point cloud (laser scan) in /base_link frame
# use rtabmap::util3d::uncompressData() from <rtabmap/core/util3d.h>
Bytes depth2D
# compressed 2D laser scan in /base_link frame
# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
uint8[] laserScan
# local transform (/base_link -> /camera_link)
geometry_msgs/Transform localTransform
# std::multimap<int, cv::Keypoint> words
# std::multimap<int, pcl::PointXYZ> words3D
int32[] wordsKeys
KeyPoint[] wordsValues
sensor_msgs/PointCloud2 words3DValues
# std::multimap<wordId, cv::Keypoint>
# std::multimap<wordId, pcl::PointXYZ>
int32[] wordIds
KeyPoint[] wordKpts
sensor_msgs/PointCloud2 wordPts
msg/OdomInfo.msg
+21
View File
@@ -0,0 +1,21 @@
Header header
#class rtabmap::OdometryInfo
#{
# bool lost;
# int matches;
# int inliers;
# float variance;
# int features;
# int localMapSize;
# float time;
#}
bool lost
int32 matches
int32 inliers
float32 variance
int32 features
int32 localMapSize
float32 time
nodelet_plugins.xml
+14 -14
View File
@@ -1,54 +1,54 @@
<library path="lib/librtabmap_ros">
<class name="rtabmap/data_throttle"
type="rtabmap::DataThrottleNodelet"
<class name="rtabmap_ros/data_throttle"
type="rtabmap_ros::DataThrottleNodelet"
base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
</description>
</class>
<class name="rtabmap/stereo_throttle"
type="rtabmap::StereoThrottleNodelet"
<class name="rtabmap_ros/stereo_throttle"
type="rtabmap_ros::StereoThrottleNodelet"
base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
</description>
</class>
<class name="rtabmap/data_odom_sync"
type="rtabmap::DataOdomSyncNodelet"
<class name="rtabmap_ros/data_odom_sync"
type="rtabmap_ros::DataOdomSyncNodelet"
base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
</description>
</class>
<class name="rtabmap/point_cloud_xyzrgb"
type="rtabmap::PointCloudXYZRGB"
<class name="rtabmap_ros/point_cloud_xyzrgb"
type="rtabmap_ros::PointCloudXYZRGB"
base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
</description>
</class>
<class name="rtabmap/point_cloud_xyz"
type="rtabmap::PointCloudXYZ"
<class name="rtabmap_ros/point_cloud_xyz"
type="rtabmap_ros::PointCloudXYZ"
base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
</description>
</class>
<class name="rtabmap/disparity_to_depth"
type="rtabmap::DisparityToDepth"
<class name="rtabmap_ros/disparity_to_depth"
type="rtabmap_ros::DisparityToDepth"
base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
</description>
</class>
<class name="rtabmap/obstacles_detection"
type="rtabmap::ObstaclesDetection"
<class name="rtabmap_ros/obstacles_detection"
type="rtabmap_ros::ObstaclesDetection"
base_class_type="nodelet::Nodelet">
<description>
This is my nodelet.
package.xml
+1 -1
View File
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>rtabmap_ros</name>
<version>0.7.3</version>
<version>0.8.0</version>
<description>RTAB-Map's ros-pkg. RTAB-Map is an RGB-D SLAM approach with real-time constraints.</description>
<maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
plugin_description.xml
+11 -11
View File
@@ -1,27 +1,27 @@
<library path="lib/librtabmap_ros">
<class name="rtabmap/MapCloud"
type="rtabmap::MapCloudDisplay"
<class name="rtabmap_ros/MapCloud"
type="rtabmap_ros::MapCloudDisplay"
base_class_type="rviz::Display">
<description>
Displays graph point clouds from rtabmap/MapData messages.
Displays graph point clouds from rtabmap_ros/MapData messages.
</description>
</class>
<class name="rtabmap/MapGraph"
type="rtabmap::MapGraphDisplay"
<class name="rtabmap_ros/MapGraph"
type="rtabmap_ros::MapGraphDisplay"
base_class_type="rviz::Display">
<description>
Displays graphs from rtabmap/MapData messages.
Displays graphs from rtabmap_ros/MapData messages.
</description>
</class>
<class name="rtabmap/Info"
type="rtabmap::InfoDisplay"
<class name="rtabmap_ros/Info"
type="rtabmap_ros::InfoDisplay"
base_class_type="rviz::Display">
<description>
Displays information from rtabmap/Info messages.
Displays information from rtabmap_ros/Info messages.
</description>
</class>
<class name="rtabmap/OrbitOriented"
type="rtabmap::OrbitOrientedViewController"
<class name="rtabmap_ros/OrbitOriented"
type="rtabmap_ros::OrbitOrientedViewController"
base_class_type="rviz::ViewController">
<description>
Camera orbit with orientation.
src/CameraNode.cpp
+3 -3
View File
@@ -267,7 +267,7 @@ private:
};
CameraWrapper * camera = 0;
void callback(rtabmap::CameraConfig &config, uint32_t level)
void callback(rtabmap_ros::CameraConfig &config, uint32_t level)
{
if(camera)
{
@@ -287,8 +287,8 @@ int main(int argc, char** argv)
camera = new CameraWrapper(); // webcam device 0
dynamic_reconfigure::Server<rtabmap::CameraConfig> server;
dynamic_reconfigure::Server<rtabmap::CameraConfig>::CallbackType f;
dynamic_reconfigure::Server<rtabmap_ros::CameraConfig> server;
dynamic_reconfigure::Server<rtabmap_ros::CameraConfig>::CallbackType f;
f = boost::bind(&callback, _1, _2);
server.setCallback(f);
src/CoreWrapper.cpp
+101 -291
View File
@@ -51,7 +51,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//msgs
#include "rtabmap_ros/Info.h"
#include "rtabmap_ros/InfoEx.h"
#include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/GetMap.h"
#include "rtabmap_ros/PublishMap.h"
@@ -125,8 +124,8 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
infoPubEx_ = nh.advertise<rtabmap_ros::InfoEx>("infoEx", 1);
mapData_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
mapGraph_ = nh.advertise<rtabmap_ros::Graph>("graph", 1);
configPath_ = uReplaceChar(configPath_, '~', UDirectory::homeDir());
@@ -484,7 +483,7 @@ void CoreWrapper::depthCallback(
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
@@ -492,7 +491,7 @@ void CoreWrapper::depthCallback(
return;
}
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrImage;
if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
@@ -517,6 +516,7 @@ void CoreWrapper::depthCallback(
ptrImage->image,
odom,
odomMsg->header.frame_id,
odomMsg->pose.covariance[0]>0?odomMsg->pose.covariance[0]:1.0f,
ptrDepth->image,
fx,
fy,
@@ -577,7 +577,7 @@ void CoreWrapper::depthScanCallback(
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
@@ -591,9 +591,9 @@ void CoreWrapper::depthScanCallback(
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ> pclScan;
pcl::fromROSMsg(scanOut, pclScan);
cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
cv::Mat scan = util3d::laserScanFromPointCloud(pclScan);
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrImage;
if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
@@ -618,6 +618,7 @@ void CoreWrapper::depthScanCallback(
ptrImage->image,
odom,
odomMsg->header.frame_id,
odomMsg->pose.covariance[0]>0?odomMsg->pose.covariance[0]:1.0f,
ptrDepth->image,
fx,
fy,
@@ -674,7 +675,7 @@ void CoreWrapper::stereoCallback(
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
@@ -682,7 +683,7 @@ void CoreWrapper::stereoCallback(
return;
}
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
@@ -708,6 +709,7 @@ void CoreWrapper::stereoCallback(
ptrLeftImage->image,
odom,
odomMsg->header.frame_id,
odomMsg->pose.covariance[0]>0?odomMsg->pose.covariance[0]:1.0f,
ptrRightImage->image,
fx,
baseline,
@@ -771,7 +773,7 @@ void CoreWrapper::stereoScanCallback(
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
@@ -785,9 +787,9 @@ void CoreWrapper::stereoScanCallback(
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ> pclScan;
pcl::fromROSMsg(scanOut, pclScan);
cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
cv::Mat scan = util3d::laserScanFromPointCloud(pclScan);
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
@@ -813,6 +815,7 @@ void CoreWrapper::stereoScanCallback(
ptrLeftImage->image,
odom,
odomMsg->header.frame_id,
odomMsg->pose.covariance[0],
ptrRightImage->image,
fx,
baseline,
@@ -828,6 +831,7 @@ void CoreWrapper::process(
const cv::Mat & image,
const Transform & odom,
const std::string & odomFrameId,
float odomVariance,
const cv::Mat & depthOrRightImage,
float fx,
float fyOrBaseline,
@@ -875,15 +879,16 @@ void CoreWrapper::process(
}
}
SensorData data(image.clone(),
SensorData data(scan,
image.clone(),
imageB,
scan,
fx,
fyOrBaseline,
cx,
cy,
odom,
localTransform,
odom,
odomVariance,
id);
if(!rtabmap_.process(data))
@@ -893,7 +898,7 @@ void CoreWrapper::process(
else
{
mapToOdomMutex_.lock();
rtabmap::transformToTF(rtabmap_.getMapCorrection(), mapToOdom_);
rtabmap_ros::transformToTF(rtabmap_.getMapCorrection(), mapToOdom_);
odomFrameId_ = odomFrameId;
mapToOdomMutex_.unlock();
@@ -1052,99 +1057,25 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
req.global);
}
int i=0;
rep.data.mapIDs.resize(mapIds.size());
rep.data.maps.resize(mapIds.size());
i=0;
for(std::map<int, int>::iterator iter = mapIds.begin(); iter!=mapIds.end(); ++iter)
if(poses.size() && poses.size() != mapIds.size())
{
rep.data.mapIDs[i] = iter->first;
rep.data.maps[i] = iter->second;
++i;
ROS_ERROR("poses and map ids are not the same size!? %d vs %d", (int)poses.size(), (int)mapIds.size());
return false;
}
rep.data.poseIDs.resize(poses.size());
rep.data.poses.resize(poses.size());
i=0;
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
rep.data.poseIDs[i] = iter->first;
transformToPoseMsg(iter->second, rep.data.poses[i]);
++i;
}
rep.data.constraintFromIDs.resize(constraints.size());
rep.data.constraintToIDs.resize(constraints.size());
rep.data.constraintTypes.resize(constraints.size());
rep.data.constraints.resize(constraints.size());
i=0;
for(std::multimap<int, Link>::iterator iter = constraints.begin(); iter!=constraints.end(); ++iter)
{
rep.data.constraintFromIDs[i] = iter->first;
rep.data.constraintToIDs[i] = iter->second.to();
rep.data.constraintTypes[i] = iter->second.type();
transformToGeometryMsg(iter->second.transform(), rep.data.constraints[i]);
++i;
}
//RGB-D SLAM data
rtabmap_ros::mapGraphToROS(poses,
mapIds,
constraints,
Transform::getIdentity(),
rep.data.graph);
// add data
rep.data.nodes.resize(signatures.size());
i=0;
int i=0;
for(std::map<int, Signature>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
{
rep.data.nodes[i].id = iter->second.id();
rep.data.nodes[i].mapId = iter->second.mapId();
transformToPoseMsg(iter->second.getPose(), rep.data.nodes[i].pose);
compressedMatToBytes(iter->second.getImageCompressed(), rep.data.nodes[i].image.bytes);
compressedMatToBytes(iter->second.getDepthCompressed(), rep.data.nodes[i].depth.bytes);
compressedMatToBytes(iter->second.getDepth2DCompressed(), rep.data.nodes[i].depth2D.bytes);
rep.data.nodes[i].fx = iter->second.getDepthFx();
rep.data.nodes[i].fy = iter->second.getDepthFy();
rep.data.nodes[i].cx = iter->second.getDepthCx();
rep.data.nodes[i].cy = iter->second.getDepthCy();
transformToGeometryMsg(iter->second.getLocalTransform(), rep.data.nodes[i].localTransform);
//Features stuff...
rep.data.nodes[i].wordsKeys = uKeys(iter->second.getWords());
rep.data.nodes[i].wordsValues.resize(iter->second.getWords().size());
int j = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator jter=iter->second.getWords().begin();
jter!=iter->second.getWords().end();
++jter)
{
rep.data.nodes[i].wordsValues.at(j).angle = jter->second.angle;
rep.data.nodes[i].wordsValues.at(j).response = jter->second.response;
rep.data.nodes[i].wordsValues.at(j).ptx = jter->second.pt.x;
rep.data.nodes[i].wordsValues.at(j).pty = jter->second.pt.y;
rep.data.nodes[i].wordsValues.at(j).size = jter->second.size;
rep.data.nodes[i].wordsValues.at(j).octave = jter->second.octave;
rep.data.nodes[i].wordsValues.at(j).class_id = jter->second.class_id;
++j;
}
if(iter->second.getWords3().size() && iter->second.getWords3().size() == iter->second.getWords().size())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.resize(iter->second.getWords3().size());
j = 0;
for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=iter->second.getWords3().begin();
jter!=iter->second.getWords3().end();
++jter)
{
cloud[j++] = jter->second;
}
pcl::toROSMsg(cloud, rep.data.nodes[i].words3DValues);
}
else if(iter->second.getWords3().size())
{
ROS_ERROR("Words 2D and words 3D must have the same size (%d vs %d)!",
(int)iter->second.getWords().size(),
(int)iter->second.getWords3().size());
}
++i;
rtabmap_ros::nodeDataToROS(iter->second, rep.data.nodes[i++]);
}
rep.data.header.stamp = ros::Time::now();
@@ -1155,7 +1086,7 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtabmap_ros::PublishMap::Response& res)
{
if(mapData_.getNumSubscribers())
if(mapData_.getNumSubscribers() || mapGraph_.getNumSubscribers())
{
ROS_INFO("rtabmap: Publishing map...");
@@ -1164,7 +1095,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
std::multimap<int, Link> constraints;
std::map<int, int> mapIds;
if(req.graphOnly)
if(mapData_.getNumSubscribers() == 0 || req.graphOnly)
{
rtabmap_.getGraph(
poses,
@@ -1184,99 +1115,48 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
req.global);
}
//RGB-D SLAM data
rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
msg->header.stamp = ros::Time::now();
msg->header.frame_id = mapFrameId_;
int i=0;
msg->mapIDs = uKeys(mapIds);
msg->maps = uValues(mapIds);
msg->poseIDs.resize(poses.size());
msg->poses.resize(poses.size());
i=0;
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
if(poses.size() && poses.size() != mapIds.size())
{
msg->poseIDs[i] = iter->first;
transformToPoseMsg(iter->second, msg->poses[i]);
++i;
ROS_ERROR("poses and map ids are not the same size!? %d vs %d", (int)poses.size(), (int)mapIds.size());
return false;
}
msg->constraintFromIDs.resize(constraints.size());
msg->constraintToIDs.resize(constraints.size());
msg->constraintTypes.resize(constraints.size());
msg->constraints.resize(constraints.size());
i=0;
for(std::multimap<int, Link>::iterator iter = constraints.begin(); iter!=constraints.end(); ++iter)
rtabmap_ros::GraphPtr graphMsg(new rtabmap_ros::Graph);
graphMsg->header.stamp = ros::Time::now();
graphMsg->header.frame_id = mapFrameId_;
rtabmap_ros::mapGraphToROS(poses,
mapIds,
constraints,
Transform::getIdentity(),
*graphMsg);
if(mapData_.getNumSubscribers())
{
msg->constraintFromIDs[i] = iter->first;
msg->constraintToIDs[i] = iter->second.to();
msg->constraintTypes[i] = iter->second.type();
transformToGeometryMsg(iter->second.transform(), msg->constraints[i]);
++i;
//RGB-D SLAM data
rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
msg->header = graphMsg->header;
msg->graph = *graphMsg;
// add data
msg->nodes.resize(signatures.size());
int i=0;
for(std::map<int, Signature>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
{
rtabmap_ros::nodeDataToROS(iter->second, msg->nodes[i++]);
}
mapData_.publish(msg);
}
// add data
msg->nodes.resize(signatures.size());
i=0;
for(std::map<int, Signature>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
if(mapGraph_.getNumSubscribers())
{
msg->nodes[i].id = iter->second.id();
msg->nodes[i].mapId = iter->second.mapId();
transformToPoseMsg(iter->second.getPose(), msg->nodes[i].pose);
compressedMatToBytes(iter->second.getImageCompressed(), msg->nodes[i].image.bytes);
compressedMatToBytes(iter->second.getDepthCompressed(), msg->nodes[i].depth.bytes);
compressedMatToBytes(iter->second.getDepth2DCompressed(), msg->nodes[i].depth2D.bytes);
msg->nodes[i].fx = iter->second.getDepthFx();
msg->nodes[i].fy = iter->second.getDepthFy();
msg->nodes[i].cx = iter->second.getDepthCx();
msg->nodes[i].cy = iter->second.getDepthCy();
transformToGeometryMsg(iter->second.getLocalTransform(), msg->nodes[i].localTransform);
//Features stuff...
msg->nodes[i].wordsKeys = uKeys(iter->second.getWords());
msg->nodes[i].wordsValues.resize(iter->second.getWords().size());
int j = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator jter=iter->second.getWords().begin();
jter!=iter->second.getWords().end();
++jter)
{
msg->nodes[i].wordsValues.at(j).angle = jter->second.angle;
msg->nodes[i].wordsValues.at(j).response = jter->second.response;
msg->nodes[i].wordsValues.at(j).ptx = jter->second.pt.x;
msg->nodes[i].wordsValues.at(j).pty = jter->second.pt.y;
msg->nodes[i].wordsValues.at(j).size = jter->second.size;
msg->nodes[i].wordsValues.at(j).octave = jter->second.octave;
msg->nodes[i].wordsValues.at(j).class_id = jter->second.class_id;
++j;
}
if(iter->second.getWords3().size() && iter->second.getWords3().size() == iter->second.getWords().size())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.resize(iter->second.getWords3().size());
j = 0;
for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=iter->second.getWords3().begin();
jter!=iter->second.getWords3().end();
++jter)
{
cloud[j++] = jter->second;
}
pcl::toROSMsg(cloud, msg->nodes[i].words3DValues);
}
else if(iter->second.getWords3().size())
{
ROS_ERROR("Words 2D and words 3D must have the same size (%d vs %d)!",
(int)iter->second.getWords().size(),
(int)iter->second.getWords3().size());
}
++i;
mapGraph_.publish(graphMsg);
}
mapData_.publish(msg);
}
else
{
ROS_INFO("rtabmap: not publishing the map because there are no subscribers to MapData...");
}
return true;
}
@@ -1295,23 +1175,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
msg->loopClosureId = stats.loopClosureId();
msg->localLoopClosureId = stats.localLoopClosureId();
transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform);
infoPub_.publish(msg);
}
if(infoPubEx_.getNumSubscribers())
{
//ROS_INFO("Sending infoEx msg (last_id=%d)...", stat.refImageId());
rtabmap_ros::InfoExPtr msg(new rtabmap_ros::InfoEx);
msg->header.stamp = timeNow;
msg->header.frame_id = mapFrameId_;
msg->refId = stats.refImageId();
msg->loopClosureId = stats.loopClosureId();
msg->localLoopClosureId = stats.localLoopClosureId();
transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform);
rtabmap_ros::transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform);
// Detailed info
if(stats.extended())
@@ -1330,100 +1194,46 @@ void CoreWrapper::publishStats(const Statistics & stats)
msg->statsKeys = uKeys(stats.data());
msg->statsValues = uValues(stats.data());
}
infoPubEx_.publish(msg);
infoPub_.publish(msg);
}
if(mapData_.getNumSubscribers())
if(mapData_.getNumSubscribers() || mapGraph_.getNumSubscribers())
{
//RGB-D SLAM data
rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
msg->header.stamp = timeNow;
msg->header.frame_id = mapFrameId_;
transformToGeometryMsg(stats.mapCorrection(), msg->mapToOdom);
msg->mapIDs = uKeys(stats.getMapIds());
msg->maps = uValues(stats.getMapIds());
msg->poseIDs.resize(stats.poses().size());
msg->poses.resize(stats.poses().size());
int index = 0;
for(std::map<int, Transform>::const_iterator iter = stats.poses().begin();
iter!=stats.poses().end();
++iter)
if(stats.poses().size() == 0 || stats.poses().size() == stats.getMapIds().size())
{
msg->poseIDs[index] = iter->first;
transformToPoseMsg(iter->second, msg->poses[index]);
++index;
}
rtabmap_ros::GraphPtr graphMsg(new rtabmap_ros::Graph);
graphMsg->header.stamp = timeNow;
graphMsg->header.frame_id = mapFrameId_;
msg->constraintFromIDs.resize(stats.constraints().size());
msg->constraintToIDs.resize(stats.constraints().size());
msg->constraintTypes.resize(stats.constraints().size());
msg->constraints.resize(stats.constraints().size());
index=0;
for(std::multimap<int, Link>::const_iterator iter = stats.constraints().begin(); iter!=stats.constraints().end(); ++iter)
{
msg->constraintFromIDs[index] = iter->first;
msg->constraintToIDs[index] = iter->second.to();
msg->constraintTypes[index] = iter->second.type();
transformToGeometryMsg(iter->second.transform(), msg->constraints[index]);
++index;
}
rtabmap_ros::mapGraphToROS(
stats.poses(),
stats.getMapIds(),
stats.constraints(),
stats.mapCorrection(),
*graphMsg);
// add data
msg->nodes.resize(1);
msg->nodes[0].id = stats.getSignature().id();
msg->nodes[0].mapId = stats.getSignature().mapId();
transformToPoseMsg(stats.getSignature().getPose(), msg->nodes[0].pose);
compressedMatToBytes(stats.getSignature().getImageCompressed(), msg->nodes[0].image.bytes);
compressedMatToBytes(stats.getSignature().getDepthCompressed(), msg->nodes[0].depth.bytes);
compressedMatToBytes(stats.getSignature().getDepth2DCompressed(), msg->nodes[0].depth2D.bytes);
msg->nodes[0].fx = stats.getSignature().getDepthFx();
msg->nodes[0].fy = stats.getSignature().getDepthFy();
msg->nodes[0].cx = stats.getSignature().getDepthCx();
msg->nodes[0].cy = stats.getSignature().getDepthCy();
transformToGeometryMsg(stats.getSignature().getLocalTransform(), msg->nodes[0].localTransform);
//Features stuff...
msg->nodes[0].wordsKeys = uKeys(stats.getSignature().getWords());
msg->nodes[0].wordsValues.resize(stats.getSignature().getWords().size());
index = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator jter=stats.getSignature().getWords().begin();
jter!=stats.getSignature().getWords().end();
++jter)
{
msg->nodes[0].wordsValues.at(index).angle = jter->second.angle;
msg->nodes[0].wordsValues.at(index).response = jter->second.response;
msg->nodes[0].wordsValues.at(index).ptx = jter->second.pt.x;
msg->nodes[0].wordsValues.at(index).pty = jter->second.pt.y;
msg->nodes[0].wordsValues.at(index).size = jter->second.size;
msg->nodes[0].wordsValues.at(index).octave = jter->second.octave;
msg->nodes[0].wordsValues.at(index).class_id = jter->second.class_id;
++index;
}
if(stats.getSignature().getWords3().size() && stats.getSignature().getWords3().size() == stats.getSignature().getWords().size())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.resize(stats.getSignature().getWords3().size());
index = 0;
for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=stats.getSignature().getWords3().begin();
jter!=stats.getSignature().getWords3().end();
++jter)
if(mapData_.getNumSubscribers())
{
cloud[index++] = jter->second;
}
pcl::toROSMsg(cloud, msg->nodes[0].words3DValues);
}
else if(stats.getSignature().getWords3().size())
{
ROS_ERROR("Words 2D and words 3D must have the same size (%d vs %d)!",
(int)stats.getSignature().getWords().size(),
(int)stats.getSignature().getWords3().size());
}
//RGB-D SLAM data
rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
msg->header = graphMsg->header;
msg->graph = *graphMsg;
mapData_.publish(msg);
msg->nodes.resize(1);
rtabmap_ros::nodeDataToROS(stats.getSignature(), msg->nodes[0]);
mapData_.publish(msg);
}
if(mapGraph_.getNumSubscribers())
{
mapGraph_.publish(graphMsg);
}
}
else
{
ROS_ERROR("Poses and map ids are not the same size!? %d vs %d", (int)stats.poses().size(), (int)stats.getMapIds().size());
}
}
}
src/CoreWrapper.h
+2 -1
View File
@@ -96,6 +96,7 @@ private:
const cv::Mat & image,
const rtabmap::Transform & odom = rtabmap::Transform(),
const std::string & odomFrameId = "",
float odomVariance = 1.0f,
const cv::Mat & depthOrRightImage = cv::Mat(),
float fx = 0.0f,
float fyOrBaseline = 0.0f,
@@ -136,8 +137,8 @@ private:
boost::mutex mapToOdomMutex_;
ros::Publisher infoPub_;
ros::Publisher infoPubEx_;
ros::Publisher mapData_;
ros::Publisher mapGraph_;
// for loop closure detection only
image_transport::Subscriber defaultSub_;
src/DataRecorderNode.cpp
+26 -107
View File
@@ -73,7 +73,6 @@ public:
waitForTransform_(false),
depthScanSync_(0),
depthSync_(0),
scanSync_(0),
depthImageSync_(0)
{
ros::NodeHandle pnh("~");
@@ -106,8 +105,6 @@ public:
delete depthScanSync_;
if(depthSync_)
delete depthSync_;
if(scanSync_)
delete scanSync_;
if(depthImageSync_)
delete depthImageSync_;
}
@@ -185,15 +182,6 @@ private:
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
depthSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthCallback, this, _1, _2, _3, _4));
}
else if(subscribeOdom && !subscribeDepth && subscribeLaserScan)
{
ROS_INFO("Registering LaserScan callback...");
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
odomSub_.subscribe(nh, "odom", 1);
scanSub_.subscribe(nh, "scan", 1);
scanSync_ = new message_filters::Synchronizer<MyScanSyncPolicy>(MyScanSyncPolicy(queueSize), imageSub_, odomSub_, scanSub_);
scanSync_->registerCallback(boost::bind(&DataRecorderWrapper::scanCallback, this, _1, _2, _3));
}
else if(!subscribeOdom && subscribeDepth)
{
ROS_INFO("Registering to depth without odometry callback...");
@@ -205,7 +193,11 @@ private:
}
else
{
ROS_INFO("Registering default callback...");
if(subscribeOdom && !subscribeDepth && subscribeLaserScan)
{
ROS_WARN("Cannot record only laser scan without depth images...");
}
ROS_INFO("Registering default callback (\"image\" only)...");
defaultSub_ = rgb_it.subscribe("image", 1, &DataRecorderWrapper::defaultCallback, this);
}
}
@@ -214,16 +206,7 @@ private:
void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
{
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
rtabmap::SensorData data(
ptrImage->image.clone(),
cv::Mat(),
cv::Mat(),
0.0f,
0.0f,
0.0f,
0.0f,
Transform(),
Transform());
rtabmap::SensorData data(ptrImage->image.clone());
recorder_.addData(data);
}
@@ -247,7 +230,7 @@ private:
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
@@ -294,13 +277,13 @@ private:
rtabmap::SensorData data(
ptrImage->image.clone(),
depth16,
cv::Mat(),
fx,
fy,
cx,
cy,
localTransform,
Transform(),
localTransform);
1.0f);
recorder_.addData(data);
}
@@ -325,7 +308,7 @@ private:
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
@@ -333,8 +316,6 @@ private:
return;
}
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
@@ -374,69 +355,16 @@ private:
rtabmap::SensorData data(
ptrImage->image.clone(),
depth16,
cv::Mat(),
fx,
fy,
cx,
cy,
odom,
localTransform);
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0]>0?odomMsg->pose.covariance[0]:1.0f);
recorder_.addData(data);
}
void scanCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
// TF ready?
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), scanMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
//transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ> pclScan;
pcl::fromROSMsg(scanOut, pclScan);
cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
rtabmap::SensorData data(
ptrImage->image.clone(),
cv::Mat(),
scan,
0.0f,
0.0f,
0.0f,
0.0f,
odom,
Transform());
recorder_.addData(data);
}
void depthScanCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
@@ -465,7 +393,7 @@ private:
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
@@ -479,9 +407,7 @@ private:
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ> pclScan;
pcl::fromROSMsg(scanOut, pclScan);
cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
cv::Mat scan = util3d::laserScanFromPointCloud(pclScan);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
@@ -520,15 +446,16 @@ private:
}
rtabmap::SensorData data(
scan,
ptrImage->image.clone(),
depth16,
scan,
fx,
fy,
cx,
cy,
odom,
localTransform);
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0]>0?odomMsg->pose.covariance[0]:1.0f);
recorder_.addData(data);
}
@@ -554,7 +481,7 @@ private:
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
@@ -562,8 +489,6 @@ private:
return;
}
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
@@ -577,13 +502,13 @@ private:
rtabmap::SensorData data(
ptrLeftImage->image.clone(),
ptrRightImage->image.clone(),
cv::Mat(),
fx,
baseline,
cx,
cy,
odom,
localTransform);
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0]>0?odomMsg->pose.covariance[0]:1.0f);
recorder_.addData(data);
}
@@ -608,7 +533,7 @@ private:
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
@@ -629,13 +554,13 @@ private:
rtabmap::SensorData data(
ptrLeftImage->image.clone(),
ptrRightImage->image.clone(),
cv::Mat(),
fx,
baseline,
cx,
cy,
localTransform,
Transform(),
localTransform);
1.0f);
recorder_.addData(data);
}
@@ -669,12 +594,6 @@ private:
sensor_msgs::CameraInfo> MyDepthSyncPolicy;
message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
nav_msgs::Odometry,
sensor_msgs::LaserScan> MyScanSyncPolicy;
message_filters::Synchronizer<MyScanSyncPolicy> * scanSync_;
//without odom
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
src/DbPlayerNode.cpp
+46 -52
View File
@@ -134,17 +134,11 @@ int main(int argc, char** argv)
ros::Publisher rightCamInfoPub;
ros::Publisher odometryPub;
tf::TransformBroadcaster tfBroadcaster;
ros::Publisher cloudPub;
cv::Mat image, depth, depth2d;
float fx,fy,cx,cy;
rtabmap::Transform localTransform, pose;
int seq = 0;
reader.getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose, seq);
while(ros::ok() && !image.empty())
rtabmap::SensorData data = reader.getNextData();
while(ros::ok() && !data.isValid())
{
ROS_INFO("Reading image %d...", seq);
ROS_INFO("Reading sensor data %d...", data.id());
ros::Time time = ros::Time::now();
@@ -164,19 +158,19 @@ int main(int argc, char** argv)
camInfoB = camInfoA;
int type = -1;
if(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1))
if(!data.depth().empty() && (data.depth().type() == CV_32FC1 || data.depth().type() == CV_16UC1))
{
//depth
camInfoA.D.resize(5,0);
camInfoA.P[0] = fx;
camInfoA.K[0] = fx;
camInfoA.P[5] = fy;
camInfoA.K[4] = fy;
camInfoA.P[2] = cx;
camInfoA.K[2] = cx;
camInfoA.P[6] = cy;
camInfoA.K[5] = cy;
camInfoA.P[0] = data.fx();
camInfoA.K[0] = data.fx();
camInfoA.P[5] = data.fy();
camInfoA.K[4] = data.fy();
camInfoA.P[2] = data.cx();
camInfoA.K[2] = data.cx();
camInfoA.P[6] = data.cy();
camInfoA.K[5] = data.cy();
camInfoB = camInfoA;
@@ -187,22 +181,22 @@ int main(int argc, char** argv)
if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
}
else if(!depth.empty() && depth.type() == CV_8U)
else if(!data.rightImage().empty() && data.rightImage().type() == CV_8U)
{
//stereo
camInfoA.D.resize(8,0);
camInfoA.P[0] = fx;
camInfoA.K[0] = fx;
camInfoA.P[5] = fx; // fx = fy
camInfoA.K[4] = fx; // fx = fy
camInfoA.P[2] = cx;
camInfoA.K[2] = cx;
camInfoA.P[6] = cy;
camInfoA.K[5] = cy;
camInfoA.P[0] = data.fx();
camInfoA.K[0] = data.fx();
camInfoA.P[5] = data.fx(); // fx = fy
camInfoA.K[4] = data.fx(); // fx = fy
camInfoA.P[2] = data.cx();
camInfoA.K[2] = data.cx();
camInfoA.P[6] = data.cy();
camInfoA.K[5] = data.cy();
camInfoB = camInfoA;
camInfoB.P[3] = fy*-fx; // Right_Tx = -baseline*fx
camInfoB.P[3] = data.baseline()*-data.fx(); // Right_Tx = -baseline*fx
type=1;
@@ -217,30 +211,30 @@ int main(int argc, char** argv)
if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
}
camInfoA.height = image.rows;
camInfoA.width = image.cols;
camInfoB.height = depth.rows;
camInfoB.width = depth.cols;
camInfoA.height = data.image().rows;
camInfoA.width = data.image().cols;
camInfoB.height = data.depthOrRightImage().rows;
camInfoB.width = data.depthOrRightImage().cols;
// publish transforms first
if(publishTf)
{
ros::Time tfExpiration = time + ros::Duration(1.0/rate);
if(!localTransform.isNull())
if(!data.localTransform().isNull())
{
tf::Transform baseToCamera;
rtabmap::transformToTF(localTransform, baseToCamera);
rtabmap_ros::transformToTF(data.localTransform(), baseToCamera);
tfBroadcaster.sendTransform( tf::StampedTransform (baseToCamera, tfExpiration, frameId, cameraFrameId));
}
if(!pose.isNull())
if(!data.pose().isNull())
{
tf::Transform odomToBase;
rtabmap::transformToTF(pose, odomToBase);
rtabmap_ros::transformToTF(data.pose(), odomToBase);
tfBroadcaster.sendTransform( tf::StampedTransform (odomToBase, tfExpiration, odomFrameId, frameId));
}
}
if(!pose.isNull())
if(!data.pose().isNull())
{
if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
@@ -250,7 +244,13 @@ int main(int argc, char** argv)
odom.child_frame_id = frameId;
odom.header.frame_id = odomFrameId;
odom.header.stamp = time;
rtabmap::transformToPoseMsg(pose, odom.pose.pose);
rtabmap_ros::transformToPoseMsg(data.pose(), odom.pose.pose);
odom.pose.covariance[0] = data.poseVariance();
odom.pose.covariance[7] = data.poseVariance();
odom.pose.covariance[14] = data.poseVariance();
odom.pose.covariance[21] = data.poseVariance();
odom.pose.covariance[28] = data.poseVariance();
odom.pose.covariance[35] = data.poseVariance();
odometryPub.publish(odom);
}
}
@@ -278,7 +278,7 @@ int main(int argc, char** argv)
if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
{
cv_bridge::CvImage img;
if(image.channels() == 1)
if(data.image().channels() == 1)
{
img.encoding = sensor_msgs::image_encodings::MONO8;
}
@@ -286,7 +286,7 @@ int main(int argc, char** argv)
{
img.encoding = sensor_msgs::image_encodings::BGR8;
}
img.image = image;
img.image = data.image();
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
imageRosMsg->header.frame_id = cameraFrameId;
imageRosMsg->header.stamp = time;
@@ -306,10 +306,10 @@ int main(int argc, char** argv)
}
}
if(depthPub.getNumSubscribers() && !depth.empty() && type==0)
if(depthPub.getNumSubscribers() && !data.depth().empty() && type==0)
{
cv_bridge::CvImage img;
if(depth.type() == CV_32FC1)
if(data.depth().type() == CV_32FC1)
{
img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
}
@@ -317,7 +317,7 @@ int main(int argc, char** argv)
{
img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
}
img.image = depth;
img.image = data.depth();
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
imageRosMsg->header.frame_id = cameraFrameId;
imageRosMsg->header.stamp = time;
@@ -326,11 +326,11 @@ int main(int argc, char** argv)
depthCamInfoPub.publish(camInfoB);
}
if(rightPub.getNumSubscribers() && !depth.empty() && type==1)
if(rightPub.getNumSubscribers() && !data.rightImage().empty() && type==1)
{
cv_bridge::CvImage img;
img.encoding = sensor_msgs::image_encodings::MONO8;
img.image = depth;
img.image = data.rightImage();
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
imageRosMsg->header.frame_id = cameraFrameId;
imageRosMsg->header.stamp = time;
@@ -347,13 +347,7 @@ int main(int argc, char** argv)
ros::spinOnce();
}
image = cv::Mat();
depth = cv::Mat();
depth2d = cv::Mat();
fx=fy=cx=cy=seq=0;
pose.setNull();
localTransform.setNull();
reader.getNextImage(image, depth, depth2d, fx, fy, cx, cy, localTransform, pose, seq);
data = reader.getNextData();
}
src/GridMapAssemblerNode.cpp
+5 -5
View File
@@ -78,17 +78,17 @@ public:
{
for(unsigned int i=0; i<msg->nodes.size(); ++i)
{
if(!uContains(scans_, msg->nodes[i].id) && msg->nodes[i].depth2D.bytes.size())
if(!uContains(scans_, msg->nodes[i].id) && msg->nodes[i].laserScan.size())
{
cv::Mat depth2d = util3d::uncompressData(msg->nodes[i].depth2D.bytes);
scans_.insert(std::make_pair(msg->nodes[i].id, util3d::depth2DToPointCloud(depth2d)));
cv::Mat laserScan = util3d::uncompressData(msg->nodes[i].laserScan);
scans_.insert(std::make_pair(msg->nodes[i].id, util3d::laserScanToPointCloud(laserScan)));
}
}
std::map<int, Transform> poses;
for(unsigned int i=0; i<msg->poseIDs.size() && i<msg->poses.size(); ++i)
for(unsigned int i=0; i<msg->graph.nodeIds.size() && i<msg->graph.poses.size(); ++i)
{
poses.insert(std::make_pair(msg->poseIDs[i], transformFromPoseMsg(msg->poses[i])));
poses.insert(std::make_pair(msg->graph.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i])));
}
if(filterRadius_ > 0.0 && filterAngle_ > 0.0)
src/GuiWrapper.cpp
+43 -217
View File
@@ -56,8 +56,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl_conversions/pcl_conversions.h>
#include <laser_geometry/laser_geometry.h>
using namespace rtabmap;
GuiWrapper::GuiWrapper(int & argc, char** argv) :
app_(0),
mainWindow_(0),
@@ -111,9 +109,9 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
UEventsManager::addHandler(this);
UEventsManager::addHandler(mainWindow_);
infoExTopic_.subscribe(nh, "infoEx", 1);
infoTopic_.subscribe(nh, "info", 1);
mapDataTopic_.subscribe(nh, "mapData", 1);
infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(MyInfoMapSyncPolicy(queueSize), infoExTopic_, mapDataTopic_);
infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(MyInfoMapSyncPolicy(queueSize), infoTopic_, mapDataTopic_);
infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, _1, _2));
}
@@ -129,7 +127,7 @@ int GuiWrapper::exec()
}
void GuiWrapper::infoMapCallback(
const rtabmap_ros::InfoExConstPtr & infoMsg,
const rtabmap_ros::InfoConstPtr & infoMsg,
const rtabmap_ros::MapDataConstPtr & mapMsg)
{
//ROS_INFO("rtabmapviz: RTAB-Map info ex received!");
@@ -175,78 +173,26 @@ void GuiWrapper::infoMapCallback(
stat.addStatistic(infoMsg->statsKeys.at(i), infoMsg->statsValues.at(i));
}
stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(infoMsg->loopClosureTransform));
//RGB-D SLAM data
stat.setMapCorrection(transformFromGeometryMsg(mapMsg->mapToOdom));
stat.setLoopClosureTransform(transformFromGeometryMsg(infoMsg->loopClosureTransform));
Transform mapToOdom;
std::map<int, Transform> poses;
for(unsigned int i=0; i<mapMsg->poseIDs.size() && i<mapMsg->poses.size(); ++i)
{
poses.insert(std::make_pair(mapMsg->poseIDs[i], transformFromPoseMsg(mapMsg->poses[i])));
}
stat.setPoses(poses);
std::multimap<int, Link> constraints;
for(unsigned int i=0; i<mapMsg->constraintFromIDs.size() && i<mapMsg->constraintToIDs.size() && i<mapMsg->constraintTypes.size() && i < mapMsg->constraints.size(); ++i)
{
Transform t = transformFromGeometryMsg(mapMsg->constraints[i]);
constraints.insert(std::make_pair(mapMsg->constraintFromIDs[i], Link(mapMsg->constraintFromIDs[i], mapMsg->constraintToIDs[i], t, (Link::Type)mapMsg->constraintTypes[i])));
}
stat.setConstraints(constraints);
std::map<int, int> mapIds;
for(unsigned int i=0; i<mapMsg->mapIDs.size() && i<mapMsg->maps.size(); ++i)
{
mapIds.insert(std::make_pair(mapMsg->mapIDs[i], mapMsg->maps[i]));
}
std::multimap<int, Link> links;
rtabmap_ros::mapGraphFromROS(mapMsg->graph, poses, mapIds, links, mapToOdom);
stat.setMapCorrection(mapToOdom);
stat.setPoses(poses);
stat.setMapIds(mapIds);
stat.setConstraints(links);
//data
if(mapMsg->nodes.size() == 1)
{
//Features stuff...
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, pcl::PointXYZ> words3D;
pcl::PointCloud<pcl::PointXYZ> cloud;
if(mapMsg->nodes[0].words3DValues.data.size())
{
pcl::fromROSMsg(mapMsg->nodes[0].words3DValues, cloud);
}
for(unsigned int i=0; i<mapMsg->nodes[0].wordsKeys.size() && i<mapMsg->nodes[0].wordsValues.size(); ++i)
{
cv::KeyPoint pt;
pt.angle = mapMsg->nodes[0].wordsValues.at(i).angle;
pt.response = mapMsg->nodes[0].wordsValues.at(i).response;
pt.pt.x = mapMsg->nodes[0].wordsValues.at(i).ptx;
pt.pt.y = mapMsg->nodes[0].wordsValues.at(i).pty;
pt.size = mapMsg->nodes[0].wordsValues.at(i).size;
int wordId = mapMsg->nodes[0].wordsKeys.at(i);
words.insert(std::make_pair(wordId, pt));
if(i< cloud.size())
{
words3D.insert(std::make_pair(wordId, cloud[i]));
}
}
if(words3D.size() && words3D.size() != words.size())
{
ROS_ERROR("Words 2D and 3D should be the same size (%d, %d)!", (int)words.size(), (int)words3D.size());
}
Signature signature(mapMsg->nodes[0].id,
mapMsg->nodes[0].mapId,
words,
words3D,
transformFromPoseMsg(mapMsg->nodes[0].pose),
compressedMatFromBytes(mapMsg->nodes[0].depth2D.bytes),
compressedMatFromBytes(mapMsg->nodes[0].image.bytes),
compressedMatFromBytes(mapMsg->nodes[0].depth.bytes),
mapMsg->nodes[0].fx,
mapMsg->nodes[0].fy,
mapMsg->nodes[0].cx,
mapMsg->nodes[0].cy,
transformFromGeometryMsg(mapMsg->nodes[0].localTransform));
stat.setSignature(signature);
stat.setSignature(rtabmap_ros::nodeDataFromROS(mapMsg->nodes[0]));
}
else if(mapMsg->nodes.size() > 1)
{
@@ -260,92 +206,30 @@ void GuiWrapper::processRequestedMap(const rtabmap_ros::MapData & map)
{
std::map<int, Signature> signatures;
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
std::multimap<int, rtabmap::Link> constraints;
std::map<int, int> mapIds;
Transform mapToOdom;
if(map.mapIDs.size() != map.maps.size())
if(map.graph.nodeIds.size() != map.graph.mapIds.size())
{
ROS_WARN("rtabmapviz: receiving map... maps and IDs are not the same size (%d vs %d)!",
(int)map.maps.size(), (int)map.mapIDs.size());
ROS_ERROR("rtabmapviz: receiving map... node and amp IDs are not the same size (%d vs %d)!",
(int)map.graph.nodeIds.size(), (int)map.graph.mapIds.size());
return;
}
if(map.poseIDs.size() != map.poses.size())
if(map.graph.poses.size() && map.graph.nodeIds.size() != map.graph.poses.size())
{
ROS_WARN("rtabmapviz: receiving map... poses and IDs are not the same size (%d vs %d)!",
(int)map.poses.size(), (int)map.poseIDs.size());
ROS_ERROR("rtabmapviz: receiving map... poses and node IDs are not the same size (%d vs %d)!",
(int)map.graph.poses.size(), (int)map.graph.nodeIds.size());
return;
}
if(map.constraintFromIDs.size() != map.constraints.size() ||
map.constraintToIDs.size() != map.constraints.size() ||
map.constraintTypes.size() != map.constraints.size())
{
ROS_WARN("rtabmapviz: receiving map... constraints and IDs are not the same size (%d vs %d vs %d vs %d)!",
(int)map.constraints.size(), (int)map.constraintFromIDs.size(), (int)map.constraintToIDs.size(), (int)map.constraintTypes.size());
}
for(unsigned int i=0; i<map.mapIDs.size() && i < map.maps.size(); ++i)
{
mapIds.insert(std::make_pair(map.mapIDs[i], map.maps[i]));
}
for(unsigned int i=0; i<map.poseIDs.size() && i < map.poses.size(); ++i)
{
Transform t = transformFromPoseMsg(map.poses[i]);
poses.insert(std::make_pair(map.poseIDs[i], t));
}
for(unsigned int i=0; i<map.constraintFromIDs.size() && i<map.constraintToIDs.size() && i<map.constraintTypes.size() && i < map.constraints.size(); ++i)
{
Transform t = transformFromGeometryMsg(map.constraints[i]);
constraints.insert(std::make_pair(map.constraintFromIDs[i], Link(map.constraintFromIDs[i], map.constraintToIDs[i], t, (Link::Type)map.constraintTypes[i])));
}
rtabmap_ros::mapGraphFromROS(map.graph, poses, mapIds, constraints, mapToOdom);
//data
for(unsigned int i=0; i<map.nodes.size(); ++i)
{
//Features stuff...
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, pcl::PointXYZ> words3D;
pcl::PointCloud<pcl::PointXYZ> cloud;
if(map.nodes[i].words3DValues.data.size())
{
pcl::fromROSMsg(map.nodes[i].words3DValues, cloud);
}
for(unsigned int j=0; j<map.nodes[i].wordsKeys.size() && j<map.nodes[0].wordsValues.size(); ++j)
{
cv::KeyPoint pt;
pt.angle = map.nodes[i].wordsValues.at(j).angle;
pt.response = map.nodes[i].wordsValues.at(j).response;
pt.pt.x = map.nodes[i].wordsValues.at(j).ptx;
pt.pt.y = map.nodes[i].wordsValues.at(j).pty;
pt.size = map.nodes[i].wordsValues.at(j).size;
int wordId = map.nodes[i].wordsKeys.at(j);
words.insert(std::make_pair(wordId, pt));
if(j < cloud.size())
{
words3D.insert(std::make_pair(wordId, cloud[j]));
}
}
if(words3D.size() && words3D.size() != words.size())
{
ROS_ERROR("Words 2D and 3D should be the same size (%d, %d)!", (int)words.size(), (int)words3D.size());
}
signatures.insert(std::make_pair(map.nodes[i].id,
Signature(map.nodes[i].id,
map.nodes[i].mapId,
words,
words3D,
transformFromPoseMsg(map.nodes[i].pose),
compressedMatFromBytes(map.nodes[i].depth2D.bytes),
compressedMatFromBytes(map.nodes[i].image.bytes),
compressedMatFromBytes(map.nodes[i].depth.bytes),
map.nodes[i].fx,
map.nodes[i].fy,
map.nodes[i].cx,
map.nodes[i].cy,
transformFromGeometryMsg(map.nodes[i].localTransform))));
signatures.insert(std::make_pair(map.nodes[i].id, rtabmap_ros::nodeDataFromROS(map.nodes[i])));
}
this->post(new RtabmapEvent3DMap(signatures,
@@ -479,9 +363,9 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
{
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
rtabmap::SensorData data;
data.setPose(odom);
data.setPose(odom, odomMsg->pose.covariance[0]);
this->post(new OdometryEvent(data));
}
@@ -506,7 +390,7 @@ void GuiWrapper::depthCallback(
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
@@ -514,8 +398,6 @@ void GuiWrapper::depthCallback(
return;
}
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
@@ -533,59 +415,9 @@ void GuiWrapper::depthCallback(
fy,
cx,
cy,
odom,
localTransform);
this->post(new OdometryEvent(image));
}
void GuiWrapper::scanCallback(
const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg)
{
// TF ready?
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), scanMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
//transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ> pclScan;
pcl::fromROSMsg(scanOut, pclScan);
cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
rtabmap::SensorData image(
ptrImage->image.clone(),
cv::Mat(),
scan,
0.0f,
0.0f,
0.0f,
0.0f,
odom,
Transform());
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0]);
this->post(new OdometryEvent(image));
}
@@ -616,7 +448,7 @@ void GuiWrapper::depthScanCallback(
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
@@ -626,9 +458,7 @@ void GuiWrapper::depthScanCallback(
pcl::PointCloud<pcl::PointXYZ> pclScan;
pcl::fromROSMsg(scanOut, pclScan);
cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
cv::Mat scan = util3d::laserScanFromPointCloud(pclScan);
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
@@ -641,15 +471,16 @@ void GuiWrapper::depthScanCallback(
float cy = model.cy();
rtabmap::SensorData image(
scan,
ptrImage->image.clone(),
ptrDepth->image.clone(),
scan,
fx,
fy,
cx,
cy,
odom,
localTransform);
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0]);
this->post(new OdometryEvent(image));
}
@@ -690,18 +521,13 @@ void GuiWrapper::setupCallbacks(
depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
depthSync_->registerCallback(boost::bind(&GuiWrapper::depthCallback, this, _1, _2, _3, _4));
}
else if(!subscribeDepth && subscribeLaserScan)
{
ROS_INFO("Registering LaserScan callback...");
imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
odomSub_.subscribe(nh, "odom", 1);
scanSub_.subscribe(nh, "scan", 1);
scanSync_ = new message_filters::Synchronizer<MyScanSyncPolicy>(MyScanSyncPolicy(queueSize), imageSub_, odomSub_, scanSub_);
scanSync_->registerCallback(boost::bind(&GuiWrapper::scanCallback, this, _1, _2, _3));
}
else // default odom only
{
ROS_INFO("Registering default callback...");
if(!subscribeDepth && subscribeLaserScan)
{
ROS_WARN("Cannot subscribe to laser scan without depth subscription...");
}
ROS_INFO("Registering default callback (\"odom\" only)...");
defaultSub_ = nh.subscribe("odom", 1, &GuiWrapper::defaultCallback, this);
}
}
src/GuiWrapper.h
+4 -13
View File
@@ -29,7 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define GUIWRAPPER_H_
#include <ros/ros.h>
#include "rtabmap_ros/InfoEx.h"
#include "rtabmap_ros/Info.h"
#include "rtabmap_ros/MapData.h"
#include "rtabmap/utilite/UEventsHandler.h"
@@ -69,7 +69,7 @@ protected:
virtual void handleEvent(UEvent * anEvent);
private:
void infoMapCallback(const rtabmap_ros::InfoExConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);
void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);
void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, int queueSize);
void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); // odom
@@ -77,9 +77,6 @@ private:
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
void scanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg);
void depthScanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
@@ -98,7 +95,7 @@ private:
bool waitForTransform_;
tf::TransformListener tfListener_;
message_filters::Subscriber<rtabmap_ros::InfoEx> infoExTopic_;
message_filters::Subscriber<rtabmap_ros::Info> infoTopic_;
message_filters::Subscriber<rtabmap_ros::MapData> mapDataTopic_;
ros::Subscriber defaultSub_; // odometry only
@@ -109,7 +106,7 @@ private:
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
typedef message_filters::sync_policies::ExactTime<
rtabmap_ros::InfoEx,
rtabmap_ros::Info,
rtabmap_ros::MapData> MyInfoMapSyncPolicy;
message_filters::Synchronizer<MyInfoMapSyncPolicy> * infoMapSync_;
@@ -127,12 +124,6 @@ private:
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthSyncPolicy;
message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
nav_msgs::Odometry,
sensor_msgs::LaserScan> MyScanSyncPolicy;
message_filters::Synchronizer<MyScanSyncPolicy> * scanSync_;
};
#endif /* GUIWRAPPER_H_ */
src/MapAssemblerNode.cpp
+48 -27
View File
@@ -49,6 +49,8 @@ public:
scanVoxelSize_(0.01),
nodeFilteringAngle_(30), // degrees
nodeFilteringRadius_(0.5),
noiseFilterRadius_(0.0),
noiseFilterMinNeighbors_(5),
computeOccupancyGrid_(false),
gridCellSize_(0.05),
groundMaxAngle_(M_PI_4),
@@ -66,6 +68,9 @@ public:
pnh.param("filter_radius", nodeFilteringRadius_, nodeFilteringRadius_);
pnh.param("filter_angle", nodeFilteringAngle_, nodeFilteringAngle_);
pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
pnh.param("occupancy_grid", computeOccupancyGrid_, computeOccupancyGrid_);
pnh.param("occupancy_cell_size", gridCellSize_, gridCellSize_);
pnh.param("occupancy_ground_max_angle", groundMaxAngle_, groundMaxAngle_);
@@ -104,7 +109,7 @@ public:
int id = msg->nodes[i].id;
if(!uContains(rgbClouds_, id))
{
rtabmap::Transform localTransform = transformFromGeometryMsg(msg->nodes[i].localTransform);
rtabmap::Transform localTransform = rtabmap_ros::transformFromGeometryMsg(msg->nodes[i].localTransform);
if(!localTransform.isNull())
{
cv::Mat image, depth;
@@ -114,8 +119,8 @@ public:
float cy = msg->nodes[i].cy;
//uncompress data
util3d::CompressionThread ctImage(compressedMatFromBytes(msg->nodes[i].image.bytes, false), true);
util3d::CompressionThread ctDepth(compressedMatFromBytes(msg->nodes[i].depth.bytes, false), true);
util3d::CompressionThread ctImage(rtabmap_ros::compressedMatFromBytes(msg->nodes[i].image, false), true);
util3d::CompressionThread ctDepth(rtabmap_ros::compressedMatFromBytes(msg->nodes[i].depth, false), true);
ctImage.start();
ctDepth.start();
ctImage.join();
@@ -135,57 +140,70 @@ public:
cloud = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloudDecimation_);
}
if(cloudMaxDepth_ > 0)
if(cloud->size() && cloudMaxDepth_ > 0)
{
cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, cloudMaxDepth_);
}
if(cloudVoxelSize_ > 0)
if(cloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering<pcl::PointXYZRGB>(cloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *indices, *tmp);
cloud = tmp;
}
if(cloud->size() && cloudVoxelSize_ > 0)
{
cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, cloudVoxelSize_);
}
cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
rgbClouds_.insert(std::make_pair(id, cloud));
if(computeOccupancyGrid_)
if(cloud->size())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloud;
if(maxHeight_ > 0)
cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
rgbClouds_.insert(std::make_pair(id, cloud));
if(computeOccupancyGrid_)
{
cloudClipped = util3d::passThrough<pcl::PointXYZRGB>(cloudClipped, "z", std::numeric_limits<int>::min(), maxHeight_);
}
cv::Mat ground, obstacles;
if(util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_))
{
occupancyLocalMaps_.insert(std::make_pair(id, std::make_pair(ground, obstacles)));
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloud;
if(maxHeight_ > 0)
{
cloudClipped = util3d::passThrough<pcl::PointXYZRGB>(cloudClipped, "z", std::numeric_limits<int>::min(), maxHeight_);
}
cv::Mat ground, obstacles;
if(util3d::occupancy2DFromCloud3D(cloudClipped, ground, obstacles, gridCellSize_, groundMaxAngle_, clusterMinSize_))
{
occupancyLocalMaps_.insert(std::make_pair(id, std::make_pair(ground, obstacles)));
}
}
}
}
}
}
if(!uContains(scans_, id) && msg->nodes[i].depth2D.bytes.size())
if(!uContains(scans_, id) && msg->nodes[i].laserScan.size())
{
cv::Mat depth2d = util3d::uncompressData(msg->nodes[i].depth2D.bytes);
if(!depth2d.empty())
cv::Mat laserScan = util3d::uncompressData(msg->nodes[i].laserScan);
if(!laserScan.empty())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::depth2DToPointCloud(depth2d);
if(scanVoxelSize_ > 0)
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
if(cloud->size() && scanVoxelSize_ > 0)
{
cloud = util3d::voxelize<pcl::PointXYZ>(cloud, scanVoxelSize_);
}
scans_.insert(std::make_pair(id, cloud));
if(cloud->size())
{
scans_.insert(std::make_pair(id, cloud));
}
}
}
}
// filter poses
std::map<int, Transform> poses;
for(unsigned int i=0; i<msg->poseIDs.size() && i<msg->poses.size(); ++i)
for(unsigned int i=0; i<msg->graph.nodeIds.size() && i<msg->graph.poses.size(); ++i)
{
poses.insert(std::make_pair(msg->poseIDs[i], transformFromPoseMsg(msg->poses[i])));
poses.insert(std::make_pair(msg->graph.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i])));
}
if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0)
{
@@ -305,6 +323,9 @@ private:
double nodeFilteringAngle_;
double nodeFilteringRadius_;
double noiseFilterRadius_;
double noiseFilterMinNeighbors_;
bool computeOccupancyGrid_;
double gridCellSize_;
double groundMaxAngle_;
src/MapOptimizerNode.cpp
+19 -31
View File
@@ -48,6 +48,7 @@ public:
mapFrameId_("map"),
odomFrameId_("odom"),
iterations_(100),
ignoreVariance_(false),
globalOptimization_(true),
optimizeFromLastNode_(false),
mapToOdom_(tf::Transform::getIdentity()),
@@ -59,6 +60,7 @@ public:
pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("iterations", iterations_, iterations_);
pnh.param("ignore_variance", ignoreVariance_, ignoreVariance_);
pnh.param("global_optimization", globalOptimization_, globalOptimization_);
pnh.param("optimize_from_last_node", optimizeFromLastNode_, optimizeFromLastNode_);
@@ -110,19 +112,15 @@ public:
{
// save new poses and constraints
// Assuming that nodes/constraints are all linked together
UASSERT(msg->poseIDs.size() == msg->poses.size());
UASSERT(msg->mapIDs.size() == msg->poseIDs.size());
UASSERT(msg->mapIDs.size() == msg->maps.size());
UASSERT(msg->constraints.size() == msg->constraintFromIDs.size() &&
msg->constraints.size() == msg->constraintToIDs.size() &&
msg->constraints.size() == msg->constraintTypes.size());
UASSERT(msg->graph.nodeIds.size() == msg->graph.poses.size());
UASSERT(msg->graph.nodeIds.size() == msg->graph.mapIds.size());
bool dataChanged = false;
std::multimap<int, Link> newConstraints;
for(unsigned int i=0; i<msg->constraints.size() && i<msg->constraints.size(); ++i)
for(unsigned int i=0; i<msg->graph.links.size(); ++i)
{
Link link(msg->constraintFromIDs[i], msg->constraintToIDs[i], transformFromGeometryMsg(msg->constraints[i]), (Link::Type)msg->constraintTypes[i]);
Link link = rtabmap_ros::linkFromROS(msg->graph.links[i]);
newConstraints.insert(std::make_pair(link.from(), link));
bool edgeAlreadyAdded = false;
@@ -151,7 +149,7 @@ public:
for(unsigned int i=0; i<msg->nodes.size(); ++i)
{
int id = msg->nodes[i].id;
Transform pose = transformFromPoseMsg(msg->nodes[i].pose);
Transform pose = rtabmap_ros::transformFromPoseMsg(msg->nodes[i].pose);
newPoses.insert(std::make_pair(id, pose));
newMapIds.insert(std::make_pair(id, msg->nodes[i].mapId));
@@ -187,9 +185,9 @@ public:
else
{
constraints = newConstraints;
for(unsigned int i=0; i<msg->poseIDs.size(); ++i)
for(unsigned int i=0; i<msg->graph.nodeIds.size(); ++i)
{
std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->poseIDs[i]);
std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->graph.nodeIds[i]);
if(iter != cachedPoses_.end())
{
poses.insert(*iter);
@@ -197,7 +195,7 @@ public:
}
else
{
ROS_ERROR("Odometry pose of node %d not found in cache!", msg->poseIDs[i]);
ROS_ERROR("Odometry pose of node %d not found in cache!", msg->graph.nodeIds[i]);
return;
}
}
@@ -214,16 +212,16 @@ public:
if(optimizeFromLastNode_)
{
std::map<int, int> depthGraph = util3d::generateDepthGraph(constraints, poses.rbegin()->first);
util3d::optimizeTOROGraph(depthGraph, poses, constraints, optimizedPoses, iterations_);
util3d::optimizeTOROGraph(depthGraph, poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
}
else
{
util3d::optimizeTOROGraph(poses, constraints, optimizedPoses, iterations_);
util3d::optimizeTOROGraph(poses, constraints, optimizedPoses, iterations_, true, ignoreVariance_);
}
mapToOdomMutex_.lock();
mapCorrection = optimizedPoses.at(poses.rbegin()->first) * poses.rbegin()->second.inverse();
rtabmap::transformToTF(mapCorrection, mapToOdom_);
rtabmap_ros::transformToTF(mapCorrection, mapToOdom_);
mapToOdomMutex_.unlock();
}
else if(poses.size() == 1 && constraints.size() == 0)
@@ -239,22 +237,11 @@ public:
}
UASSERT(optimizedPoses.size() == mapIds.size());
rtabmap_ros::MapData outputMsg = *msg;
outputMsg.poseIDs.resize(optimizedPoses.size());
outputMsg.poses.resize(optimizedPoses.size());
outputMsg.mapIDs.resize(mapIds.size());
outputMsg.maps.resize(mapIds.size());
rtabmap::transformToGeometryMsg(mapCorrection, outputMsg.mapToOdom);
int i=0;
std::map<int, int>::iterator jter = mapIds.begin();
for(std::map<int, Transform>::iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter, ++jter)
{
outputMsg.poseIDs[i] = iter->first;
transformToPoseMsg(iter->second, outputMsg.poses[i]);
outputMsg.mapIDs[i] = jter->first;
outputMsg.maps[i] = jter->second;
++i;
}
rtabmap_ros::MapData outputMsg;
rtabmap_ros::mapGraphToROS(optimizedPoses, mapIds, std::multimap<int, rtabmap::Link>(), mapCorrection, outputMsg.graph);
outputMsg.graph.links = msg->graph.links;
outputMsg.header = msg->header;
outputMsg.nodes = msg->nodes;
mapDataPub_.publish(outputMsg);
ROS_INFO("Time graph optimization = %f s", timer.ticks());
@@ -265,6 +252,7 @@ private:
std::string mapFrameId_;
std::string odomFrameId_;
int iterations_;
bool ignoreVariance_;
bool globalOptimization_;
bool optimizeFromLastNode_;
src/MsgConversion.cpp
+209 -3
View File
@@ -31,15 +31,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <zlib.h>
#include <ros/ros.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/utilite/UStl.h>
#include <tf_conversions/tf_eigen.h>
#include <pcl_conversions/pcl_conversions.h>
namespace rtabmap {
namespace rtabmap_ros {
void transformToTF(const rtabmap::Transform & transform, tf::Transform & tfTransform)
{
if(!transform.isNull())
{
tf::transformEigenToTF(util3d::transformToEigen3d(transform), tfTransform);
tf::transformEigenToTF(rtabmap::util3d::transformToEigen3d(transform), tfTransform);
}
else
{
@@ -51,7 +53,7 @@ rtabmap::Transform transformFromTF(const tf::Transform & transform)
{
Eigen::Affine3d eigenTf;
tf::transformTFToEigen(transform, eigenTf);
return util3d::transformFromEigen3d(eigenTf);
return rtabmap::util3d::transformFromEigen3d(eigenTf);
}
void transformToGeometryMsg(const rtabmap::Transform & transform, geometry_msgs::Transform & msg)
@@ -122,4 +124,208 @@ cv::Mat compressedMatFromBytes(const std::vector<unsigned char> & bytes, bool co
return out;
}
rtabmap::Link linkFromROS(const rtabmap_ros::Link & msg)
{
return rtabmap::Link(msg.fromId, msg.toId, (rtabmap::Link::Type)msg.type, transformFromGeometryMsg(msg.transform), msg.variance);
}
void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg)
{
msg.fromId = link.from();
msg.toId = link.to();
msg.type = link.type();
msg.variance = link.variance();
transformToGeometryMsg(link.transform(), msg.transform);
}
cv::KeyPoint keypointFromROS(const rtabmap_ros::KeyPoint & msg)
{
return cv::KeyPoint(msg.ptx, msg.pty, msg.size, msg.angle, msg.response, msg.octave, msg.class_id);
}
void keypointToROS(const cv::KeyPoint & kpt, rtabmap_ros::KeyPoint & msg)
{
msg.angle = kpt.angle;
msg.class_id = kpt.class_id;
msg.octave = kpt.octave;
msg.ptx = kpt.pt.x;
msg.pty = kpt.pt.y;
msg.response = kpt.response;
msg.size = kpt.size;
}
void mapGraphFromROS(
const rtabmap_ros::Graph & msg,
std::map<int, rtabmap::Transform> & poses,
std::map<int, int> & mapIds,
std::multimap<int, rtabmap::Link> & links,
rtabmap::Transform & mapToOdom)
{
mapToOdom = transformFromGeometryMsg(msg.mapToOdom);
for(unsigned int i=0; i<msg.nodeIds.size() && i<msg.mapIds.size(); ++i)
{
if(msg.poses.size())
{
poses.insert(std::make_pair(msg.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg.poses[i])));
}
mapIds.insert(std::make_pair(msg.nodeIds[i], msg.mapIds[i]));
}
for(unsigned int i=0; i<msg.links.size(); ++i)
{
rtabmap::Transform t = rtabmap_ros::transformFromGeometryMsg(msg.links[i].transform);
links.insert(std::make_pair(msg.links[i].fromId, linkFromROS(msg.links[i])));
}
}
void mapGraphToROS(
const std::map<int, rtabmap::Transform> & poses,
const std::map<int, int> & mapIds,
const std::multimap<int, rtabmap::Link> & links,
const rtabmap::Transform & mapToOdom,
rtabmap_ros::Graph & msg)
{
UASSERT(poses.size() == 0 || poses.size() == mapIds.size());
transformToGeometryMsg(mapToOdom, msg.mapToOdom);
msg.nodeIds.resize(mapIds.size());
msg.poses.resize(poses.size());
msg.mapIds.resize(mapIds.size());
int index = 0;
std::map<int, rtabmap::Transform>::const_iterator iterPoses = poses.begin();
for(std::map<int, int>::const_iterator iter = mapIds.begin();
iter!=mapIds.end();
++iter)
{
msg.nodeIds[index] = iter->first;
msg.mapIds[index] = iter->second;
if(iterPoses != poses.end())
{
transformToPoseMsg(iterPoses->second, msg.poses[index]);
++iterPoses;
}
++index;
}
msg.links.resize(links.size());
index=0;
for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
{
linkToROS(iter->second, msg.links[index++]);
}
}
rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
{
//Features stuff...
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, pcl::PointXYZ> words3D;
pcl::PointCloud<pcl::PointXYZ> cloud;
if(msg.wordPts.data.size() &&
msg.wordPts.data.size() == msg.wordIds.size())
{
pcl::fromROSMsg(msg.wordPts, cloud);
}
for(unsigned int i=0; i<msg.wordIds.size() && i<msg.wordKpts.size(); ++i)
{
cv::KeyPoint pt = keypointFromROS(msg.wordKpts.at(i));
int wordId = msg.wordIds.at(i);
words.insert(std::make_pair(wordId, pt));
if(i< cloud.size())
{
words3D.insert(std::make_pair(wordId, cloud[i]));
}
}
if(words3D.size() && words3D.size() != words.size())
{
ROS_ERROR("Words 2D and 3D should be the same size (%d, %d)!", (int)words.size(), (int)words3D.size());
}
return rtabmap::Signature(msg.id,
msg.mapId,
words,
words3D,
transformFromPoseMsg(msg.pose),
compressedMatFromBytes(msg.laserScan),
compressedMatFromBytes(msg.image),
compressedMatFromBytes(msg.depth),
msg.fx,
msg.fy,
msg.cx,
msg.cy,
transformFromGeometryMsg(msg.localTransform));
}
void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg)
{
// add data
msg.id = signature.id();
msg.mapId = signature.mapId();
transformToPoseMsg(signature.getPose(), msg.pose);
compressedMatToBytes(signature.getImageCompressed(), msg.image);
compressedMatToBytes(signature.getDepthCompressed(), msg.depth);
compressedMatToBytes(signature.getLaserScanCompressed(), msg.laserScan);
msg.fx = signature.getDepthFx();
msg.fy = signature.getDepthFy();
msg.cx = signature.getDepthCx();
msg.cy = signature.getDepthCy();
transformToGeometryMsg(signature.getLocalTransform(), msg.localTransform);
//Features stuff...
msg.wordIds = uKeys(signature.getWords());
msg.wordKpts.resize(signature.getWords().size());
int index = 0;
for(std::multimap<int, cv::KeyPoint>::const_iterator jter=signature.getWords().begin();
jter!=signature.getWords().end();
++jter)
{
keypointToROS(jter->second, msg.wordKpts.at(index++));
}
if(signature.getWords3().size() && signature.getWords3().size() == signature.getWords().size())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.resize(signature.getWords3().size());
index = 0;
for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=signature.getWords3().begin();
jter!=signature.getWords3().end();
++jter)
{
cloud[index++] = jter->second;
}
pcl::toROSMsg(cloud, msg.wordPts);
}
else if(signature.getWords3().size())
{
ROS_ERROR("Words 2D and words 3D must have the same size (%d vs %d)!",
(int)signature.getWords().size(),
(int)signature.getWords3().size());
}
}
rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
{
rtabmap::OdometryInfo info;
info.lost = msg.lost;
info.matches = msg.matches;
info.features = msg.features;
info.inliers = msg.inliers;
info.localMapSize = msg.localMapSize;
info.time = msg.time;
info.variance = msg.variance;
return info;
}
void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & msg)
{
msg.lost = info.lost;
msg.matches = info.matches;
msg.features = info.features;
msg.inliers = info.inliers;
msg.localMapSize = info.localMapSize;
msg.time = info.time;
msg.variance = info.variance;
}
}
src/OdomMsgToTFNode.cpp
+2 -2
View File
@@ -60,14 +60,14 @@ public:
odomFrameId_ = msg->header.frame_id;
}
tf::StampedTransform t;
rtabmap::Transform pose = rtabmap::transformFromPoseMsg(msg->pose.pose);
rtabmap::Transform pose = rtabmap_ros::transformFromPoseMsg(msg->pose.pose);
if(pose.isNull())
{
ROS_WARN("Odometry received is null! Cannot send tf...");
}
else
{
rtabmap::transformToTF(pose, t);
rtabmap_ros::transformToTF(pose, t);
tfBroadcaster_.sendTransform(tf::StampedTransform (t, msg->header.stamp, odomFrameId_, frameId_));
}
}
src/OdometryROS.cpp
+31 -8
View File
@@ -41,11 +41,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Memory.h>
#include <rtabmap/core/Signature.h>
#include "rtabmap_ros/MsgConversion.h"
#include "rtabmap_ros/OdomInfo.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UStl.h"
namespace rtabmap {
using namespace rtabmap;
namespace rtabmap_ros {
OdometryROS::OdometryROS(int argc, char * argv[]) :
odometry_(0),
@@ -61,6 +64,7 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
ros::NodeHandle nh;
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);
@@ -278,7 +282,7 @@ void OdometryROS::processArguments(int argc, char * argv[])
}
}
Transform OdometryROS::processData(SensorData & data, const std_msgs::Header & header, int & quality)
void OdometryROS::processData(const SensorData & data, const std_msgs::Header & header)
{
if(odometry_->getPose().isNull() &&
!groundTruthFrameId_.empty())
@@ -291,7 +295,7 @@ Transform OdometryROS::processData(SensorData & data, const std_msgs::Header & h
if(!this->tfListener().waitForTransform(groundTruthFrameId_, frameId_, header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", groundTruthFrameId_.c_str(), frameId_.c_str());
return rtabmap::Transform(); // return null
return;
}
}
this->tfListener().lookupTransform(groundTruthFrameId_, frameId_, header.stamp, initialPose);
@@ -299,9 +303,9 @@ Transform OdometryROS::processData(SensorData & data, const std_msgs::Header & h
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return rtabmap::Transform(); // return null
return;
}
Transform pose = rtabmap::transformFromTF(initialPose);
rtabmap::Transform pose = rtabmap_ros::transformFromTF(initialPose);
ROS_INFO("Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
pose.prettyPrint().c_str(),
groundTruthFrameId_.c_str(),
@@ -310,14 +314,16 @@ Transform OdometryROS::processData(SensorData & data, const std_msgs::Header & h
}
// process data
rtabmap::Transform pose = odometry_->process(data, &quality);
ros::WallTime time = ros::WallTime::now();
rtabmap::OdometryInfo info;
rtabmap::Transform pose = odometry_->process(data, &info);
if(!pose.isNull())
{
//*********************
// Update odometry
//*********************
tf::Transform poseTF;
rtabmap::transformToTF(pose, poseTF);
rtabmap_ros::transformToTF(pose, poseTF);
if(publishTf_)
{
@@ -338,6 +344,14 @@ Transform OdometryROS::processData(SensorData & data, const std_msgs::Header & h
odom.pose.pose.position.z = poseTF.getOrigin().z();
tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
//set covariance
odom.pose.covariance.at(0) = info.variance; // xx
odom.pose.covariance.at(7) = info.variance; // yy
odom.pose.covariance.at(14) = info.variance; // zz
odom.pose.covariance.at(21) = info.variance; // rr
odom.pose.covariance.at(28) = info.variance; // pp
odom.pose.covariance.at(35) = info.variance; // yawyaw
//publish the message
odomPub_.publish(odom);
}
@@ -412,7 +426,16 @@ Transform OdometryROS::processData(SensorData & data, const std_msgs::Header & h
odomPub_.publish(odom);
}
return pose;
if(odomInfoPub_.getNumSubscribers())
{
rtabmap_ros::OdomInfo infoMsg;
odomInfoToROS(info, infoMsg);
infoMsg.header.stamp = header.stamp; // use corresponding time stamp to image
infoMsg.header.frame_id = odomFrameId_;
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());
}
bool OdometryROS::isOdometryBOW() const
src/OdometryROS.h
+7 -3
View File
@@ -43,6 +43,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
class Odometry;
}
namespace rtabmap_ros {
class OdometryROS
{
@@ -51,7 +54,7 @@ public:
~OdometryROS();
void processArguments(int argc, char * argv[]);
Transform processData(SensorData & data, const std_msgs::Header & header, int & quality);
void processData(const rtabmap::SensorData & data, const std_msgs::Header & header);
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::ResetPose::Response&);
@@ -60,7 +63,7 @@ public:
const std::string & frameId() const {return frameId_;}
const std::string & odomFrameId() const {return odomFrameId_;}
const ParametersMap & parameters() const {return parameters_;}
const rtabmap::ParametersMap & parameters() const {return parameters_;}
const tf::TransformListener & tfListener() const {return tfListener_;}
bool isPaused() const {return paused_;}
bool isOdometryBOW() const;
@@ -75,9 +78,10 @@ private:
std::string groundTruthFrameId_;
bool publishTf_;
bool waitForTransform_;
ParametersMap parameters_;
rtabmap::ParametersMap parameters_;
ros::Publisher odomPub_;
ros::Publisher odomInfoPub_;
ros::Publisher odomLocalMap_;
ros::Publisher odomLastFrame_;
ros::ServiceServer resetSrv_;
src/RGBDOdometryNode.cpp
+7 -10
View File
@@ -46,11 +46,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
using namespace rtabmap;
class RGBDOdometry : public OdometryROS
class RGBDOdometry : public rtabmap_ros::OdometryROS
{
public:
RGBDOdometry(int argc, char * argv[]) :
OdometryROS(argc, argv),
rtabmap_ros::OdometryROS(argc, argv),
sync_(0)
{
ros::NodeHandle nh;
@@ -127,9 +127,6 @@ public:
return;
}
ros::WallTime time = ros::WallTime::now();
int quality = -1;
if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
{
image_geometry::PinholeCameraModel model;
@@ -141,19 +138,19 @@ public:
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, "mono8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
rtabmap::SensorData data(ptrImage->image,
rtabmap::SensorData data(
ptrImage->image,
ptrDepth->image,
fx,
fy,
cx,
cy,
rtabmap_ros::transformFromTF(localTransform),
rtabmap::Transform(),
rtabmap::transformFromTF(localTransform));
quality=0;
1.0f);
this->processData(data, image->header, quality);
this->processData(data, image->header);
}
ROS_INFO("Odom: quality=%d, update time=%fs", quality, (ros::WallTime::now()-time).toSec());
}
}
src/StereoOdometryNode.cpp
+6 -10
View File
@@ -48,11 +48,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
using namespace rtabmap;
class StereoOdometry : public OdometryROS
class StereoOdometry : public rtabmap_ros::OdometryROS
{
public:
StereoOdometry(int argc, char * argv[]) :
OdometryROS(argc, argv),
rtabmap_ros::OdometryROS(argc, argv),
approxSync_(0),
exactSync_(0)
{
@@ -162,22 +162,18 @@ public:
UTimer stepTimer;
//
UDEBUG("localTransform = %s", rtabmap::transformFromTF(localTransform).prettyPrint().c_str());
UDEBUG("localTransform = %s", rtabmap_ros::transformFromTF(localTransform).prettyPrint().c_str());
rtabmap::SensorData data(ptrImageLeft->image,
ptrImageRight->image,
fx,
baseline,
cx,
cy,
rtabmap_ros::transformFromTF(localTransform),
rtabmap::Transform(),
rtabmap::transformFromTF(localTransform));
quality=0;
1.0f);
this->processData(data, imageRectLeft->header, quality);
UDEBUG("time odometry->process()=%fs", stepTimer.ticks());
ROS_INFO("Odom: quality=%d, update time=%fs",
quality, (ros::WallTime::now()-time).toSec());
this->processData(data, imageRectLeft->header);
}
else
{
src/VisualOdometryDEPRECATED.cpp
-37
View File
@@ -1,37 +0,0 @@
/*
Copyright (c) 2010-2014, 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 <rtabmap/utilite/ULogger.h>
int main(int argc, char *argv[])
{
ULogger::setType(ULogger::kTypeConsole);
UFATAL("visual_odometry node is deprecated, use rgbd_odometry instead!");
return 0;
}
src/nodelets/data_odom_sync.cpp
+2 -2
View File
@@ -39,7 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/CameraInfo.h>
#include <nav_msgs/Odometry.h>
namespace rtabmap
namespace rtabmap_ros
{
class DataOdomSyncNodelet : public nodelet::Nodelet
@@ -126,5 +126,5 @@ private:
};
PLUGINLIB_EXPORT_CLASS(rtabmap::DataOdomSyncNodelet, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::DataOdomSyncNodelet, nodelet::Nodelet);
}
src/nodelets/data_throttle.cpp
+2 -2
View File
@@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/CameraInfo.h>
namespace rtabmap
namespace rtabmap_ros
{
class DataThrottleNodelet : public nodelet::Nodelet
@@ -134,5 +134,5 @@ private:
};
PLUGINLIB_EXPORT_CLASS(rtabmap::DataThrottleNodelet, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::DataThrottleNodelet, nodelet::Nodelet);
}
src/nodelets/disparity_to_depth.cpp
+2 -2
View File
@@ -37,7 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <cv_bridge/cv_bridge.h>
namespace rtabmap
namespace rtabmap_ros
{
class DisparityToDepth : public nodelet::Nodelet
@@ -136,5 +136,5 @@ private:
ros::Subscriber sub_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap::DisparityToDepth, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::DisparityToDepth, nodelet::Nodelet);
}
src/nodelets/obstacles_detection.cpp
+7 -7
View File
@@ -57,7 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
namespace rtabmap
namespace rtabmap_ros
{
class ObstaclesDetection : public nodelet::Nodelet
@@ -102,7 +102,7 @@ private:
{
if(groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers())
{
Transform localTransform;
rtabmap::Transform localTransform;
try
{
if(waitForTransform_)
@@ -115,7 +115,7 @@ private:
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
@@ -128,13 +128,13 @@ private:
pcl::IndicesPtr ground, obstacles;
if(cloud->size())
{
cloud = util3d::transformPointCloud<pcl::PointXYZ>(cloud, localTransform);
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(cloud, localTransform);
if(maxObstaclesHeight_ > 0)
{
cloud = util3d::passThrough<pcl::PointXYZ>(cloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
cloud = rtabmap::util3d::passThrough<pcl::PointXYZ>(cloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
}
util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud,
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud,
ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
}
@@ -189,6 +189,6 @@ private:
ros::Subscriber cloudSub_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap::ObstaclesDetection, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::ObstaclesDetection, nodelet::Nodelet);
}
src/nodelets/point_cloud_xyz.cpp
+7 -7
View File
@@ -53,7 +53,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
namespace rtabmap
namespace rtabmap_ros
{
class PointCloudXYZ : public nodelet::Nodelet
@@ -154,7 +154,7 @@ private:
float cy = model.cy();
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
pclCloud = util3d::cloudFromDepth(
pclCloud = rtabmap::util3d::cloudFromDepth(
imageDepthPtr->image,
cx,
cy,
@@ -195,7 +195,7 @@ private:
float cy = model.cy();
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
pclCloud = util3d::cloudFromDisparity(
pclCloud = rtabmap::util3d::cloudFromDisparity(
disparity,
cx,
cy,
@@ -211,12 +211,12 @@ private:
{
if(pclCloud->size() && maxDepth_ > 0)
{
pclCloud = util3d::passThrough<pcl::PointXYZ>(pclCloud, "z", 0, maxDepth_);
pclCloud = rtabmap::util3d::passThrough<pcl::PointXYZ>(pclCloud, "z", 0, maxDepth_);
}
if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{
pcl::IndicesPtr indices = util3d::radiusFiltering<pcl::PointXYZ>(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering<pcl::PointXYZ>(pclCloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*pclCloud, *indices, *tmp);
pclCloud = tmp;
@@ -224,7 +224,7 @@ private:
if(pclCloud->size() && voxelSize_ > 0.0)
{
pclCloud = util3d::voxelize<pcl::PointXYZ>(pclCloud, voxelSize_);
pclCloud = rtabmap::util3d::voxelize<pcl::PointXYZ>(pclCloud, voxelSize_);
}
sensor_msgs::PointCloud2 rosCloud;
@@ -266,6 +266,6 @@ private:
};
PLUGINLIB_EXPORT_CLASS(rtabmap::PointCloudXYZ, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZ, nodelet::Nodelet);
}
src/nodelets/point_cloud_xyzrgb.cpp
+2 -2
View File
@@ -51,7 +51,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d.h"
namespace rtabmap
namespace rtabmap_ros
{
class PointCloudXYZRGB : public nodelet::Nodelet
@@ -162,6 +162,6 @@ private:
message_filters::Synchronizer<MySyncPolicy> * sync_;
};
PLUGINLIB_EXPORT_CLASS(rtabmap::PointCloudXYZRGB, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::PointCloudXYZRGB, nodelet::Nodelet);
}
src/nodelets/stereo_throttle.cpp
+2 -2
View File
@@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/CameraInfo.h>
namespace rtabmap
namespace rtabmap_ros
{
class StereoThrottleNodelet : public nodelet::Nodelet
@@ -170,5 +170,5 @@ private:
};
PLUGINLIB_EXPORT_CLASS(rtabmap::StereoThrottleNodelet, nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::StereoThrottleNodelet, nodelet::Nodelet);
}
src/rviz/InfoDisplay.cpp
+4 -4
View File
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "InfoDisplay.h"
#include "rtabmap_ros/MsgConversion.h"
namespace rtabmap
namespace rtabmap_ros
{
InfoDisplay::InfoDisplay()
@@ -75,7 +75,7 @@ void InfoDisplay::processMessage( const rtabmap_ros::InfoConstPtr& msg )
{
info_ = "";
}
loopTransform_ = transformFromGeometryMsg(msg->loopClosureTransform);
loopTransform_ = rtabmap_ros::transformFromGeometryMsg(msg->loopClosureTransform);
}
this->emitTimeSignal(msg->header.stamp);
@@ -114,7 +114,7 @@ void InfoDisplay::reset()
}
}
} // namespace rtabmap
} // namespace rtabmap_ros
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS( rtabmap::InfoDisplay, rviz::Display )
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::InfoDisplay, rviz::Display )
src/rviz/InfoDisplay.h
+3 -3
View File
@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rviz/message_filter_display.h>
#include <rtabmap/core/Transform.h>
namespace rtabmap
namespace rtabmap_ros
{
class InfoDisplay: public rviz::MessageFilterDisplay<rtabmap_ros::Info>
@@ -60,10 +60,10 @@ private:
QString info_;
int globalCount_;
int localCount_;
Transform loopTransform_;
rtabmap::Transform loopTransform_;
boost::mutex info_mutex_;
};
} // namespace rtabmap
} // namespace rtabmap_ros
#endif
src/rviz/MapCloudDisplay.cpp
+19 -19
View File
@@ -55,13 +55,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_ros/GetMap.h>
namespace rtabmap
namespace rtabmap_ros
{
MapCloudDisplay::CloudInfo::CloudInfo() :
manager_(0),
pose_(Transform::getIdentity()),
pose_(rtabmap::Transform::getIdentity()),
scene_node_(0)
{}
@@ -258,8 +258,8 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
float cy = map.nodes[i].cy;
//uncompress data
util3d::CompressionThread ctImage(compressedMatFromBytes(map.nodes[i].image.bytes, false), true);
util3d::CompressionThread ctDepth(compressedMatFromBytes(map.nodes[i].depth.bytes, false), true);
rtabmap::util3d::CompressionThread ctImage(compressedMatFromBytes(map.nodes[i].image, false), true);
rtabmap::util3d::CompressionThread ctDepth(compressedMatFromBytes(map.nodes[i].depth, false), true);
ctImage.start();
ctDepth.start();
ctImage.join();
@@ -272,27 +272,27 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(depth.type() == CV_8UC1)
{
cloud = util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
cloud = rtabmap::util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
}
else
{
cloud = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
cloud = rtabmap::util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
}
if(cloud_max_depth_->getFloat() > 0.0f)
{
cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, cloud_max_depth_->getFloat());
cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, cloud_max_depth_->getFloat());
}
if(cloud_voxel_size_->getFloat() > 0.0f)
{
cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, cloud_voxel_size_->getFloat());
cloud = rtabmap::util3d::voxelize<pcl::PointXYZRGB>(cloud, cloud_voxel_size_->getFloat());
}
cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
// do it after local transform
if(cloud_filter_floor_height_->getFloat() > 0.0f)
{
cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
@@ -315,15 +315,15 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
}
// Update graph
std::map<int, Transform> poses;
for(unsigned int i=0; i<map.poseIDs.size() && i<map.poses.size(); ++i)
std::map<int, rtabmap::Transform> poses;
for(unsigned int i=0; i<map.graph.nodeIds.size() && i<map.graph.poses.size(); ++i)
{
poses.insert(std::make_pair(map.poseIDs[i], transformFromPoseMsg(map.poses[i])));
poses.insert(std::make_pair(map.graph.nodeIds[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
}
if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
{
poses = util3d::radiusPosesFiltering(poses,
poses = rtabmap::util3d::radiusPosesFiltering(poses,
node_filtering_radius_->getFloat(),
node_filtering_angle_->getFloat()*CV_PI/180.0);
}
@@ -499,12 +499,12 @@ void MapCloudDisplay::downloadMap()
else
{
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
QApplication::processEvents();
this->reset();
processMapData(getMapSrv.response.data);
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
QTimer::singleShot(1000, messageBox, SLOT(close()));
}
@@ -556,10 +556,10 @@ void MapCloudDisplay::downloadGraph()
}
else
{
messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.poses.size()));
messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size()));
QApplication::processEvents();
processMapData(getMapSrv.response.data);
messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.poses.size()));
messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size()));
QTimer::singleShot(1000, messageBox, SLOT(close()));
}
@@ -885,4 +885,4 @@ bool MapCloudDisplay::transformCloud(const CloudInfoPtr& cloud_info, bool update
} // namespace rtabmap
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS( rtabmap::MapCloudDisplay, rviz::Display )
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::MapCloudDisplay, rviz::Display )
src/rviz/MapCloudDisplay.h
+2 -2
View File
@@ -54,7 +54,7 @@ typedef std::vector<std::string> V_string;
using namespace rviz;
namespace rtabmap
namespace rtabmap_ros
{
class PointCloudCommon;
@@ -185,6 +185,6 @@ private:
pluginlib::ClassLoader<rviz::PointCloudTransformer>* transformer_class_loader_;
};
} // namespace rtabmap
} // namespace rtabmap_ros
#endif
src/rviz/MapGraphDisplay.cpp
+9 -9
View File
@@ -43,7 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "MapGraphDisplay.h"
namespace rtabmap
namespace rtabmap_ros
{
MapGraphDisplay::MapGraphDisplay()
@@ -84,22 +84,22 @@ void MapGraphDisplay::destroyObjects()
void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg )
{
if(!(msg->maps.size() == msg->poseIDs.size() && msg->poses.size() == msg->poseIDs.size()))
if(!(msg->graph.mapIds.size() == msg->graph.nodeIds.size() && msg->graph.poses.size() == msg->graph.nodeIds.size()))
{
ROS_ERROR("rtambap::MapGraph: Error map ids, pose ids and poses must have all the same size.");
ROS_ERROR("rtabmap_ros::MapGraph: Error map ids, pose ids and poses must have all the same size.");
return;
}
// Find all graphs
std::map<int, std::map<int, geometry_msgs::Point> > graphs;
for(unsigned int i=0; i<msg->poses.size(); ++i)
for(unsigned int i=0; i<msg->graph.poses.size(); ++i)
{
std::map<int, std::map<int, geometry_msgs::Point> >::iterator iter = graphs.find(msg->maps[i]);
std::map<int, std::map<int, geometry_msgs::Point> >::iterator iter = graphs.find(msg->graph.mapIds[i]);
if(iter == graphs.end())
{
iter = graphs.insert(std::make_pair(msg->maps[i], std::map<int, geometry_msgs::Point>())).first;
iter = graphs.insert(std::make_pair(msg->graph.mapIds[i], std::map<int, geometry_msgs::Point>())).first;
}
iter->second.insert(std::make_pair(msg->poseIDs[i], msg->poses[i].position));
iter->second.insert(std::make_pair(msg->graph.nodeIds[i], msg->graph.poses[i].position));
}
destroyObjects();
@@ -142,7 +142,7 @@ void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg
}
}
} // namespace rviz
} // namespace rtabmap_ros
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS( rtabmap::MapGraphDisplay, rviz::Display )
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::MapGraphDisplay, rviz::Display )
src/rviz/MapGraphDisplay.h
+5 -5
View File
@@ -26,8 +26,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef RVIZ_PATH_DISPLAY_H
#define RVIZ_PATH_DISPLAY_H
#ifndef MAP_GRAPH_DISPLAY_H
#define MAP_GRAPH_DISPLAY_H
#include <rtabmap_ros/MapData.h>
@@ -46,7 +46,7 @@ class FloatProperty;
using namespace rviz;
namespace rtabmap
namespace rtabmap_ros
{
/**
@@ -79,7 +79,7 @@ private:
FloatProperty* alpha_property_;
};
} // namespace rviz
} // namespace rtabmap_ros
#endif /* RVIZ_PATH_DISPLAY_H */
#endif /* MAP_GRAPH_DISPLAY_H */
src/rviz/OrbitOrientedViewController.cpp
+2 -2
View File
@@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rviz/properties/vector_property.h"
#include "rviz/ogre_helpers/shape.h"
namespace rtabmap
namespace rtabmap_ros
{
void OrbitOrientedViewController::updateCamera()
@@ -73,4 +73,4 @@ void OrbitOrientedViewController::updateCamera()
}
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS( rtabmap::OrbitOrientedViewController, rviz::ViewController )
PLUGINLIB_EXPORT_CLASS( rtabmap_ros::OrbitOrientedViewController, rviz::ViewController )
src/rviz/OrbitOrientedViewController.h
+1 -1
View File
@@ -30,7 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rviz/default_plugin/view_controllers/orbit_view_controller.h"
namespace rtabmap
namespace rtabmap_ros
{
class OrbitOrientedViewController: public rviz::OrbitViewController