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

Merge pull request #23 from introlab/devel

Browse Source 
Merging devel to master
This commit is contained in:
matlabbe
2015-07-19 18:59:07 -04:00
parent 48658d4e55 4694c7ffe4
commit a80f60ed12
28 changed files with 2685 additions and 1592 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -3
View File
@@ -17,7 +17,7 @@ find_package(octomap_ros)
## System dependencies are found with CMake's conventions ## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system) # find_package(Boost REQUIRED COMPONENTS system)
find_package(RTABMap 0.9.0 REQUIRED) find_package(RTABMap 0.10.1 REQUIRED)
find_package(OpenCV REQUIRED) find_package(OpenCV REQUIRED)
@@ -46,11 +46,9 @@ add_message_files(
Info.msg Info.msg
KeyPoint.msg KeyPoint.msg
MapData.msg MapData.msg
Graph.msg
NodeData.msg NodeData.msg
Link.msg Link.msg
OdomInfo.msg OdomInfo.msg
UserData.msg
Point2f.msg Point2f.msg
) )
cfg/Camera.cfg
-2
View File
@@ -7,8 +7,6 @@ gen = ParameterGenerator()
gen.add("device_id", int_t, 0, "Camera device ID", 0, 0, 7) gen.add("device_id", int_t, 0, "Camera device ID", 0, 0, 7)
gen.add("frame_rate", double_t, 0, "Frame rate", 15.0, 0.0, 100.0) gen.add("frame_rate", double_t, 0, "Frame rate", 15.0, 0.0, 100.0)
gen.add("width", int_t, 0, "Width", 640, 0, 1920)
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("video_or_images_path", str_t, 0, "Video or images directory path", "")
gen.add("pause", bool_t, 0, "Pause", False) gen.add("pause", bool_t, 0, "Pause", False)
include/rtabmap_ros/MsgConversion.h
+16 -13
View File
@@ -45,7 +45,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_ros/KeyPoint.h> #include <rtabmap_ros/KeyPoint.h>
#include <rtabmap_ros/Point2f.h> #include <rtabmap_ros/Point2f.h>
#include <rtabmap_ros/MapData.h> #include <rtabmap_ros/MapData.h>
#include <rtabmap_ros/Graph.h>
#include <rtabmap_ros/NodeData.h> #include <rtabmap_ros/NodeData.h>
#include <rtabmap_ros/OdomInfo.h> #include <rtabmap_ros/OdomInfo.h>
#include <rtabmap_ros/Info.h> #include <rtabmap_ros/Info.h>
@@ -83,24 +82,28 @@ void point2fToROS(const cv::Point2f & kpt, rtabmap_ros::Point2f & msg);
std::vector<cv::Point2f> points2fFromROS(const std::vector<rtabmap_ros::Point2f> & msg); std::vector<cv::Point2f> points2fFromROS(const std::vector<rtabmap_ros::Point2f> & msg);
void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ros::Point2f> & msg); void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ros::Point2f> & msg);
void mapGraphFromROS( void mapDataFromROS(
const rtabmap_ros::Graph & msg, const rtabmap_ros::MapData & msg,
std::map<int, rtabmap::Transform> & poses,
std::multimap<int, rtabmap::Link> & links,
std::map<int, rtabmap::Signature> & signatures,
rtabmap::Transform & mapToOdom);
void mapDataFromROS(
const rtabmap_ros::MapData & msg,
std::map<int, rtabmap::Transform> & poses, std::map<int, rtabmap::Transform> & poses,
std::map<int, int> & mapIds,
std::map<int, double> & stamps,
std::map<int, std::string> & labels,
std::map<int, std::vector<unsigned char> > & userDatas,
std::multimap<int, rtabmap::Link> & links, std::multimap<int, rtabmap::Link> & links,
rtabmap::Transform & mapToOdom); rtabmap::Transform & mapToOdom);
void mapGraphToROS( void mapDataToROS(
const std::map<int, rtabmap::Transform> & poses,
const std::multimap<int, rtabmap::Link> & links,
const std::map<int, rtabmap::Signature> & signatures,
const rtabmap::Transform & mapToOdom,
rtabmap_ros::MapData & msg);
void mapDataToROS(
const std::map<int, rtabmap::Transform> & poses, const std::map<int, rtabmap::Transform> & poses,
const std::map<int, int> & mapIds,
const std::map<int, double> & stamps,
const std::map<int, std::string> & labels,
const std::map<int, std::vector<unsigned char> > & userDatas,
const std::multimap<int, rtabmap::Link> & links, const std::multimap<int, rtabmap::Link> & links,
const rtabmap::Transform & mapToOdom, const rtabmap::Transform & mapToOdom,
rtabmap_ros::Graph & msg); rtabmap_ros::MapData & msg);
rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg); rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg);
void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg); void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg);
launch/demo/demo_two_kinects.launch
+139
View File
@@ -0,0 +1,139 @@
<launch>
<!-- Multi-cameras demo with 2 Kinects -->
<!-- Cameras -->
<include file="$(find freenect_launch)/launch/freenect.launch">
<arg name="depth_registration" value="True" />
<arg name="camera" value="camera1" />
<arg name="device_id" value="#1" />
</include>
<include file="$(find freenect_launch)/launch/freenect.launch">
<arg name="depth_registration" value="True" />
<arg name="camera" value="camera2" />
<arg name="device_id" value="#2" />
</include>
<!-- Frames: Kinects are placed at 90 degrees -->
<node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf"
args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_link 100" />
<node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
args="-0.1325 -0.1975 0.0 -1.570796327 0.0 0.0 /base_link /camera2_link 100" />
<!-- Choose visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<!-- 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
-"odom_info_data" : Fill odometry info messages with inliers/outliers data.
-->
<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" />
<arg name="odom_info_data" default="true" />
<arg name="wait_for_transform" default="true" />
<group ns="rtabmap">
<!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="rgb0/image" to="/camera1/rgb/image_rect_color"/>
<remap from="depth0/image" to="/camera1/depth_registered/image_raw"/>
<remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
<remap from="rgb1/image" to="/camera2/rgb/image_rect_color"/>
<remap from="depth1/image" to="/camera2/depth_registered/image_raw"/>
<remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="depth_cameras" type="int" value="2"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/>
<param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
<param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/>
</node>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="depth_cameras" type="int" value="2"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="gen_scan" type="bool" value="true"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<remap from="rgb0/image" to="/camera1/rgb/image_rect_color"/>
<remap from="depth0/image" to="/camera1/depth_registered/image_raw"/>
<remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
<remap from="rgb1/image" to="/camera2/rgb/image_rect_color"/>
<remap from="depth1/image" to="/camera2/depth_registered/image_raw"/>
<remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
<param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="$(arg odom_info_data)"/>
<param name="frame_id" type="string" value="base_link"/>
<param name="depth_cameras" type="int" value="2"/>
<param name="wait_for_transform" type="bool" value="$(arg wait_for_transform)"/>
<remap from="rgb0/image" to="/camera1/rgb/image_rect_color"/>
<remap from="depth0/image" to="/camera1/depth_registered/image_raw"/>
<remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
<remap from="rgb1/image" to="/camera2/rgb/image_rect_color"/>
<remap from="depth1/image" to="/camera2/depth_registered/image_raw"/>
<remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="camera1/rgb/image_rect_color"/>
<remap from="depth/image_in" to="camera1/depth_registered/image_raw"/>
<remap from="rgb/camera_info_in" to="camera1/rgb/camera_info"/>
<remap from="odom_in" to="rtabmap/odom"/>
<remap from="rgb/image_out" to="data_odom_sync/image"/>
<remap from="depth/image_out" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
<remap from="odom_out" to="odom_sync"/>
</node>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
<remap from="rgb/image" to="data_odom_sync/image"/>
<remap from="depth/image" to="data_odom_sync/depth"/>
<remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
<remap from="cloud" to="voxel_cloud" />
<param name="voxel_size" type="double" value="0.01"/>
</node>
</launch>
launch/rgbd_mapping.launch
+44 -37
View File
@@ -6,11 +6,18 @@
<!-- See "delete_db_on_start" option below... --> <!-- See "delete_db_on_start" option below... -->
<!-- Choose visualization --> <!-- Choose visualization -->
<arg name="rviz" default="false" /> <arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" /> <arg name="rtabmapviz" default="true" />
<!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published --> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="frame_id" default="camera_link"/> <arg name="frame_id" default="camera_link"/>
<!-- Image topics input -->
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="depth_registered_topic" default="/camera/depth_registered/image_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<arg name="namespace" default="rtabmap"/>
<!-- ODOMETRY MAIN ARGUMENTS: <!-- ODOMETRY MAIN ARGUMENTS:
-"strategy" : Strategy: 0=BOW (bag-of-words) 1=Optical Flow -"strategy" : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
@@ -24,43 +31,43 @@
-"local_map" : Local map size: number of unique features to keep track -"local_map" : Local map size: number of unique features to keep track
-"odom_info_data" : Fill odometry info messages with inliers/outliers data. -"odom_info_data" : Fill odometry info messages with inliers/outliers data.
--> -->
<arg name="strategy" default="0" /> <arg name="strategy" default="0" />
<arg name="feature" default="6" /> <arg name="feature" default="6" />
<arg name="nn" default="3" /> <arg name="nn" default="3" />
<arg name="max_depth" default="4.0" /> <arg name="max_depth" default="4.0" />
<arg name="min_inliers" default="20" /> <arg name="min_inliers" default="20" />
<arg name="inlier_distance" default="0.02" /> <arg name="inlier_distance" default="0.02" />
<arg name="local_map" default="1000" /> <arg name="local_map" default="1000" />
<arg name="odom_info_data" default="true" /> <arg name="odom_info_data" default="true" />
<group ns="rtabmap"> <group ns="$(arg namespace)">
<!-- Odometry --> <!-- Odometry -->
<node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen"> <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/> <remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/> <remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="Odom/Strategy" type="string" value="$(arg strategy)"/> <param name="Odom/Strategy" type="string" value="$(arg strategy)"/>
<param name="Odom/FeatureType" type="string" value="$(arg feature)"/> <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/MaxDepth" type="string" value="$(arg max_depth)"/>
<param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/> <param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/>
<param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/> <param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>
<param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/> <param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
<param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/> <param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/>
</node> </node>
<!-- Visual SLAM (robot side) --> <!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" --> <!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start"> <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/> <param name="subscribe_depth" type="bool" value="true"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/> <remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/> <remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<param name="LccBow/MinInliers" type="string" value="10"/> <param name="LccBow/MinInliers" type="string" value="10"/>
<param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/> <param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
@@ -68,13 +75,13 @@
<!-- Visualisation RTAB-Map --> <!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen"> <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="true"/> <param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="$(arg odom_info_data)"/> <param name="subscribe_odom_info" type="bool" value="$(arg odom_info_data)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/> <param name="frame_id" type="string" value="$(arg frame_id)"/>
<remap from="rgb/image" to="/camera/rgb/image_rect_color"/> <remap from="rgb/image" to="$(arg rgb_topic)"/>
<remap from="depth/image" to="/camera/depth_registered/image_raw"/> <remap from="depth/image" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/> <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
</node> </node>
</group> </group>
@@ -84,9 +91,9 @@
<!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) --> <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/> <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet"> <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
<remap from="rgb/image_in" to="camera/rgb/image_rect_color"/> <remap from="rgb/image_in" to="$(arg rgb_topic)"/>
<remap from="depth/image_in" to="camera/depth_registered/image_raw"/> <remap from="depth/image_in" to="$(arg depth_registered_topic)"/>
<remap from="rgb/camera_info_in" to="camera/rgb/camera_info"/> <remap from="rgb/camera_info_in" to="$(arg camera_info_topic)"/>
<remap from="odom_in" to="rtabmap/odom"/> <remap from="odom_in" to="rtabmap/odom"/>
<remap from="rgb/image_out" to="data_odom_sync/image"/> <remap from="rgb/image_out" to="data_odom_sync/image"/>
msg/Graph.msg
-26
View File
@@ -1,26 +0,0 @@
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
string[] labels
float64[] stamps
UserData[] userDatas
# std::map<nodeId, Pose>
geometry_msgs/Pose[] poses
##
# The links
##
Link[] links
msg/MapData.msg
+15 -4
View File
@@ -2,14 +2,25 @@
Header header Header header
################## ##################
# Graph stuff # Optimized graph
################## ##################
##
# /map to /odom transform
# Always identity when the graph is optimized from the latest pose.
##
geometry_msgs/Transform mapToOdom
Graph graph # The poses
int32[] posesId
geometry_msgs/Pose[] poses
# The links
Link[] links
################## ##################
# Point cloud stuff # Graph data
################## ##################
NodeData[] nodes NodeData[] nodes
msg/NodeData.msg
+12 -8
View File
@@ -4,7 +4,6 @@ int32 mapId
int32 weight int32 weight
float64 stamp float64 stamp
string label string label
UserData userData
# Pose from odometry not corrected # Pose from odometry not corrected
geometry_msgs/Pose pose geometry_msgs/Pose pose
@@ -17,18 +16,23 @@ uint8[] image
# use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h" # use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h"
uint8[] depth uint8[] depth
float32 fx # Camera models
float32 fy float32[] fx
float32 cx float32[] fy
float32 cy float32[] cx
float32[] cy
float32 baseline
# local transform (/base_link -> /camera_link)
geometry_msgs/Transform[] localTransform
# compressed 2D laser scan in /base_link frame # compressed 2D laser scan in /base_link frame
# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h" # use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
uint8[] laserScan uint8[] laserScan
int32 laserScanMaxPts
# local transform (/base_link -> /camera_link) # compressed user data
geometry_msgs/Transform localTransform # use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
uint8[] userData
# std::multimap<wordId, cv::Keypoint> # std::multimap<wordId, cv::Keypoint>
# std::multimap<wordId, pcl::PointXYZ> # std::multimap<wordId, pcl::PointXYZ>
msg/OdomInfo.msg
+8 -1
View File
@@ -30,7 +30,11 @@ int32 inliers
float32 variance float32 variance
int32 features int32 features
int32 localMapSize int32 localMapSize
float32 time float32 timeEstimation
float32 timeParticleFiltering
float32 stamp
float32 interval
float32 distanceTravelled
int32 type int32 type
@@ -43,3 +47,6 @@ Point2f[] refCorners
Point2f[] newCorners Point2f[] newCorners
int32[] cornerInliers int32[] cornerInliers
geometry_msgs/Transform transform
geometry_msgs/Transform transformFiltered
msg/UserData.msg
-2
View File
@@ -1,2 +0,0 @@
uint8[] data
src/CameraNode.cpp
+16 -28
View File
@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <image_transport/image_transport.h> #include <image_transport/image_transport.h>
#include <std_srvs/Empty.h> #include <std_srvs/Empty.h>
#include <rtabmap/core/Camera.h> #include <rtabmap/core/CameraRGB.h>
#include <rtabmap/core/CameraThread.h> #include <rtabmap/core/CameraThread.h>
#include <rtabmap/core/CameraEvent.h> #include <rtabmap/core/CameraEvent.h>
@@ -55,6 +55,7 @@ public:
unsigned int imageWidth = 0, unsigned int imageWidth = 0,
unsigned int imageHeight = 0) : unsigned int imageHeight = 0) :
cameraThread_(0), cameraThread_(0),
camera_(0),
frameId_("camera") frameId_("camera")
{ {
ros::NodeHandle nh; ros::NodeHandle nh;
@@ -80,7 +81,7 @@ public:
{ {
if(cameraThread_) if(cameraThread_)
{ {
return cameraThread_->init(); return cameraThread_->camera()->init();
} }
return false; return false;
} }
@@ -113,10 +114,10 @@ public:
return true; return true;
} }
void setParameters(int deviceId, double frameRate, int width, int height, const std::string & path, bool pause) void setParameters(int deviceId, double frameRate, const std::string & path, bool pause)
{ {
ROS_INFO("Parameters changed: deviceId=%d, path=%s frameRate=%f w/h=%d/%d pause=%s", ROS_INFO("Parameters changed: deviceId=%d, path=%s frameRate=%f pause=%s",
deviceId, path.c_str(), frameRate, width, height, pause?"true":"false"); deviceId, path.c_str(), frameRate, pause?"true":"false");
if(cameraThread_) if(cameraThread_)
{ {
rtabmap::CameraVideo * videoCam = dynamic_cast<rtabmap::CameraVideo *>(camera_); rtabmap::CameraVideo * videoCam = dynamic_cast<rtabmap::CameraVideo *>(camera_);
@@ -128,7 +129,6 @@ public:
if(!path.empty() && UDirectory::getDir(path+"/").compare(UDirectory::getDir(imagesCam->getPath())) == 0) if(!path.empty() && UDirectory::getDir(path+"/").compare(UDirectory::getDir(imagesCam->getPath())) == 0)
{ {
imagesCam->setImageRate(frameRate); imagesCam->setImageRate(frameRate);
imagesCam->setImageSize(width, height);
if(pause && !cameraThread_->isPaused()) if(pause && !cameraThread_->isPaused())
{ {
cameraThread_->join(true); cameraThread_->join(true);
@@ -150,7 +150,6 @@ public:
{ {
// video // video
videoCam->setImageRate(frameRate); videoCam->setImageRate(frameRate);
videoCam->setImageSize(width, height);
if(pause && !cameraThread_->isPaused()) if(pause && !cameraThread_->isPaused())
{ {
cameraThread_->join(true); cameraThread_->join(true);
@@ -165,25 +164,14 @@ public:
videoCam->getUsbDevice() == deviceId) videoCam->getUsbDevice() == deviceId)
{ {
// usb device // usb device
unsigned int w; videoCam->setImageRate(frameRate);
unsigned int h; if(pause && !cameraThread_->isPaused())
videoCam->getImageSize(w, h);
if((int)w == width && (int)h == height)
{ {
videoCam->setImageRate(frameRate); cameraThread_->join(true);
if(pause && !cameraThread_->isPaused())
{
cameraThread_->join(true);
}
else if(!pause && cameraThread_->isPaused())
{
cameraThread_->start();
}
} }
else else if(!pause && cameraThread_->isPaused())
{ {
delete cameraThread_; cameraThread_->start();
cameraThread_ = 0;
} }
} }
else else
@@ -205,12 +193,12 @@ public:
if(!path.empty() && UDirectory::exists(path)) if(!path.empty() && UDirectory::exists(path))
{ {
//images //images
camera_ = new rtabmap::CameraImages(path, 1, false, frameRate, width, height); camera_ = new rtabmap::CameraImages(path, 1, false, frameRate);
} }
else if(!path.empty() && UFile::exists(path)) else if(!path.empty() && UFile::exists(path))
{ {
//video //video
camera_ = new rtabmap::CameraVideo(path, frameRate, width, height); camera_ = new rtabmap::CameraVideo(path, frameRate);
} }
else else
{ {
@@ -219,7 +207,7 @@ public:
ROS_ERROR("Path \"%s\" does not exist (or you don't have the permissions to read)... falling back to usb device...", path.c_str()); ROS_ERROR("Path \"%s\" does not exist (or you don't have the permissions to read)... falling back to usb device...", path.c_str());
} }
//usb device //usb device
camera_ = new rtabmap::CameraVideo(deviceId, frameRate, width, height); camera_ = new rtabmap::CameraVideo(deviceId, frameRate);
} }
cameraThread_ = new rtabmap::CameraThread(camera_); cameraThread_ = new rtabmap::CameraThread(camera_);
init(); init();
@@ -236,7 +224,7 @@ protected:
if(event->getClassName().compare("CameraEvent") == 0) if(event->getClassName().compare("CameraEvent") == 0)
{ {
rtabmap::CameraEvent * e = (rtabmap::CameraEvent*)event; rtabmap::CameraEvent * e = (rtabmap::CameraEvent*)event;
const cv::Mat & image = e->data().image(); const cv::Mat & image = e->data().imageRaw();
if(!image.empty() && image.depth() == CV_8U) if(!image.empty() && image.depth() == CV_8U)
{ {
cv_bridge::CvImage img; cv_bridge::CvImage img;
@@ -271,7 +259,7 @@ void callback(rtabmap_ros::CameraConfig &config, uint32_t level)
{ {
if(camera) if(camera)
{ {
camera->setParameters(config.device_id, config.frame_rate, config.width, config.height, config.video_or_images_path, config.pause); camera->setParameters(config.device_id, config.frame_rate, config.video_or_images_path, config.pause);
} }
} }
src/CoreWrapper.cpp
+487 -355
View File
File diff suppressed because it is too large Load Diff
src/CoreWrapper.h
+45 -23
View File
@@ -83,7 +83,13 @@ public:
virtual ~CoreWrapper(); virtual ~CoreWrapper();
private: private:
void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeStereo, int queueSize, bool stereoApproxSync); void setupCallbacks(
bool subscribeDepth,
bool subscribeLaserScan,
bool subscribeStereo,
int queueSize,
bool stereoApproxSync,
int depthCameras);
void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
bool commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg); bool commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
@@ -93,8 +99,14 @@ private:
void commonDepthCallback( void commonDepthCallback(
const std::string & odomFrameId, const std::string & odomFrameId,
const sensor_msgs::ImageConstPtr& imageMsg, const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg, const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg, const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg);
void commonDepthCallback(
const std::string & odomFrameId,
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
const sensor_msgs::LaserScanConstPtr& scanMsg); const sensor_msgs::LaserScanConstPtr& scanMsg);
void commonStereoCallback( void commonStereoCallback(
const std::string & odomFrameId, const std::string & odomFrameId,
@@ -129,6 +141,14 @@ private:
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg, const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg, const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg); const nav_msgs::OdometryConstPtr & odomMsg);
void depth2Callback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& image1Msg,
const sensor_msgs::ImageConstPtr& imageDepth1Msg,
const sensor_msgs::CameraInfoConstPtr& camInfo1Msg,
const sensor_msgs::ImageConstPtr& image2Msg,
const sensor_msgs::ImageConstPtr& imageDept2hMsg,
const sensor_msgs::CameraInfoConstPtr& camInfo2Msg);
// without odom, when TF is used for odom // without odom, when TF is used for odom
void depthTFCallback( void depthTFCallback(
@@ -154,25 +174,15 @@ private:
void goalCommonCallback(const std::vector<std::pair<int, rtabmap::Transform> > & poses); void goalCommonCallback(const std::vector<std::pair<int, rtabmap::Transform> > & poses);
void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg); void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
void goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
void updateGoal(const ros::Time & stamp); void updateGoal(const ros::Time & stamp);
void process( void process(
int id,
const ros::Time & stamp, const ros::Time & stamp,
const cv::Mat & image, const rtabmap::SensorData & data,
const rtabmap::Transform & odom = rtabmap::Transform(), const rtabmap::Transform & odom = rtabmap::Transform(),
const std::string & odomFrameId = "", const std::string & odomFrameId = "",
float odomRotationalVariance = 1.0f, double odomRotationalVariance = 1.0,
float odomTransitionalVariance = 1.0f, double odomTransitionalVariance = 1.0);
const cv::Mat & depthOrRightImage = cv::Mat(),
float fx = 0.0f,
float fyOrBaseline = 0.0f,
float cx = 0.0f,
float cy = 0.0f,
const rtabmap::Transform & localTransform = rtabmap::Transform(),
const cv::Mat & scan = cv::Mat(),
int scanMaxPts = 0);
bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -187,6 +197,7 @@ private:
bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); bool getGridMapCallback(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res);
bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&); bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res); bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res); bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res);
bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res); bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res);
#ifdef WITH_OCTOMAP #ifdef WITH_OCTOMAP
@@ -211,8 +222,8 @@ private:
bool paused_; bool paused_;
rtabmap::Transform lastPose_; rtabmap::Transform lastPose_;
ros::Time lastPoseStamp_; ros::Time lastPoseStamp_;
float rotVariance_; double rotVariance_;
float transVariance_; double transVariance_;
rtabmap::Transform currentMetricGoal_; rtabmap::Transform currentMetricGoal_;
bool latestNodeWasReached_; bool latestNodeWasReached_;
rtabmap::ParametersMap parameters_; rtabmap::ParametersMap parameters_;
@@ -224,6 +235,8 @@ private:
std::string databasePath_; std::string databasePath_;
bool waitForTransform_; bool waitForTransform_;
bool useActionForGoal_; bool useActionForGoal_;
bool genScan_;
double genScanMaxDepth_;
rtabmap::Transform mapToOdom_; rtabmap::Transform mapToOdom_;
boost::mutex mapToOdomMutex_; boost::mutex mapToOdomMutex_;
@@ -232,12 +245,10 @@ private:
ros::Publisher infoPub_; ros::Publisher infoPub_;
ros::Publisher mapDataPub_; ros::Publisher mapDataPub_;
ros::Publisher mapGraphPub_;
ros::Publisher labelsPub_; ros::Publisher labelsPub_;
//Planning stuff //Planning stuff
ros::Subscriber goalSub_; ros::Subscriber goalSub_;
ros::Subscriber goalGlobalSub_;
ros::Publisher nextMetricGoalPub_; ros::Publisher nextMetricGoalPub_;
ros::Publisher goalReachedPub_; ros::Publisher goalReachedPub_;
ros::Publisher globalPathPub_; ros::Publisher globalPathPub_;
@@ -247,9 +258,9 @@ private:
image_transport::Subscriber defaultSub_; image_transport::Subscriber defaultSub_;
//for depth callback //for depth callback
image_transport::SubscriberFilter imageSub_; std::vector<image_transport::SubscriberFilter*> imageSubs_;
image_transport::SubscriberFilter imageDepthSub_; std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_; std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>*> cameraInfoSubs_;
//stereo callback //stereo callback
image_transport::SubscriberFilter imageRectLeft_; image_transport::SubscriberFilter imageRectLeft_;
@@ -300,6 +311,16 @@ private:
nav_msgs::Odometry> MyStereoExactSyncPolicy; nav_msgs::Odometry> MyStereoExactSyncPolicy;
message_filters::Synchronizer<MyStereoExactSyncPolicy> * stereoExactSync_; message_filters::Synchronizer<MyStereoExactSyncPolicy> * stereoExactSync_;
typedef message_filters::sync_policies::ApproximateTime<
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepth2SyncPolicy;
message_filters::Synchronizer<MyDepth2SyncPolicy> * depth2Sync_;
// without odom, when TF is used for odom // without odom, when TF is used for odom
typedef message_filters::sync_policies::ApproximateTime< typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image, sensor_msgs::Image,
@@ -352,6 +373,7 @@ private:
ros::ServiceServer getGridMapSrv_; ros::ServiceServer getGridMapSrv_;
ros::ServiceServer publishMapDataSrv_; ros::ServiceServer publishMapDataSrv_;
ros::ServiceServer setGoalSrv_; ros::ServiceServer setGoalSrv_;
ros::ServiceServer cancelGoalSrv_;
ros::ServiceServer setLabelSrv_; ros::ServiceServer setLabelSrv_;
ros::ServiceServer listLabelsSrv_; ros::ServiceServer listLabelsSrv_;
#ifdef WITH_OCTOMAP #ifdef WITH_OCTOMAP
src/DbPlayerNode.cpp
+86 -65
View File
@@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <std_srvs/Empty.h> #include <std_srvs/Empty.h>
#include <rtabmap_ros/MsgConversion.h> #include <rtabmap_ros/MsgConversion.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/util3d_conversions.h> #include <rtabmap/core/util3d.h>
#include <rtabmap/core/DBReader.h> #include <rtabmap/core/DBReader.h>
bool paused = false; bool paused = false;
@@ -136,10 +136,10 @@ int main(int argc, char** argv)
ros::Publisher scanPub; ros::Publisher scanPub;
tf2_ros::TransformBroadcaster tfBroadcaster; tf2_ros::TransformBroadcaster tfBroadcaster;
rtabmap::SensorData data = reader.getNextData(); rtabmap::OdometryEvent odom = reader.getNextData();
while(ros::ok() && data.isValid()) while(ros::ok() && odom.data().id())
{ {
ROS_INFO("Reading sensor data %d...", data.id()); ROS_INFO("Reading sensor data %d...", odom.data().id());
ros::Time time = ros::Time::now(); ros::Time time = ros::Time::now();
@@ -159,45 +159,58 @@ int main(int argc, char** argv)
camInfoB = camInfoA; camInfoB = camInfoA;
int type = -1; int type = -1;
if(!data.depth().empty() && (data.depth().type() == CV_32FC1 || data.depth().type() == CV_16UC1)) if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
{ {
//depth if(odom.data().cameraModels().size() > 1)
camInfoA.D.resize(5,0); {
ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
}
else
{
//depth
if(odom.data().cameraModels().size())
{
camInfoA.D.resize(5,0);
camInfoA.P[0] = data.fx(); camInfoA.P[0] = odom.data().cameraModels()[0].fx();
camInfoA.K[0] = data.fx(); camInfoA.K[0] = odom.data().cameraModels()[0].fx();
camInfoA.P[5] = data.fy(); camInfoA.P[5] = odom.data().cameraModels()[0].fy();
camInfoA.K[4] = data.fy(); camInfoA.K[4] = odom.data().cameraModels()[0].fy();
camInfoA.P[2] = data.cx(); camInfoA.P[2] = odom.data().cameraModels()[0].cx();
camInfoA.K[2] = data.cx(); camInfoA.K[2] = odom.data().cameraModels()[0].cx();
camInfoA.P[6] = data.cy(); camInfoA.P[6] = odom.data().cameraModels()[0].cy();
camInfoA.K[5] = data.cy(); camInfoA.K[5] = odom.data().cameraModels()[0].cy();
camInfoB = camInfoA; camInfoB = camInfoA;
}
type=0; type=0;
if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1); if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1); if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1); 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); if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
}
} }
else if(!data.rightImage().empty() && data.rightImage().type() == CV_8U) else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
{ {
//stereo //stereo
camInfoA.D.resize(8,0); if(odom.data().stereoCameraModel().isValid())
{
camInfoA.D.resize(8,0);
camInfoA.P[0] = data.fx(); camInfoA.P[0] = odom.data().stereoCameraModel().left().fx();
camInfoA.K[0] = data.fx(); camInfoA.K[0] = odom.data().stereoCameraModel().left().fx();
camInfoA.P[5] = data.fx(); // fx = fy camInfoA.P[5] = odom.data().stereoCameraModel().left().fy();
camInfoA.K[4] = data.fx(); // fx = fy camInfoA.K[4] = odom.data().stereoCameraModel().left().fy();
camInfoA.P[2] = data.cx(); camInfoA.P[2] = odom.data().stereoCameraModel().left().cx();
camInfoA.K[2] = data.cx(); camInfoA.K[2] = odom.data().stereoCameraModel().left().cx();
camInfoA.P[6] = data.cy(); camInfoA.P[6] = odom.data().stereoCameraModel().left().cy();
camInfoA.K[5] = data.cy(); camInfoA.K[5] = odom.data().stereoCameraModel().left().cy();
camInfoB = camInfoA; camInfoB = camInfoA;
camInfoB.P[3] = data.baseline()*-data.fx(); // Right_Tx = -baseline*fx camInfoB.P[3] = odom.data().stereoCameraModel().right().Tx(); // Right_Tx = -baseline*fx
}
type=1; type=1;
@@ -212,12 +225,12 @@ int main(int argc, char** argv)
if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1); if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
} }
camInfoA.height = data.image().rows; camInfoA.height = odom.data().imageRaw().rows;
camInfoA.width = data.image().cols; camInfoA.width = odom.data().imageRaw().cols;
camInfoB.height = data.depthOrRightImage().rows; camInfoB.height = odom.data().depthOrRightRaw().rows;
camInfoB.width = data.depthOrRightImage().cols; camInfoB.width = odom.data().depthOrRightRaw().cols;
if(!data.laserScan().empty()) if(!odom.data().laserScanRaw().empty())
{ {
if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1); if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
} }
@@ -226,44 +239,52 @@ int main(int argc, char** argv)
if(publishTf) if(publishTf)
{ {
ros::Time tfExpiration = time + ros::Duration(1.0/rate); ros::Time tfExpiration = time + ros::Duration(1.0/rate);
if(!data.localTransform().isNull())
rtabmap::Transform localTransform;
if(odom.data().cameraModels().size() == 1)
{
localTransform = odom.data().cameraModels()[0].localTransform();
}
else if(odom.data().stereoCameraModel().isValid())
{
localTransform = odom.data().stereoCameraModel().left().localTransform();
}
if(!localTransform.isNull())
{ {
geometry_msgs::TransformStamped baseToCamera; geometry_msgs::TransformStamped baseToCamera;
baseToCamera.child_frame_id = cameraFrameId; baseToCamera.child_frame_id = cameraFrameId;
baseToCamera.header.frame_id = frameId; baseToCamera.header.frame_id = frameId;
baseToCamera.header.stamp = tfExpiration; baseToCamera.header.stamp = tfExpiration;
rtabmap_ros::transformToGeometryMsg(data.localTransform(), baseToCamera.transform); rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform);
tfBroadcaster.sendTransform(baseToCamera); tfBroadcaster.sendTransform(baseToCamera);
} }
if(!data.pose().isNull()) if(!odom.pose().isNull())
{ {
geometry_msgs::TransformStamped odomToBase; geometry_msgs::TransformStamped odomToBase;
odomToBase.child_frame_id = frameId; odomToBase.child_frame_id = frameId;
odomToBase.header.frame_id = odomFrameId; odomToBase.header.frame_id = odomFrameId;
odomToBase.header.stamp = tfExpiration; odomToBase.header.stamp = tfExpiration;
rtabmap_ros::transformToGeometryMsg(data.pose(), odomToBase.transform); rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform);
tfBroadcaster.sendTransform(odomToBase); tfBroadcaster.sendTransform(odomToBase);
} }
} }
if(!data.pose().isNull()) if(!odom.pose().isNull())
{ {
if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1); if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
if(odometryPub.getNumSubscribers()) if(odometryPub.getNumSubscribers())
{ {
nav_msgs::Odometry odom; nav_msgs::Odometry odomMsg;
odom.child_frame_id = frameId; odomMsg.child_frame_id = frameId;
odom.header.frame_id = odomFrameId; odomMsg.header.frame_id = odomFrameId;
odom.header.stamp = time; odomMsg.header.stamp = time;
rtabmap_ros::transformToPoseMsg(data.pose(), odom.pose.pose); rtabmap_ros::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
odom.pose.covariance[0] = data.poseTransVariance(); UASSERT(odomMsg.pose.covariance.size() == 36 &&
odom.pose.covariance[7] = data.poseTransVariance(); odom.covariance().total() == 36 &&
odom.pose.covariance[14] = data.poseTransVariance(); odom.covariance().type() == CV_64FC1);
odom.pose.covariance[21] = data.poseRotVariance(); memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
odom.pose.covariance[28] = data.poseRotVariance(); odometryPub.publish(odomMsg);
odom.pose.covariance[35] = data.poseRotVariance();
odometryPub.publish(odom);
} }
} }
@@ -290,7 +311,7 @@ int main(int argc, char** argv)
if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers()) if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
{ {
cv_bridge::CvImage img; cv_bridge::CvImage img;
if(data.image().channels() == 1) if(odom.data().imageRaw().channels() == 1)
{ {
img.encoding = sensor_msgs::image_encodings::MONO8; img.encoding = sensor_msgs::image_encodings::MONO8;
} }
@@ -298,7 +319,7 @@ int main(int argc, char** argv)
{ {
img.encoding = sensor_msgs::image_encodings::BGR8; img.encoding = sensor_msgs::image_encodings::BGR8;
} }
img.image = data.image(); img.image = odom.data().imageRaw();
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
imageRosMsg->header.frame_id = cameraFrameId; imageRosMsg->header.frame_id = cameraFrameId;
imageRosMsg->header.stamp = time; imageRosMsg->header.stamp = time;
@@ -318,10 +339,10 @@ int main(int argc, char** argv)
} }
} }
if(depthPub.getNumSubscribers() && !data.depth().empty() && type==0) if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0)
{ {
cv_bridge::CvImage img; cv_bridge::CvImage img;
if(data.depth().type() == CV_32FC1) if(odom.data().depthRaw().type() == CV_32FC1)
{ {
img.encoding = sensor_msgs::image_encodings::TYPE_32FC1; img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
} }
@@ -329,7 +350,7 @@ int main(int argc, char** argv)
{ {
img.encoding = sensor_msgs::image_encodings::TYPE_16UC1; img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
} }
img.image = data.depth(); img.image = odom.data().depthRaw();
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
imageRosMsg->header.frame_id = cameraFrameId; imageRosMsg->header.frame_id = cameraFrameId;
imageRosMsg->header.stamp = time; imageRosMsg->header.stamp = time;
@@ -338,11 +359,11 @@ int main(int argc, char** argv)
depthCamInfoPub.publish(camInfoB); depthCamInfoPub.publish(camInfoB);
} }
if(rightPub.getNumSubscribers() && !data.rightImage().empty() && type==1) if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1)
{ {
cv_bridge::CvImage img; cv_bridge::CvImage img;
img.encoding = sensor_msgs::image_encodings::MONO8; img.encoding = sensor_msgs::image_encodings::MONO8;
img.image = data.rightImage(); img.image = odom.data().rightRaw();
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg(); sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
imageRosMsg->header.frame_id = cameraFrameId; imageRosMsg->header.frame_id = cameraFrameId;
imageRosMsg->header.stamp = time; imageRosMsg->header.stamp = time;
@@ -351,9 +372,9 @@ int main(int argc, char** argv)
rightCamInfoPub.publish(camInfoB); rightCamInfoPub.publish(camInfoB);
} }
if(scanPub.getNumSubscribers() && !data.laserScan().empty()) if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().empty())
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(data.laserScan()); pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(odom.data().laserScanRaw());
sensor_msgs::PointCloud2 msg; sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(*cloud, msg); pcl::toROSMsg(*cloud, msg);
msg.header.frame_id = frameId; msg.header.frame_id = frameId;
@@ -369,7 +390,7 @@ int main(int argc, char** argv)
ros::spinOnce(); ros::spinOnce();
} }
data = reader.getNextData(); odom = reader.getNextData();
} }
src/GridMapAssemblerNode.cpp
+3 -2
View File
@@ -99,9 +99,10 @@ public:
} }
std::map<int, Transform> poses; std::map<int, Transform> poses;
for(unsigned int i=0; i<msg->graph.nodeIds.size() && i<msg->graph.poses.size(); ++i) UASSERT(msg->posesId.size() == msg->poses.size());
for(unsigned int i=0; i<msg->posesId.size(); ++i)
{ {
poses.insert(std::make_pair(msg->graph.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i]))); poses.insert(std::make_pair(msg->posesId[i], rtabmap_ros::transformFromPoseMsg(msg->poses[i])));
} }
if(filterRadius_ > 0.0 && filterAngle_ > 0.0) if(filterRadius_ > 0.0 && filterAngle_ > 0.0)
src/GuiWrapper.cpp
+1060 -600
View File
File diff suppressed because it is too large Load Diff
src/GuiWrapper.h
+180 -26
View File
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_ros/MapData.h" #include "rtabmap_ros/MapData.h"
#include "rtabmap_ros/OdomInfo.h" #include "rtabmap_ros/OdomInfo.h"
#include "rtabmap/utilite/UEventsHandler.h" #include "rtabmap/utilite/UEventsHandler.h"
#include "rtabmap/core/Transform.h"
#include <tf/transform_listener.h> #include <tf/transform_listener.h>
@@ -72,34 +73,85 @@ protected:
private: private:
void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg); void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);
void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeOdomInfo, bool subscribeStereo, int queueSize); void setupCallbacks(
void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); // odom bool subscribeDepth,
void depthCallback(const sensor_msgs::ImageConstPtr& imageMsg, bool subscribeLaserScan,
const nav_msgs::OdometryConstPtr & odomMsg, bool subscribeOdomInfo,
const sensor_msgs::ImageConstPtr& imageDepthMsg, bool subscribeStereo,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg); int queueSize,
void depthOdomInfoCallback( int depthCameras);
const sensor_msgs::ImageConstPtr& imageMsg,
void commonDepthCallback(
const nav_msgs::OdometryConstPtr & odomMsg, const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
void commonDepthCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
void commonStereoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg);
// With odom msg
void depthCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
void depth2Callback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& image1Msg,
const sensor_msgs::ImageConstPtr& depth1Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
const sensor_msgs::ImageConstPtr& image2Msg,
const sensor_msgs::ImageConstPtr& depth2Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg);
void depthOdomInfoCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg, const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg); const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
void depthScanCallback(const sensor_msgs::ImageConstPtr& imageMsg, void depthOdomInfo2Callback(
const nav_msgs::OdometryConstPtr & odomMsg, const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg, const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg, const sensor_msgs::ImageConstPtr& image1Msg,
const sensor_msgs::LaserScanConstPtr& scanMsg); const sensor_msgs::ImageConstPtr& depth1Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
const sensor_msgs::ImageConstPtr& image2Msg,
const sensor_msgs::ImageConstPtr& depth2Msg,
const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg);
void depthScanCallback(
const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
void stereoScanCallback( void stereoScanCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::LaserScanConstPtr& scanMsg, const sensor_msgs::LaserScanConstPtr& scanMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg, const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
void stereoOdomInfoCallback( void stereoOdomInfoCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg, const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const nav_msgs::OdometryConstPtr & odomMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg, const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg, const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
@@ -111,7 +163,41 @@ private:
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg, const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg); const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
// with TF
void depthTFCallback(const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
void depthOdomInfoTFCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& depthMsg,
const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
void depthScanTFCallback(
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::ImageConstPtr& imageMsg,
const sensor_msgs::ImageConstPtr& imageDepthMsg,
const sensor_msgs:: CameraInfoConstPtr& camInfoMsg);
void stereoScanTFCallback(
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
void stereoOdomInfoTFCallback(
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
void stereoTFCallback(
const sensor_msgs::ImageConstPtr& leftImageMsg,
const sensor_msgs::ImageConstPtr& rightImageMsg,
const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
void processRequestedMap(const rtabmap_ros::MapData & map); void processRequestedMap(const rtabmap_ros::MapData & map);
rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
private: private:
QApplication * app_; QApplication * app_;
@@ -121,16 +207,18 @@ private:
// odometry subscription stuffs // odometry subscription stuffs
std::string frameId_; std::string frameId_;
std::string odomFrameId_;
bool waitForTransform_; bool waitForTransform_;
tf::TransformListener tfListener_; tf::TransformListener tfListener_;
message_filters::Subscriber<rtabmap_ros::Info> infoTopic_; message_filters::Subscriber<rtabmap_ros::Info> infoTopic_;
message_filters::Subscriber<rtabmap_ros::MapData> mapDataTopic_; message_filters::Subscriber<rtabmap_ros::MapData> mapDataTopic_;
ros::Subscriber globalPathTopic_;
ros::Subscriber defaultSub_; // odometry only ros::Subscriber defaultSub_; // odometry only
image_transport::SubscriberFilter imageSub_; std::vector<image_transport::SubscriberFilter*> imageSubs_;
image_transport::SubscriberFilter imageDepthSub_; std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_; std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>* > cameraInfoSubs_;
message_filters::Subscriber<nav_msgs::Odometry> odomSub_; message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_; message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_; message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
@@ -145,25 +233,26 @@ private:
rtabmap_ros::MapData> MyInfoMapSyncPolicy; rtabmap_ros::MapData> MyInfoMapSyncPolicy;
message_filters::Synchronizer<MyInfoMapSyncPolicy> * infoMapSync_; message_filters::Synchronizer<MyInfoMapSyncPolicy> * infoMapSync_;
// with odom msg
typedef message_filters::sync_policies::ApproximateTime< typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image, sensor_msgs::LaserScan,
nav_msgs::Odometry, nav_msgs::Odometry,
sensor_msgs::Image, sensor_msgs::Image,
sensor_msgs::CameraInfo, sensor_msgs::Image,
sensor_msgs::LaserScan> MyDepthScanSyncPolicy; sensor_msgs::CameraInfo> MyDepthScanSyncPolicy;
message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_; message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
typedef message_filters::sync_policies::ApproximateTime< typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
nav_msgs::Odometry, nav_msgs::Odometry,
sensor_msgs::Image, sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthSyncPolicy; sensor_msgs::CameraInfo> MyDepthSyncPolicy;
message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_; message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
typedef message_filters::sync_policies::ApproximateTime< typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
nav_msgs::Odometry,
rtabmap_ros::OdomInfo, rtabmap_ros::OdomInfo,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::Image, sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthOdomInfoSyncPolicy; sensor_msgs::CameraInfo> MyDepthOdomInfoSyncPolicy;
message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy> * depthOdomInfoSync_; message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy> * depthOdomInfoSync_;
@@ -177,8 +266,8 @@ private:
message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_; message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
typedef message_filters::sync_policies::ApproximateTime< typedef message_filters::sync_policies::ApproximateTime<
nav_msgs::Odometry,
sensor_msgs::LaserScan, sensor_msgs::LaserScan,
nav_msgs::Odometry,
sensor_msgs::Image, sensor_msgs::Image,
sensor_msgs::Image, sensor_msgs::Image,
sensor_msgs::CameraInfo, sensor_msgs::CameraInfo,
@@ -186,13 +275,78 @@ private:
message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_; message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
typedef message_filters::sync_policies::ApproximateTime< typedef message_filters::sync_policies::ApproximateTime<
nav_msgs::Odometry,
rtabmap_ros::OdomInfo, rtabmap_ros::OdomInfo,
nav_msgs::Odometry,
sensor_msgs::Image, sensor_msgs::Image,
sensor_msgs::Image, sensor_msgs::Image,
sensor_msgs::CameraInfo, sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo> MyStereoOdomInfoSyncPolicy; sensor_msgs::CameraInfo> MyStereoOdomInfoSyncPolicy;
message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy> * stereoOdomInfoSync_; message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy> * stereoOdomInfoSync_;
typedef message_filters::sync_policies::ApproximateTime<
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepth2SyncPolicy;
message_filters::Synchronizer<MyDepth2SyncPolicy> * depth2Sync_;
typedef message_filters::sync_policies::ApproximateTime<
rtabmap_ros::OdomInfo,
nav_msgs::Odometry,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthOdomInfo2SyncPolicy;
message_filters::Synchronizer<MyDepthOdomInfo2SyncPolicy> * depthOdomInfo2Sync_;
// with odom TF
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::LaserScan,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthScanTFSyncPolicy;
message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
typedef message_filters::sync_policies::ApproximateTime<
rtabmap_ros::OdomInfo,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo> MyDepthOdomInfoTFSyncPolicy;
message_filters::Synchronizer<MyDepthOdomInfoTFSyncPolicy> * depthOdomInfoTFSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo> MyStereoTFSyncPolicy;
message_filters::Synchronizer<MyStereoTFSyncPolicy> * stereoTFSync_;
typedef message_filters::sync_policies::ApproximateTime<
sensor_msgs::LaserScan,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo> MyStereoScanTFSyncPolicy;
message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
typedef message_filters::sync_policies::ApproximateTime<
rtabmap_ros::OdomInfo,
sensor_msgs::Image,
sensor_msgs::Image,
sensor_msgs::CameraInfo,
sensor_msgs::CameraInfo> MyStereoOdomInfoTFSyncPolicy;
message_filters::Synchronizer<MyStereoOdomInfoTFSyncPolicy> * stereoOdomInfoTFSync_;
}; };
#endif /* GUIWRAPPER_H_ */ #endif /* GUIWRAPPER_H_ */
src/MapAssemblerNode.cpp
+13 -34
View File
@@ -32,7 +32,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d.h> #include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h> #include <rtabmap/core/util3d_filtering.h>
#include <rtabmap/core/util3d_mapping.h> #include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/util3d_conversions.h>
#include <rtabmap/core/Compression.h> #include <rtabmap/core/Compression.h>
#include <rtabmap/core/Graph.h> #include <rtabmap/core/Graph.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
@@ -114,41 +113,23 @@ public:
int id = msg->nodes[i].id; int id = msg->nodes[i].id;
if(!uContains(rgbClouds_, id)) if(!uContains(rgbClouds_, id))
{ {
rtabmap::Transform localTransform = rtabmap_ros::transformFromGeometryMsg(msg->nodes[i].localTransform); rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(msg->nodes[i]);
if(!localTransform.isNull()) if(!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() &&
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValid()))
{ {
cv::Mat image, depth; cv::Mat image, depth;
float fx = msg->nodes[i].fx; s.sensorData().uncompressData(&image, &depth, 0);
float fy = msg->nodes[i].fy;
float cx = msg->nodes[i].cx;
float cy = msg->nodes[i].cy;
//uncompress data
rtabmap::CompressionThread ctImage(rtabmap_ros::compressedMatFromBytes(msg->nodes[i].image, false), true);
rtabmap::CompressionThread ctDepth(rtabmap_ros::compressedMatFromBytes(msg->nodes[i].depth, false), true);
ctImage.start();
ctDepth.start();
ctImage.join();
ctDepth.join();
image = ctImage.getUncompressedData();
depth = ctDepth.getUncompressedData();
if(!image.empty() && !depth.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f) if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(depth.type() == CV_8UC1) cloud = rtabmap::util3d::cloudRGBFromSensorData(
{ s.sensorData(),
cloud = util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloudDecimation_); cloudDecimation_,
} cloudMaxDepth_);
else
{
cloud = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloudDecimation_);
}
if(cloud->size() && cloudMaxDepth_ > 0)
{
cloud = util3d::passThrough(cloud, "z", 0, cloudMaxDepth_);
}
if(cloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0) if(cloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
{ {
pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloud, noiseFilterRadius_, noiseFilterMinNeighbors_); pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
@@ -163,9 +144,6 @@ public:
if(cloud->size()) if(cloud->size())
{ {
cloud = util3d::transformPointCloud(cloud, localTransform);
rgbClouds_.insert(std::make_pair(id, cloud)); rgbClouds_.insert(std::make_pair(id, cloud));
if(computeOccupancyGrid_) if(computeOccupancyGrid_)
@@ -213,9 +191,10 @@ public:
// filter poses // filter poses
std::map<int, Transform> poses; std::map<int, Transform> poses;
for(unsigned int i=0; i<msg->graph.nodeIds.size() && i<msg->graph.poses.size(); ++i) UASSERT(msg->posesId.size() == msg->poses.size());
for(unsigned int i=0; i<msg->posesId.size(); ++i)
{ {
poses.insert(std::make_pair(msg->graph.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i]))); poses.insert(std::make_pair(msg->posesId[i], rtabmap_ros::transformFromPoseMsg(msg->poses[i])));
} }
if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0) if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0)
{ {
src/MapOptimizerNode.cpp
+11 -56
View File
@@ -117,18 +117,14 @@ public:
{ {
// save new poses and constraints // save new poses and constraints
// Assuming that nodes/constraints are all linked together // Assuming that nodes/constraints are all linked together
UASSERT(msg->graph.nodeIds.size() == msg->graph.poses.size()); UASSERT(msg->posesId.size() == msg->poses.size());
UASSERT(msg->graph.nodeIds.size() == msg->graph.mapIds.size());
UASSERT(msg->graph.nodeIds.size() == msg->graph.stamps.size());
UASSERT(msg->graph.nodeIds.size() == msg->graph.labels.size());
UASSERT(msg->graph.nodeIds.size() == msg->graph.userDatas.size());
bool dataChanged = false; bool dataChanged = false;
std::multimap<int, Link> newConstraints; std::multimap<int, Link> newConstraints;
for(unsigned int i=0; i<msg->graph.links.size(); ++i) for(unsigned int i=0; i<msg->links.size(); ++i)
{ {
Link link = rtabmap_ros::linkFromROS(msg->graph.links[i]); Link link = rtabmap_ros::linkFromROS(msg->links[i]);
newConstraints.insert(std::make_pair(link.from(), link)); newConstraints.insert(std::make_pair(link.from(), link));
bool edgeAlreadyAdded = false; bool edgeAlreadyAdded = false;
@@ -152,79 +148,48 @@ public:
} }
std::map<int, Transform> newPoses; std::map<int, Transform> newPoses;
std::map<int, int> newMapIds;
std::map<int, double> newStamps;
std::map<int, std::string> newLabels;
std::map<int, std::vector<unsigned char> > newUserDatas;
// add new odometry poses // add new odometry poses
for(unsigned int i=0; i<msg->nodes.size(); ++i) for(unsigned int i=0; i<msg->nodes.size(); ++i)
{ {
int id = msg->nodes[i].id; int id = msg->nodes[i].id;
Transform pose = rtabmap_ros::transformFromPoseMsg(msg->nodes[i].pose); Transform pose = rtabmap_ros::transformFromPoseMsg(msg->nodes[i].pose);
newPoses.insert(std::make_pair(id, pose)); newPoses.insert(std::make_pair(id, pose));
newMapIds.insert(std::make_pair(id, msg->nodes[i].mapId));
newStamps.insert(std::make_pair(id, msg->nodes[i].stamp));
newLabels.insert(std::make_pair(id, msg->nodes[i].label));
newUserDatas.insert(std::make_pair(id, msg->nodes[i].userData.data));
std::pair<std::map<int, Transform>::iterator, bool> p = cachedPoses_.insert(std::make_pair(id, pose)); std::pair<std::map<int, Transform>::iterator, bool> p = cachedPoses_.insert(std::make_pair(id, pose));
if(!p.second && pose != cachedPoses_.at(id)) if(!p.second && pose != cachedPoses_.at(id))
{ {
dataChanged = true; dataChanged = true;
} }
else if(p.second)
{
cachedMapIds_.insert(std::make_pair(id, msg->nodes[i].mapId));
cachedStamps_.insert(std::make_pair(id, msg->nodes[i].stamp));
cachedLabels_.insert(std::make_pair(id, msg->nodes[i].label));
cachedUserDatas_.insert(std::make_pair(id, msg->nodes[i].userData.data));
}
} }
if(dataChanged) if(dataChanged)
{ {
ROS_WARN("Graph data has changed! Reset cache..."); ROS_WARN("Graph data has changed! Reset cache...");
cachedPoses_ = newPoses; cachedPoses_ = newPoses;
cachedMapIds_ = newMapIds;
cachedStamps_ = newStamps;
cachedLabels_ = newLabels;
cachedUserDatas_ = newUserDatas;
cachedConstraints_ = newConstraints; cachedConstraints_ = newConstraints;
} }
//match poses in the graph //match poses in the graph
std::map<int, Transform> poses; std::map<int, Transform> poses;
std::map<int, int> mapIds;
std::map<int, double> stamps;
std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas;
std::multimap<int, Link> constraints; std::multimap<int, Link> constraints;
if(globalOptimization_) if(globalOptimization_)
{ {
poses = cachedPoses_; poses = cachedPoses_;
mapIds = cachedMapIds_;
stamps = cachedStamps_;
labels = cachedLabels_;
userDatas = cachedUserDatas_;
constraints = cachedConstraints_; constraints = cachedConstraints_;
} }
else else
{ {
constraints = newConstraints; constraints = newConstraints;
for(unsigned int i=0; i<msg->graph.nodeIds.size(); ++i) for(unsigned int i=0; i<msg->posesId.size(); ++i)
{ {
std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->graph.nodeIds[i]); std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->posesId[i]);
if(iter != cachedPoses_.end()) if(iter != cachedPoses_.end())
{ {
poses.insert(*iter); poses.insert(*iter);
mapIds.insert(*cachedMapIds_.find(iter->first));
stamps.insert(*cachedStamps_.find(iter->first));
labels.insert(*cachedLabels_.find(iter->first));
userDatas.insert(*cachedUserDatas_.find(iter->first));
} }
else else
{ {
ROS_ERROR("Odometry pose of node %d not found in cache!", msg->graph.nodeIds[i]); ROS_ERROR("Odometry pose of node %d not found in cache!", msg->posesId[i]);
return; return;
} }
} }
@@ -236,12 +201,12 @@ public:
UTimer timer; UTimer timer;
std::map<int, Transform> optimizedPoses; std::map<int, Transform> optimizedPoses;
Transform mapCorrection = Transform::getIdentity(); Transform mapCorrection = Transform::getIdentity();
std::multimap<int, rtabmap::Link> linksOut;
if(poses.size() > 1 && constraints.size() > 0) if(poses.size() > 1 && constraints.size() > 0)
{ {
graph::TOROOptimizer optimizer(iterations_, false, ignoreVariance_); graph::TOROOptimizer optimizer(iterations_, false, ignoreVariance_);
int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first; int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
std::map<int, rtabmap::Transform> posesOut; std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
optimizer.getConnectedGraph( optimizer.getConnectedGraph(
fromId, fromId,
poses, poses,
@@ -264,19 +229,13 @@ public:
ROS_ERROR("map_optimizer: Poses=%d and edges=%d (poses must " ROS_ERROR("map_optimizer: Poses=%d and edges=%d (poses must "
"not be null if there are edges, and edges must be null if poses <= 1)", "not be null if there are edges, and edges must be null if poses <= 1)",
(int)poses.size(), (int)constraints.size()); (int)poses.size(), (int)constraints.size());
mapIds.clear();
labels.clear();
stamps.clear();
userDatas.clear();
} }
UASSERT(optimizedPoses.size() == mapIds.size());
UASSERT(optimizedPoses.size() == labels.size());
UASSERT(optimizedPoses.size() == stamps.size());
UASSERT(optimizedPoses.size() == userDatas.size());
rtabmap_ros::MapData outputMsg; rtabmap_ros::MapData outputMsg;
rtabmap_ros::mapGraphToROS(optimizedPoses, mapIds, stamps, labels, userDatas, std::multimap<int, rtabmap::Link>(), mapCorrection, outputMsg.graph); rtabmap_ros::mapDataToROS(optimizedPoses,
outputMsg.graph.links = msg->graph.links; linksOut,
mapCorrection,
outputMsg);
outputMsg.header = msg->header; outputMsg.header = msg->header;
outputMsg.nodes = msg->nodes; outputMsg.nodes = msg->nodes;
mapDataPub_.publish(outputMsg); mapDataPub_.publish(outputMsg);
@@ -301,10 +260,6 @@ private:
ros::Publisher mapDataPub_; ros::Publisher mapDataPub_;
std::map<int, Transform> cachedPoses_; std::map<int, Transform> cachedPoses_;
std::map<int, int> cachedMapIds_;
std::map<int, double> cachedStamps_;
std::map<int, std::string> cachedLabels_;
std::map<int, std::vector<unsigned char> > cachedUserDatas_;
std::multimap<int, Link> cachedConstraints_; std::multimap<int, Link> cachedConstraints_;
tf2_ros::TransformBroadcaster tfBroadcaster_; tf2_ros::TransformBroadcaster tfBroadcaster_;
src/MapsManager.cpp
+19 -91
View File
@@ -71,7 +71,7 @@ MapsManager::MapsManager() :
// common map stuff // common map stuff
pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_); pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_); pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
pnh.param("map_mapsManager_cleanup", mapCacheCleanup_, mapCacheCleanup_); pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_);
// mapping topics // mapping topics
cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1); cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1);
@@ -162,7 +162,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
{ {
if(!iter->second.isNull()) if(!iter->second.isNull())
{ {
rtabmap::Signature data; rtabmap::SensorData data;
bool rgbDepthRequired = updateCloud && !uContains(clouds_, iter->first); bool rgbDepthRequired = updateCloud && !uContains(clouds_, iter->first);
bool depthRequired = updateProj && !uContains(projMaps_, iter->first); bool depthRequired = updateProj && !uContains(projMaps_, iter->first);
bool scanRequired = updateGrid && !uContains(gridMaps_, iter->first); bool scanRequired = updateGrid && !uContains(gridMaps_, iter->first);
@@ -175,7 +175,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first); std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
if(findIter != signatures.end()) if(findIter != signatures.end())
{ {
data = findIter->second; data = findIter->second.sensorData();
} }
} }
else else
@@ -186,55 +186,25 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
if(data.id() > 0) if(data.id() > 0)
{ {
rtabmap::Transform localTransform = data.getLocalTransform(); if(!data.imageCompressed().empty() &&
if(!localTransform.isNull()) !data.depthOrRightCompressed().empty() &&
(data.cameraModels().size() || data.stereoCameraModel().isValid()))
{ {
// Which data should we decompress? // Which data should we decompress?
cv::Mat image, depth, scan; cv::Mat image, depth, scan;
data.uncompressDataConst(rgbDepthRequired?&image:0, rgbDepthRequired||depthRequired?&depth:0, scanRequired?&scan:0); data.uncompressData(rgbDepthRequired||data.stereoCameraModel().isValid()?&image:0, rgbDepthRequired||depthRequired?&depth:0, scanRequired?&scan:0);
if(!depth.empty() &&
depth.type() == CV_8UC1 &&
image.empty() &&
!rgbDepthRequired)
{
// Stereo detected, we should uncompress left image too
data.uncompressDataConst(&image, 0, 0);
}
float fx = data.getFx();
float fy = data.getFy();
float cx = data.getCx();
float cy = data.getCy();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ; pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;
if(rgbDepthRequired) if(rgbDepthRequired)
{ {
if(!image.empty() && if(!image.empty() && !depth.empty())
!depth.empty() &&
fx > 0.0f && fy > 0.0f &&
cx >= 0.0f && cy >= 0.0f)
{ {
if(depth.type() == CV_8UC1) cloudRGB = util3d::cloudRGBFromSensorData(
{ data,
cloudRGB = util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloudDecimation_); cloudDecimation_,
} cloudMaxDepth_,
else cloudVoxelSize_);
{
cloudRGB = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloudDecimation_);
}
if(cloudRGB->size() && cloudMaxDepth_ > 0)
{
cloudRGB = util3d::passThrough(cloudRGB, "z", 0, cloudMaxDepth_);
}
if(cloudRGB->size() && cloudVoxelSize_ > 0)
{
cloudRGB = util3d::voxelize(cloudRGB, cloudVoxelSize_);
}
if(cloudRGB->size())
{
cloudRGB = util3d::transformPointCloud(cloudRGB, localTransform);
}
} }
else else
{ {
@@ -243,55 +213,13 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
} }
else if(depthRequired) else if(depthRequired)
{ {
if( !depth.empty() && if( !depth.empty())
fx > 0.0f && fy > 0.0f &&
cx >= 0.0f && cy >= 0.0f)
{ {
if(depth.type() == CV_8UC1) cloudXYZ = util3d::cloudFromSensorData(
{ data,
if(!image.empty()) cloudDecimation_,
{ cloudMaxDepth_,
cv::Mat leftMono; gridCellSize_); // use gridCellSize since this cloud is only for the projection map
if(image.channels() == 3)
{
cv::cvtColor(image, leftMono, CV_BGR2GRAY);
}
else
{
leftMono = image;
}
cloudXYZ = rtabmap::util3d::cloudFromDisparity(
util2d::disparityFromStereoImages(leftMono, depth),
cx, cy,
fx, fy,
cloudDecimation_);
}
}
else
{
cloudXYZ = util3d::cloudFromDepth(depth, cx, cy, fx, fy, cloudDecimation_);
}
if(cloudXYZ.get())
{
if(cloudXYZ->size() && cloudMaxDepth_ > 0)
{
cloudXYZ = util3d::passThrough(cloudXYZ, "z", 0, cloudMaxDepth_);
}
if(cloudXYZ->size() && gridCellSize_ > 0)
{
// use gridCellSize since this cloud is only for the projection map
cloudXYZ = util3d::voxelize(cloudXYZ, gridCellSize_);
}
if(cloudXYZ->size())
{
cloudXYZ = util3d::transformPointCloud(cloudXYZ, localTransform);
}
}
else
{
ROS_ERROR("Left stereo image was empty! (node=%d)", iter->first);
}
} }
else else
{ {
src/MsgConversion.cpp
+170 -81
View File
@@ -290,91 +290,91 @@ void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ro
} }
} }
void mapGraphFromROS( void mapDataFromROS(
const rtabmap_ros::Graph & msg, const rtabmap_ros::MapData & msg,
std::map<int, rtabmap::Transform> & poses,
std::multimap<int, rtabmap::Link> & links,
std::map<int, rtabmap::Signature> & signatures,
rtabmap::Transform & mapToOdom)
{
//optimized graph
mapDataFromROS(msg, poses, links, mapToOdom);
//Data
for(unsigned int i=0; i<msg.nodes.size(); ++i)
{
signatures.insert(std::make_pair(msg.nodes[i].id, nodeDataFromROS(msg.nodes[i])));
}
}
void mapDataFromROS(
const rtabmap_ros::MapData & msg,
std::map<int, rtabmap::Transform> & poses, std::map<int, rtabmap::Transform> & poses,
std::map<int, int> & mapIds,
std::map<int, double> & stamps,
std::map<int, std::string> & labels,
std::map<int, std::vector<unsigned char> > & userDatas,
std::multimap<int, rtabmap::Link> & links, std::multimap<int, rtabmap::Link> & links,
rtabmap::Transform & mapToOdom) rtabmap::Transform & mapToOdom)
{ {
mapToOdom = transformFromGeometryMsg(msg.mapToOdom); //optimized graph
UASSERT(msg.posesId.size() == msg.poses.size());
UASSERT(msg.nodeIds.size() == msg.mapIds.size()); for(unsigned int i=0; i<msg.posesId.size(); ++i)
UASSERT(msg.nodeIds.size() == msg.poses.size());
UASSERT(msg.nodeIds.size() == msg.stamps.size());
UASSERT(msg.nodeIds.size() == msg.labels.size());
UASSERT(msg.nodeIds.size() == msg.userDatas.size());
for(unsigned int i=0; i<msg.nodeIds.size(); ++i)
{ {
poses.insert(std::make_pair(msg.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg.poses[i]))); poses.insert(std::make_pair(msg.posesId[i], rtabmap_ros::transformFromPoseMsg(msg.poses[i])));
mapIds.insert(std::make_pair(msg.nodeIds[i], msg.mapIds[i]));
stamps.insert(std::make_pair(msg.nodeIds[i], msg.stamps[i]));
labels.insert(std::make_pair(msg.nodeIds[i], msg.labels[i]));
userDatas.insert(std::make_pair(msg.nodeIds[i], msg.userDatas[i].data));
} }
for(unsigned int i=0; i<msg.links.size(); ++i) for(unsigned int i=0; i<msg.links.size(); ++i)
{ {
rtabmap::Transform t = rtabmap_ros::transformFromGeometryMsg(msg.links[i].transform); rtabmap::Transform t = rtabmap_ros::transformFromGeometryMsg(msg.links[i].transform);
links.insert(std::make_pair(msg.links[i].fromId, linkFromROS(msg.links[i]))); links.insert(std::make_pair(msg.links[i].fromId, linkFromROS(msg.links[i])));
} }
mapToOdom = transformFromGeometryMsg(msg.mapToOdom);
} }
void mapGraphToROS( void mapDataToROS(
const std::map<int, rtabmap::Transform> & poses,
const std::multimap<int, rtabmap::Link> & links,
const std::map<int, rtabmap::Signature> & signatures,
const rtabmap::Transform & mapToOdom,
rtabmap_ros::MapData & msg)
{
//Optimized graph
mapDataToROS(poses, links, mapToOdom, msg);
//Data
msg.nodes.resize(signatures.size());
int index=0;
for(std::multimap<int, rtabmap::Signature>::const_iterator iter = signatures.begin();
iter!=signatures.end();
++iter)
{
nodeDataToROS(iter->second, msg.nodes[index++]);
}
}
void mapDataToROS(
const std::map<int, rtabmap::Transform> & poses, const std::map<int, rtabmap::Transform> & poses,
const std::map<int, int> & mapIds,
const std::map<int, double> & stamps,
const std::map<int, std::string> & labels,
const std::map<int, std::vector<unsigned char> > & userDatas,
const std::multimap<int, rtabmap::Link> & links, const std::multimap<int, rtabmap::Link> & links,
const rtabmap::Transform & mapToOdom, const rtabmap::Transform & mapToOdom,
rtabmap_ros::Graph & msg) rtabmap_ros::MapData & msg)
{ {
UASSERT(poses.size() == 0 || //Optimized graph
(poses.size() == mapIds.size() && msg.posesId.resize(poses.size());
poses.size() == labels.size() &&
poses.size() == stamps.size() &&
poses.size() == userDatas.size()));
transformToGeometryMsg(mapToOdom, msg.mapToOdom);
msg.nodeIds.resize(poses.size());
msg.poses.resize(poses.size()); msg.poses.resize(poses.size());
msg.mapIds.resize(poses.size());
msg.stamps.resize(poses.size());
msg.labels.resize(poses.size());
msg.userDatas.resize(poses.size());
int index = 0; int index = 0;
std::map<int, rtabmap::Transform>::const_iterator iterPoses = poses.begin(); for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin();
std::map<int, int>::const_iterator iterMapIds = mapIds.begin(); iter != poses.end();
std::map<int, double>::const_iterator iterStamps = stamps.begin(); ++iter)
std::map<int, std::string>::const_iterator iterLabels = labels.begin();
std::map<int, std::vector<unsigned char> >::const_iterator iterUserDatas = userDatas.begin();
while(iterPoses != poses.end())
{ {
msg.nodeIds[index] = iterPoses->first; msg.posesId[index] = iter->first;
msg.mapIds[index] = iterMapIds->second; transformToPoseMsg(iter->second, msg.poses[index]);
msg.stamps[index] = iterStamps->second;
msg.labels[index] = iterLabels->second;
msg.userDatas[index].data = iterUserDatas->second;
transformToPoseMsg(iterPoses->second, msg.poses[index]);
++iterPoses;
++iterMapIds;
++iterStamps;
++iterLabels;
++iterUserDatas;
++index; ++index;
} }
msg.links.resize(links.size()); msg.links.resize(links.size());
index=0; index=0;
for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter) for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin();
iter!=links.end();
++iter)
{ {
linkToROS(iter->second, msg.links[index++]); linkToROS(iter->second, msg.links[index++]);
} }
transformToGeometryMsg(mapToOdom, msg.mapToOdom);
} }
rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg) rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
@@ -404,24 +404,76 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
ROS_ERROR("Words 2D and 3D should be the same size (%d, %d)!", (int)words.size(), (int)words3D.size()); ROS_ERROR("Words 2D and 3D should be the same size (%d, %d)!", (int)words.size(), (int)words3D.size());
} }
return rtabmap::Signature( rtabmap::StereoCameraModel stereoModel;
std::vector<rtabmap::CameraModel> models;
if(msg.baseline > 0.0f)
{
// stereo model
if(msg.fx.size() == 1 &&
msg.fy.size() == 1,
msg.cx.size() == 1,
msg.cy.size() == 1,
msg.localTransform.size() == 1)
{
stereoModel = rtabmap::StereoCameraModel(
msg.fx[0],
msg.fy[0],
msg.cx[0],
msg.cy[0],
msg.baseline,
transformFromGeometryMsg(msg.localTransform[0]));
}
}
else
{
// multi-cameras model
if(msg.fx.size() &&
msg.fx.size() == msg.fy.size(),
msg.fx.size() == msg.cx.size(),
msg.fx.size() == msg.cy.size(),
msg.fx.size() == msg.localTransform.size())
{
for(unsigned int i=0; i<msg.fx.size(); ++i)
{
models.push_back(rtabmap::CameraModel(
msg.fx[i],
msg.fy[i],
msg.cx[i],
msg.cy[i],
transformFromGeometryMsg(msg.localTransform[i])));
}
}
}
rtabmap::Signature s(
msg.id, msg.id,
msg.mapId, msg.mapId,
msg.weight, msg.weight,
msg.stamp, msg.stamp,
msg.label, msg.label,
words,
words3D,
transformFromPoseMsg(msg.pose), transformFromPoseMsg(msg.pose),
msg.userData.data, stereoModel.isValid()?
compressedMatFromBytes(msg.laserScan), rtabmap::SensorData(
compressedMatFromBytes(msg.image), compressedMatFromBytes(msg.laserScan),
compressedMatFromBytes(msg.depth), msg.laserScanMaxPts,
msg.fx, compressedMatFromBytes(msg.image),
msg.fy, compressedMatFromBytes(msg.depth),
msg.cx, stereoModel,
msg.cy, msg.id,
transformFromGeometryMsg(msg.localTransform)); msg.stamp,
compressedMatFromBytes(msg.userData)):
rtabmap::SensorData(
compressedMatFromBytes(msg.laserScan),
msg.laserScanMaxPts,
compressedMatFromBytes(msg.image),
compressedMatFromBytes(msg.depth),
models,
msg.id,
msg.stamp,
compressedMatFromBytes(msg.userData)));
s.setWords(words);
s.setWords3(words3D);
return s;
} }
void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg) void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg)
{ {
@@ -431,16 +483,38 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
msg.weight = signature.getWeight(); msg.weight = signature.getWeight();
msg.stamp = signature.getStamp(); msg.stamp = signature.getStamp();
msg.label = signature.getLabel(); msg.label = signature.getLabel();
msg.userData.data = signature.getUserData();
transformToPoseMsg(signature.getPose(), msg.pose); transformToPoseMsg(signature.getPose(), msg.pose);
compressedMatToBytes(signature.getImageCompressed(), msg.image); compressedMatToBytes(signature.sensorData().imageCompressed(), msg.image);
compressedMatToBytes(signature.getDepthCompressed(), msg.depth); compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth);
compressedMatToBytes(signature.getLaserScanCompressed(), msg.laserScan); compressedMatToBytes(signature.sensorData().laserScanCompressed(), msg.laserScan);
msg.fx = signature.getFx(); compressedMatToBytes(signature.sensorData().userDataCompressed(), msg.userData);
msg.fy = signature.getFy(); msg.baseline = 0;
msg.cx = signature.getCx(); if(signature.sensorData().cameraModels().size())
msg.cy = signature.getCy(); {
transformToGeometryMsg(signature.getLocalTransform(), msg.localTransform); msg.fx.resize(signature.sensorData().cameraModels().size());
msg.fy.resize(signature.sensorData().cameraModels().size());
msg.cx.resize(signature.sensorData().cameraModels().size());
msg.cy.resize(signature.sensorData().cameraModels().size());
msg.localTransform.resize(signature.sensorData().cameraModels().size());
for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
{
msg.fx[i] = signature.sensorData().cameraModels()[i].fx();
msg.fy[i] = signature.sensorData().cameraModels()[i].fy();
msg.cx[i] = signature.sensorData().cameraModels()[i].cx();
msg.cy[i] = signature.sensorData().cameraModels()[i].cy();
transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.localTransform[i]);
}
}
else if(signature.sensorData().stereoCameraModel().isValid())
{
msg.fx.push_back(signature.sensorData().stereoCameraModel().left().fx());
msg.fy.push_back(signature.sensorData().stereoCameraModel().left().fy());
msg.cx.push_back(signature.sensorData().stereoCameraModel().left().cx());
msg.cy.push_back(signature.sensorData().stereoCameraModel().left().cy());
msg.baseline = signature.sensorData().stereoCameraModel().baseline();
msg.localTransform.resize(1);
transformToGeometryMsg(signature.sensorData().stereoCameraModel().left().localTransform(), msg.localTransform[0]);
}
//Features stuff... //Features stuff...
msg.wordIds = uKeys(signature.getWords()); msg.wordIds = uKeys(signature.getWords());
@@ -482,8 +556,12 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.features = msg.features; info.features = msg.features;
info.inliers = msg.inliers; info.inliers = msg.inliers;
info.localMapSize = msg.localMapSize; info.localMapSize = msg.localMapSize;
info.time = msg.time; info.timeEstimation = msg.timeEstimation;
info.variance = msg.variance; info.variance = msg.variance;
info.timeParticleFiltering = msg.timeParticleFiltering;
info.stamp = msg.stamp;
info.interval = msg.interval;
info.distanceTravelled = msg.distanceTravelled;
info.type = msg.type; info.type = msg.type;
@@ -500,6 +578,9 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.newCorners = points2fFromROS(msg.newCorners); info.newCorners = points2fFromROS(msg.newCorners);
info.cornerInliers = msg.cornerInliers; info.cornerInliers = msg.cornerInliers;
info.transform = transformFromGeometryMsg(msg.transform);
info.transformFiltered = transformFromGeometryMsg(msg.transformFiltered);
return info; return info;
} }
@@ -510,8 +591,13 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.features = info.features; msg.features = info.features;
msg.inliers = info.inliers; msg.inliers = info.inliers;
msg.localMapSize = info.localMapSize; msg.localMapSize = info.localMapSize;
msg.time = info.time; msg.timeEstimation = info.timeEstimation;
msg.variance = info.variance; msg.variance = info.variance;
msg.timeParticleFiltering = info.timeParticleFiltering;
msg.stamp = info.stamp;
msg.interval = info.interval;
msg.distanceTravelled = info.distanceTravelled;
msg.type = info.type; msg.type = info.type;
@@ -525,6 +611,9 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
points2fToROS(info.newCorners, msg.newCorners); points2fToROS(info.newCorners, msg.newCorners);
msg.cornerInliers = info.cornerInliers; msg.cornerInliers = info.cornerInliers;
transformToGeometryMsg(info.transform, msg.transform);
transformToGeometryMsg(info.transformFiltered, msg.transformFiltered);
} }
} }
src/OdometryROS.cpp
+36 -11
View File
@@ -72,12 +72,31 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
Transform initialPose = Transform::getIdentity(); Transform initialPose = Transform::getIdentity();
std::string initialPoseStr; std::string initialPoseStr;
std::string tfPrefix;
pnh.param("frame_id", frameId_, frameId_); pnh.param("frame_id", frameId_, frameId_);
pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
pnh.param("publish_tf", publishTf_, publishTf_); pnh.param("publish_tf", publishTf_, publishTf_);
pnh.param("tf_prefix", tfPrefix, tfPrefix);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw" pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw"
pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_); pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
if(!tfPrefix.empty())
{
if(!frameId_.empty())
{
frameId_ = tfPrefix + "/" + frameId_;
}
if(!odomFrameId_.empty())
{
odomFrameId_ = tfPrefix + "/" + odomFrameId_;
}
if(!groundTruthFrameId_.empty())
{
groundTruthFrameId_ = tfPrefix + "/" + groundTruthFrameId_;
}
}
if(initialPoseStr.size()) if(initialPoseStr.size())
{ {
std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' ')); std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' '));
@@ -157,6 +176,7 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
oldParameterNames.push_back("Odom/LocalHistory"); oldParameterNames.push_back("Odom/LocalHistory");
oldParameterNames.push_back("Odom/NearestNeighbor"); oldParameterNames.push_back("Odom/NearestNeighbor");
oldParameterNames.push_back("Odom/NNDR"); oldParameterNames.push_back("Odom/NNDR");
oldParameterNames.push_back("GFTT/MaxCorners");
for(std::list<std::string>::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter) for(std::list<std::string>::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter)
{ {
std::string vStr; std::string vStr;
@@ -192,6 +212,12 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
Parameters::kOdomBowNNDR().c_str()); Parameters::kOdomBowNNDR().c_str());
parameters_.at(Parameters::kOdomBowNNDR())= vStr; parameters_.at(Parameters::kOdomBowNNDR())= vStr;
} }
else if(iter->compare("GFTT/MaxCorners") == 0)
{
ROS_WARN("Parameter GFTT/MaxCorners doesn't exist anymore, use %s. Please update your launch file accordingly.",
Parameters::kOdomMaxFeatures().c_str());
parameters_.at(Parameters::kOdomMaxFeatures())= vStr;
}
} }
} }
@@ -276,7 +302,7 @@ void OdometryROS::processArguments(int argc, char * argv[])
} }
} }
void OdometryROS::processData(const SensorData & data, const std_msgs::Header & header) void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
{ {
if(odometry_->getPose().isNull() && if(odometry_->getPose().isNull() &&
!groundTruthFrameId_.empty()) !groundTruthFrameId_.empty())
@@ -286,13 +312,13 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
{ {
if(this->waitForTransform()) if(this->waitForTransform())
{ {
if(!this->tfListener().waitForTransform(groundTruthFrameId_, frameId_, header.stamp, ros::Duration(1))) if(!this->tfListener().waitForTransform(groundTruthFrameId_, frameId_, stamp, ros::Duration(1)))
{ {
ROS_WARN("Could not get transform from %s to %s after 1 second!", groundTruthFrameId_.c_str(), frameId_.c_str()); ROS_WARN("Could not get transform from %s to %s after 1 second!", groundTruthFrameId_.c_str(), frameId_.c_str());
return; return;
} }
} }
this->tfListener().lookupTransform(groundTruthFrameId_, frameId_, header.stamp, initialPose); this->tfListener().lookupTransform(groundTruthFrameId_, frameId_, stamp, initialPose);
} }
catch(tf::TransformException & ex) catch(tf::TransformException & ex)
{ {
@@ -319,7 +345,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
geometry_msgs::TransformStamped poseMsg; geometry_msgs::TransformStamped poseMsg;
poseMsg.child_frame_id = frameId_; poseMsg.child_frame_id = frameId_;
poseMsg.header.frame_id = odomFrameId_; poseMsg.header.frame_id = odomFrameId_;
poseMsg.header.stamp = header.stamp; poseMsg.header.stamp = stamp;
rtabmap_ros::transformToGeometryMsg(pose, poseMsg.transform); rtabmap_ros::transformToGeometryMsg(pose, poseMsg.transform);
if(publishTf_) if(publishTf_)
@@ -331,7 +357,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
{ {
//next, we'll publish the odometry message over ROS //next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom; nav_msgs::Odometry odom;
odom.header.stamp = header.stamp; // use corresponding time stamp to image odom.header.stamp = stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_; odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_; odom.child_frame_id = frameId_;
@@ -363,7 +389,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
} }
sensor_msgs::PointCloud2 cloudMsg; sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg); pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_; cloudMsg.header.frame_id = odomFrameId_;
odomLocalMap_.publish(cloudMsg); odomLocalMap_.publish(cloudMsg);
} }
@@ -377,7 +403,6 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
{ {
const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3(); const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
pcl::PointCloud<pcl::PointXYZ> cloud; pcl::PointCloud<pcl::PointXYZ> cloud;
rtabmap::Transform t = data.localTransform();
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter) for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
{ {
// transform to odom frame // transform to odom frame
@@ -387,7 +412,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
sensor_msgs::PointCloud2 cloudMsg; sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg); pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_; cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg); odomLastFrame_.publish(cloudMsg);
} }
@@ -402,7 +427,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
cloudTransformed = util3d::transformPointCloud(cloud, pose); cloudTransformed = util3d::transformPointCloud(cloud, pose);
sensor_msgs::PointCloud2 cloudMsg; sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(*cloudTransformed, cloudMsg); pcl::toROSMsg(*cloudTransformed, cloudMsg);
cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_; cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg); odomLastFrame_.publish(cloudMsg);
} }
@@ -415,7 +440,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
//send null pose to notify that odometry is lost //send null pose to notify that odometry is lost
nav_msgs::Odometry odom; nav_msgs::Odometry odom;
odom.header.stamp = header.stamp; // use corresponding time stamp to image odom.header.stamp = stamp; // use corresponding time stamp to image
odom.header.frame_id = odomFrameId_; odom.header.frame_id = odomFrameId_;
odom.child_frame_id = frameId_; odom.child_frame_id = frameId_;
@@ -427,7 +452,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
{ {
rtabmap_ros::OdomInfo infoMsg; rtabmap_ros::OdomInfo infoMsg;
odomInfoToROS(info, infoMsg); odomInfoToROS(info, infoMsg);
infoMsg.header.stamp = header.stamp; // use corresponding time stamp to image infoMsg.header.stamp = stamp; // use corresponding time stamp to image
infoMsg.header.frame_id = odomFrameId_; infoMsg.header.frame_id = odomFrameId_;
odomInfoPub_.publish(infoMsg); odomInfoPub_.publish(infoMsg);
} }
src/OdometryROS.h
+1 -1
View File
@@ -53,7 +53,7 @@ public:
~OdometryROS(); ~OdometryROS();
void processArguments(int argc, char * argv[]); void processArguments(int argc, char * argv[]);
void processData(const rtabmap::SensorData & data, const std_msgs::Header & header); void processData(const rtabmap::SensorData & data, const ros::Time & stamp);
bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::ResetPose::Response&); bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::ResetPose::Response&);
src/RGBDOdometryNode.cpp
+263 -37
View File
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_ros/MsgConversion.h" #include "rtabmap_ros/MsgConversion.h"
#include <rtabmap/core/util3d.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
using namespace rtabmap; using namespace rtabmap;
@@ -51,43 +52,112 @@ class RGBDOdometry : public rtabmap_ros::OdometryROS
public: public:
RGBDOdometry(int argc, char * argv[]) : RGBDOdometry(int argc, char * argv[]) :
rtabmap_ros::OdometryROS(argc, argv), rtabmap_ros::OdometryROS(argc, argv),
sync_(0) sync_(0),
sync2_(0)
{ {
ros::NodeHandle nh; ros::NodeHandle nh;
ros::NodeHandle pnh("~"); ros::NodeHandle pnh("~");
int queueSize = 5; int queueSize = 5;
int depthCameras = 1;
pnh.param("queue_size", queueSize, queueSize); pnh.param("queue_size", queueSize, queueSize);
pnh.param("depth_cameras", depthCameras, depthCameras);
if(depthCameras <= 0)
{
depthCameras = 1;
}
if(depthCameras > 2)
{
ROS_FATAL("Only 2 cameras maximum supported yet.");
}
ros::NodeHandle rgb_nh(nh, "rgb"); if(depthCameras == 2)
ros::NodeHandle depth_nh(nh, "depth"); {
ros::NodeHandle rgb_pnh(pnh, "rgb"); ros::NodeHandle rgb0_nh(nh, "rgb0");
ros::NodeHandle depth_pnh(pnh, "depth"); ros::NodeHandle depth0_nh(nh, "depth0");
image_transport::ImageTransport rgb_it(rgb_nh); ros::NodeHandle rgb0_pnh(pnh, "rgb0");
image_transport::ImageTransport depth_it(depth_nh); ros::NodeHandle depth0_pnh(pnh, "depth0");
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh); image_transport::ImageTransport rgb0_it(rgb0_nh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh); image_transport::ImageTransport depth0_it(depth0_nh);
image_transport::TransportHints hintsRgb0("raw", ros::TransportHints(), rgb0_pnh);
image_transport::TransportHints hintsDepth0("raw", ros::TransportHints(), depth0_pnh);
image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb); image_mono_sub_.subscribe(rgb0_it, rgb0_nh.resolveName("image"), 1, hintsRgb0);
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth); image_depth_sub_.subscribe(depth0_it, depth0_nh.resolveName("image"), 1, hintsDepth0);
info_sub_.subscribe(rgb_nh, "camera_info", 1); info_sub_.subscribe(rgb0_nh, "camera_info", 1);
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s", ros::NodeHandle rgb1_nh(nh, "rgb1");
ros::this_node::getName().c_str(), ros::NodeHandle depth1_nh(nh, "depth1");
image_mono_sub_.getTopic().c_str(), ros::NodeHandle rgb1_pnh(pnh, "rgb1");
image_depth_sub_.getTopic().c_str(), ros::NodeHandle depth1_pnh(pnh, "depth1");
info_sub_.getTopic().c_str()); image_transport::ImageTransport rgb1_it(rgb1_nh);
image_transport::ImageTransport depth1_it(depth1_nh);
image_transport::TransportHints hintsRgb1("raw", ros::TransportHints(), rgb1_pnh);
image_transport::TransportHints hintsDepth1("raw", ros::TransportHints(), depth1_pnh);
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_mono_sub_, image_depth_sub_, info_sub_); image_mono2_sub_.subscribe(rgb1_it, rgb1_nh.resolveName("image"), 1, hintsRgb1);
sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3)); image_depth2_sub_.subscribe(depth1_it, depth1_nh.resolveName("image"), 1, hintsDepth1);
info2_sub_.subscribe(rgb1_nh, "camera_info", 1);
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str(),
image_mono2_sub_.getTopic().c_str(),
image_depth2_sub_.getTopic().c_str(),
info2_sub_.getTopic().c_str());
sync2_ = new message_filters::Synchronizer<MySync2Policy>(
MySync2Policy(queueSize),
image_mono_sub_,
image_depth_sub_,
info_sub_,
image_mono2_sub_,
image_depth2_sub_,
info2_sub_);
sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
}
else
{
ros::NodeHandle rgb_nh(nh, "rgb");
ros::NodeHandle depth_nh(nh, "depth");
ros::NodeHandle rgb_pnh(pnh, "rgb");
ros::NodeHandle depth_pnh(pnh, "depth");
image_transport::ImageTransport rgb_it(rgb_nh);
image_transport::ImageTransport depth_it(depth_nh);
image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info", 1);
ROS_INFO("\n%s subscribed to:\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str());
sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_mono_sub_, image_depth_sub_, info_sub_);
sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
} }
~RGBDOdometry() ~RGBDOdometry()
{ {
delete sync_; if(sync_)
{
delete sync_;
}
if(sync2_)
{
delete sync2_;
}
} }
void callback(const sensor_msgs::ImageConstPtr& image, void callback(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth, const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo) const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{ {
@@ -118,18 +188,20 @@ public:
} }
} }
ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
tf::StampedTransform localTransform; tf::StampedTransform localTransform;
try try
{ {
if(this->waitForTransform()) if(this->waitForTransform())
{ {
if(!this->tfListener().waitForTransform(this->frameId(), image->header.frame_id, image->header.stamp, ros::Duration(1))) if(!this->tfListener().waitForTransform(this->frameId(), image->header.frame_id, stamp, ros::Duration(1)))
{ {
ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), image->header.frame_id.c_str()); ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), image->header.frame_id.c_str());
return; return;
} }
} }
this->tfListener().lookupTransform(this->frameId(), image->header.frame_id, image->header.stamp, localTransform); this->tfListener().lookupTransform(this->frameId(), image->header.frame_id, stamp, localTransform);
} }
catch(tf::TransformException & ex) catch(tf::TransformException & ex)
{ {
@@ -141,38 +213,192 @@ public:
{ {
image_geometry::PinholeCameraModel model; image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo); model.fromCameraInfo(*cameraInfo);
float fx = model.fx(); rtabmap::CameraModel rtabmapModel(
float fy = model.fy(); model.fx(),
float cx = model.cx(); model.fy(),
float cy = model.cy(); model.cx(),
model.cy(),
rtabmap_ros::transformFromTF(localTransform));
cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth); cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
rtabmap::SensorData data( rtabmap::SensorData data(
ptrImage->image, ptrImage->image,
ptrDepth->image, ptrDepth->image,
fx, rtabmapModel,
fy,
cx,
cy,
rtabmap_ros::transformFromTF(localTransform),
rtabmap::Transform(),
1.0f,
1.0f,
0, 0,
rtabmap_ros::timestampFromROS(image->header.stamp)); rtabmap_ros::timestampFromROS(stamp));
this->processData(data, image->header); this->processData(data, stamp);
} }
} }
} }
void callback2(
const sensor_msgs::ImageConstPtr& image,
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo,
const sensor_msgs::ImageConstPtr& image2,
const sensor_msgs::ImageConstPtr& depth2,
const sensor_msgs::CameraInfoConstPtr& cameraInfo2)
{
if(!this->isPaused())
{
std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfoConstPtr> infoMsgs;
imageMsgs.push_back(image);
imageMsgs.push_back(image2);
depthMsgs.push_back(depth);
depthMsgs.push_back(depth2);
infoMsgs.push_back(cameraInfo);
infoMsgs.push_back(cameraInfo2);
ros::Time higherStamp;
int imageWidth = imageMsgs[0]->width;
int imageHeight = imageMsgs[0]->height;
int cameraCount = imageMsgs.size();
cv::Mat rgb;
cv::Mat depth;
pcl::PointCloud<pcl::PointXYZ> scanCloud;
std::vector<CameraModel> cameraModels;
for(unsigned int i=0; i<imageMsgs.size(); ++i)
{
if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
{
ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
return;
}
UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight);
UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight);
ros::Time stamp = imageMsgs[i]->header.stamp>depthMsgs[i]->header.stamp?imageMsgs[i]->header.stamp:depthMsgs[i]->header.stamp;
if(i == 0)
{
higherStamp = stamp;
}
else if(stamp > higherStamp)
{
higherStamp = stamp;
}
tf::StampedTransform localTransform;
try
{
if(this->waitForTransform())
{
if(!this->tfListener().waitForTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), imageMsgs[i]->header.frame_id.c_str());
return;
}
}
this->tfListener().lookupTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp, localTransform);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
cv_bridge::CvImageConstPtr ptrImage;
if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
}
else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
}
else
{
ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
}
cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
cv::Mat subDepth = ptrDepth->image;
if(subDepth.type() == CV_32FC1)
{
subDepth = rtabmap::util3d::cvtDepthFromFloat(subDepth);
static bool shown = false;
if(!shown)
{
ROS_WARN("Use depth image with \"unsigned short\" type to "
"avoid conversion. This message is only printed once...");
shown = true;
}
}
// initialize
if(rgb.empty())
{
rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
}
if(depth.empty())
{
depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
}
if(ptrImage->image.type() == rgb.type())
{
ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
ROS_ERROR("Some RGB images are not the same type!");
return;
}
if(subDepth.type() == depth.type())
{
subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
}
else
{
ROS_ERROR("Some Depth images are not the same type!");
return;
}
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*infoMsgs[i]);
cameraModels.push_back(rtabmap::CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
rtabmap_ros::transformFromTF(localTransform)));
}
rtabmap::SensorData data(
rgb,
depth,
cameraModels,
0,
rtabmap_ros::timestampFromROS(higherStamp));
this->processData(data, higherStamp);
}
}
private: private:
image_transport::SubscriberFilter image_mono_sub_; image_transport::SubscriberFilter image_mono_sub_;
image_transport::SubscriberFilter image_depth_sub_; image_transport::SubscriberFilter image_depth_sub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_; message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
image_transport::SubscriberFilter image_mono2_sub_;
image_transport::SubscriberFilter image_depth2_sub_;
message_filters::Subscriber<sensor_msgs::CameraInfo> info2_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy; typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
message_filters::Synchronizer<MySyncPolicy> * sync_; message_filters::Synchronizer<MySyncPolicy> * sync_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySync2Policy;
message_filters::Synchronizer<MySync2Policy> * sync2_;
}; };
int main(int argc, char *argv[]) int main(int argc, char *argv[])
src/StereoOdometryNode.cpp
+22 -23
View File
@@ -132,19 +132,21 @@ public:
return; return;
} }
ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
tf::StampedTransform localTransform; tf::StampedTransform localTransform;
try try
{ {
if(this->waitForTransform()) if(this->waitForTransform())
{ {
if(!this->tfListener().waitForTransform(this->frameId(), imageRectLeft->header.frame_id, imageRectLeft->header.stamp, ros::Duration(1))) if(!this->tfListener().waitForTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, ros::Duration(1)))
{ {
ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), imageRectLeft->header.frame_id.c_str()); ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), imageRectLeft->header.frame_id.c_str());
return; return;
} }
} }
this->tfListener().lookupTransform(this->frameId(), imageRectLeft->header.frame_id, imageRectLeft->header.stamp, localTransform); this->tfListener().lookupTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, localTransform);
} }
catch(tf::TransformException & ex) catch(tf::TransformException & ex)
{ {
@@ -159,38 +161,35 @@ public:
{ {
image_geometry::StereoCameraModel model; image_geometry::StereoCameraModel model;
model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight); model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight);
if(model.baseline() <= 0)
float fx = model.left().fx();
float cx = model.left().cx();
float cy = model.left().cy();
float baseline = model.baseline();
cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
if(baseline <= 0)
{ {
ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo " ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", baseline); "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", model.baseline());
return; return;
} }
rtabmap::StereoCameraModel stereoModel(
model.left().fx(),
model.left().fy(),
model.left().cx(),
model.left().cy(),
model.baseline(),
rtabmap_ros::transformFromTF(localTransform));
cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
UTimer stepTimer; UTimer stepTimer;
// //
UDEBUG("localTransform = %s", rtabmap_ros::transformFromTF(localTransform).prettyPrint().c_str()); UDEBUG("localTransform = %s", rtabmap_ros::transformFromTF(localTransform).prettyPrint().c_str());
rtabmap::SensorData data(ptrImageLeft->image, rtabmap::SensorData data(
ptrImageLeft->image,
ptrImageRight->image, ptrImageRight->image,
fx, stereoModel,
baseline,
cx,
cy,
rtabmap_ros::transformFromTF(localTransform),
rtabmap::Transform(),
1.0f,
1.0f,
0, 0,
rtabmap_ros::timestampFromROS(imageRectLeft->header.stamp)); rtabmap_ros::timestampFromROS(stamp));
this->processData(data, imageRectLeft->header); this->processData(data, stamp);
} }
else else
{ {
src/nodelets/point_cloud_xyzrgb.cpp
+1 -1
View File
@@ -171,7 +171,7 @@ private:
if(cloudPub_.getNumSubscribers()) if(cloudPub_.getNumSubscribers())
{ {
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8"); cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth); cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
image_geometry::PinholeCameraModel model; image_geometry::PinholeCameraModel model;
src/rviz/MapCloudDisplay.cpp
+34 -55
View File
@@ -252,66 +252,45 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
if(cloud_infos_.find(id) == cloud_infos_.end()) if(cloud_infos_.find(id) == cloud_infos_.end())
{ {
// Cloud not added to RVIZ, add it! // Cloud not added to RVIZ, add it!
rtabmap::Transform localTransform = transformFromGeometryMsg(map.nodes[i].localTransform); rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
if(!localTransform.isNull()) if(!s.sensorData().imageCompressed().empty() &&
!s.sensorData().depthOrRightCompressed().empty() &&
(s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValid()))
{ {
cv::Mat image, depth; cv::Mat image, depth;
float fx = map.nodes[i].fx; s.sensorData().uncompressData(&image, &depth, 0);
float fy = map.nodes[i].fy;
float cx = map.nodes[i].cx;
float cy = map.nodes[i].cy;
//uncompress data
rtabmap::CompressionThread ctImage(compressedMatFromBytes(map.nodes[i].image, false), true);
rtabmap::CompressionThread ctDepth(compressedMatFromBytes(map.nodes[i].depth, false), true);
ctImage.start();
ctDepth.start();
ctImage.join();
ctDepth.join();
image = ctImage.getUncompressedData();
depth = ctDepth.getUncompressedData();
if(!image.empty() && !depth.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f) if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
if(depth.type() == CV_8UC1) cloud = rtabmap::util3d::cloudRGBFromSensorData(
{ s.sensorData(),
cloud = rtabmap::util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt()); cloud_decimation_->getInt(),
} cloud_max_depth_->getFloat(),
else cloud_voxel_size_->getFloat());
{
cloud = rtabmap::util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
}
if(cloud_max_depth_->getFloat() > 0.0f)
{
cloud = rtabmap::util3d::passThrough(cloud, "z", 0, cloud_max_depth_->getFloat());
}
if(cloud_voxel_size_->getFloat() > 0.0f)
{
cloud = rtabmap::util3d::voxelize(cloud, cloud_voxel_size_->getFloat());
}
cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform); if(cloud->size())
// do it after local transform
if(cloud_filter_floor_height_->getFloat() > 0.0f)
{ {
cloud = rtabmap::util3d::passThrough(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f); if(cloud_filter_floor_height_->getFloat() > 0.0f)
} {
cloud = rtabmap::util3d::passThrough(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
}
sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2); sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*cloud, *cloudMsg); pcl::toROSMsg(*cloud, *cloudMsg);
cloudMsg->header = map.header; cloudMsg->header = map.header;
CloudInfoPtr info(new CloudInfo); CloudInfoPtr info(new CloudInfo);
info->message_ = cloudMsg; info->message_ = cloudMsg;
info->pose_ = rtabmap::Transform::getIdentity(); info->pose_ = rtabmap::Transform::getIdentity();
info->id_ = id; info->id_ = id;
if (transformCloud(info, true)) if (transformCloud(info, true))
{ {
boost::mutex::scoped_lock lock(new_clouds_mutex_); boost::mutex::scoped_lock lock(new_clouds_mutex_);
new_cloud_infos_.insert(std::make_pair(id, info)); new_cloud_infos_.insert(std::make_pair(id, info));
}
} }
} }
} }
@@ -320,9 +299,9 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
// Update graph // Update graph
std::map<int, rtabmap::Transform> poses; std::map<int, rtabmap::Transform> poses;
for(unsigned int i=0; i<map.graph.nodeIds.size() && i<map.graph.poses.size(); ++i) for(unsigned int i=0; i<map.posesId.size() && i<map.poses.size(); ++i)
{ {
poses.insert(std::make_pair(map.graph.nodeIds[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i]))); poses.insert(std::make_pair(map.posesId[i], rtabmap_ros::transformFromPoseMsg(map.poses[i])));
} }
if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f) if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
@@ -503,12 +482,12 @@ void MapCloudDisplay::downloadMap()
else else
{ {
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...") messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size())); .arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
QApplication::processEvents(); QApplication::processEvents();
this->reset(); this->reset();
processMapData(getMapSrv.response.data); processMapData(getMapSrv.response.data);
messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!") messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size())); .arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
QTimer::singleShot(1000, messageBox, SLOT(close())); QTimer::singleShot(1000, messageBox, SLOT(close()));
} }
@@ -560,10 +539,10 @@ void MapCloudDisplay::downloadGraph()
} }
else else
{ {
messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size())); messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.poses.size()));
QApplication::processEvents(); QApplication::processEvents();
processMapData(getMapSrv.response.data); processMapData(getMapSrv.response.data);
messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size())); messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.poses.size()));
QTimer::singleShot(1000, messageBox, SLOT(close())); QTimer::singleShot(1000, messageBox, SLOT(close()));
} }
src/rviz/MapGraphDisplay.cpp
+3 -7
View File
@@ -95,21 +95,17 @@ void MapGraphDisplay::destroyObjects()
void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg ) void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg )
{ {
if(!(msg->graph.mapIds.size() == msg->graph.nodeIds.size() && msg->graph.poses.size() == msg->graph.nodeIds.size())) if(!(msg->poses.size() == msg->posesId.size()))
{ {
ROS_ERROR("rtabmap_ros::MapGraph: Error map ids, pose ids and poses must have all the same size."); ROS_ERROR("rtabmap_ros::MapGraph: Error pose ids and poses must have all the same size.");
return; return;
} }
// Get links // Get links
std::map<int, rtabmap::Transform> poses; std::map<int, rtabmap::Transform> poses;
std::map<int, int> mapIds;
std::map<int, double> stamps;
std::map<int, std::string> labels;
std::map<int, std::vector<unsigned char> > userDatas;
std::multimap<int, rtabmap::Link> links; std::multimap<int, rtabmap::Link> links;
rtabmap::Transform mapToOdom; rtabmap::Transform mapToOdom;
rtabmap_ros::mapGraphFromROS(msg->graph, poses, mapIds, stamps, labels, userDatas, links, mapToOdom); rtabmap_ros::mapDataFromROS(*msg, poses, links, mapToOdom);
destroyObjects(); destroyObjects();