Added SaveObjectsExample node (added optional dependency on find_object_2d). Updated demo_find_object.launch. rtabmap: Added mutex around user data access.

CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS
 find_package(costmap_2d)
 find_package(octomap_ros)
 find_package(rviz)
+find_package(find_object_2d)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -122,6 +123,9 @@ ENDIF(octomap_ros_FOUND)
 IF(rviz_FOUND)		
    SET(optional_dependencies ${optional_dependencies} rviz)		
 ENDIF(rviz_FOUND)
+IF(find_object_2d_FOUND)		
+   SET(optional_dependencies ${optional_dependencies} find_object_2d)		
+ENDIF(find_object_2d_FOUND)	
 
 catkin_package(
   INCLUDE_DIRS include
@@ -310,6 +314,14 @@ target_link_libraries(wifi_signal_pub rtabmap_ros ${Libraries})
 add_executable(wifi_signal_sub src/WifiSignalSubNode.cpp)
 target_link_libraries(wifi_signal_sub rtabmap_ros ${Libraries})
 
+# If find_object_2d is found, add save objects example
+IF(find_object_2d_FOUND)
+MESSAGE(STATUS "WITH find_object_2d")
+include_directories(${find_object_2d_INCLUDE_DIRS})
+add_executable(save_objects_example src/SaveObjectsExample.cpp)
+target_link_libraries(save_objects_example rtabmap_ros ${Libraries} ${find_object_2d_LIBRARIES})
+ENDIF(find_object_2d_FOUND)
+
 add_executable(camera src/CameraNode.cpp)
 add_dependencies(camera ${${PROJECT_NAME}_EXPORTED_TARGETS})
 target_link_libraries(camera ${Libraries})

include/rtabmap_ros/CoreWrapper.h

@@ -272,6 +272,7 @@ private:
 
 	ros::Subscriber userDataAsyncSub_;
 	cv::Mat userData_;
+	UMutex userDataMutex_;
 
 	ros::Subscriber globalPoseAsyncSub_;
 	geometry_msgs::PoseWithCovarianceStamped globalPose_;

launch/config/demo_find_object.rviz

@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /Global Options1
         - /TF1/Frames1
-      Splitter Ratio: 0.434783
+      Splitter Ratio: 0.434783012
     Tree Height: 590
   - Class: rviz/Selection
     Name: Selection
@@ -15,7 +15,7 @@ Panels:
       - /2D Pose Estimate1
       - /2D Nav Goal1
     Name: Tool Properties
-    Splitter Ratio: 0.588679
+    Splitter Ratio: 0.588679016
   - Class: rviz/Views
     Expanded:
       - /Current View1
@@ -36,7 +36,7 @@ Visualization Manager:
       Color: 160; 160; 164
       Enabled: true
       Line Style:
-        Line Width: 0.03
+        Line Width: 0.0299999993
         Value: Lines
       Name: Grid
       Normal Cell Count: 0
@@ -51,8 +51,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 2.02684
+        Max Value: 2.02683997
-        Min Value: -1.12646
+        Min Value: -1.12645996
         Value: true
       Axis: Z
       Channel Name: rgb
@@ -63,17 +63,18 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 2.34177e-38
+      Max Intensity: 2.34176998e-38
       Min Color: 0; 0; 0
-      Min Intensity: 9.21942e-41
+      Min Intensity: 9.21942286e-41
       Name: PointCloud2
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
       Size (Pixels): 3
-      Size (m): 0.01
+      Size (m): 0.00999999978
       Style: Points
       Topic: /voxel_cloud
+      Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
@@ -100,16 +101,6 @@ Visualization Manager:
           Value: true
         map:
           Value: true
-        object_4:
-          Value: true
-        object_5:
-          Value: true
-        object_6:
-          Value: true
-        object_7:
-          Value: true
-        object_8:
-          Value: true
         odom:
           Value: true
         wheelLB_linkWheel_link:
@@ -146,16 +137,7 @@ Visualization Manager:
                       {}
                   camera_rgb_frame:
                     camera_rgb_optical_frame:
-                      object_4:
+                      {}
-                        {}
-                      object_5:
-                        {}
-                      object_6:
-                        {}
-                      object_7:
-                        {}
-                      object_8:
-                        {}
                 wheelLB_linkWheel_link:
                   wheelLB_wheel_link:
                     {}
@@ -173,8 +155,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: -0.574923
+        Max Value: 4.7127676
-        Min Value: -5.11672
+        Min Value: 0.0444712937
         Value: true
       Axis: X
       Channel Name: intensity
@@ -193,19 +175,22 @@ Visualization Manager:
       Queue Size: 10
       Selectable: true
       Size (Pixels): 5
-      Size (m): 0.01
+      Size (m): 0.00999999978
       Style: Points
       Topic: /base_scan
+      Unreliable: false
       Use Fixed Frame: false
       Use rainbow: true
       Value: true
-    - Alpha: 0.7
+    - Alpha: 0.699999988
       Class: rviz/Map
       Color Scheme: map
       Draw Behind: false
       Enabled: true
       Name: Map
       Topic: /rtabmap/grid_map
+      Unreliable: false
+      Use Timestamp: false
       Value: true
     - Alpha: 1
       Class: rviz/RobotModel
@@ -233,6 +218,7 @@ Visualization Manager:
       Normalize Range: true
       Queue Size: 2
       Transport Hint: compressed
+      Unreliable: false
       Value: false
     - Alpha: 1
       Autocompute Intensity Bounds: true
@@ -245,12 +231,14 @@ Visualization Manager:
       Class: rtabmap_ros/MapCloud
       Cloud decimation: 4
       Cloud max depth (m): 3
-      Cloud voxel size (m): 0.02
+      Cloud min depth (m): 0
+      Cloud voxel size (m): 0.0199999996
       Color: 255; 255; 255
       Color Transformer: RGB8
       Download graph: false
       Download map: false
       Enabled: true
+      Filter ceiling (m): 0
       Filter floor (m): 0
       Invert Rainbow: false
       Max Color: 255; 255; 255
@@ -259,12 +247,13 @@ Visualization Manager:
       Min Intensity: 0
       Name: MapCloud
       Node filtering angle (degrees): 20
-      Node filtering radius (m): 0.2
+      Node filtering radius (m): 0.200000003
       Position Transformer: XYZ
       Size (Pixels): 3
-      Size (m): 0.01
+      Size (m): 0.00999999978
       Style: Points
       Topic: /rtabmap/mapData
+      Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
@@ -272,10 +261,20 @@ Visualization Manager:
       Enabled: true
       Name: Info
       Topic: /rtabmap/info
+      Unreliable: false
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /objectsMarkers
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
+    Default Light: true
     Fixed Frame: map
     Frame Rate: 30
   Name: root
@@ -293,22 +292,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rtabmap_ros/OrbitOriented
-      Distance: 7.89956
+      Distance: 7.89955997
       Enable Stereo Rendering:
-        Stereo Eye Separation: 0.06
+        Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.0199225
+        X: 0.0199225005
-        Y: 0.23516
+        Y: 0.235159993
-        Z: -0.0896359
+        Z: -0.0896359012
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.0500000007
+      Invert Z Axis: false
       Name: Current View
-      Near Clip Distance: 0.01
+      Near Clip Distance: 0.00999999978
-      Pitch: 0.819797
+      Pitch: 0.819796979
       Target Frame: base_link
       Value: OrbitOriented (rtabmap)
-      Yaw: 3.14375
+      Yaw: 3.14374995
     Saved: ~
 Window Geometry:
   Displays:
@@ -318,7 +320,7 @@ Window Geometry:
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001b0000002ddfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000000000002dd000000dd00fffffffb0000000a0049006d006100670065000000022f000000ae0000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000377000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000463000002dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001b0000002ddfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000000000002dd000000d700fffffffb0000000a0049006d006100670065000000022f000000ae0000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000377000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000459000002dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -327,6 +329,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1561
+  Width: 1551
-  X: 42
+  X: 49
   Y: 107

launch/demo/demo_find_object.launch

@@ -4,13 +4,17 @@
   <!-- Choose visualization -->
   <arg name="rviz" default="true" />
   <arg name="rtabmapviz" default="false" /> 
+  <arg name="save_objects" default="false"/>
+  <arg name="localization" default="false"/>
+  <arg     if="$(arg localization)" name="rtabmap_args" default=""/>
+  <arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
 
   <param name="use_sim_time" type="bool" value="True"/>
   
   <!-- SLAM (robot side) -->
   <!-- args: "delete_db_on_start" and "udebug" -->
   <group ns="rtabmap">
-    <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="$(arg rtabmap_args)">
 	  <param name="frame_id" type="string" value="base_footprint"/>
 	
 	  <param name="subscribe_depth" type="bool" value="true"/>
@@ -18,6 +22,7 @@
 	
 	  <remap from="odom" to="/base_controller/odom"/>
 	  <remap from="scan" to="/base_scan"/>
+	  <remap if="$(arg save_objects)" from="user_data_async" to="/objectsData"/>
 
 	  <remap from="rgb/image"       to="/camera/data_throttled_image"/>
   	  <remap from="depth/image"     to="/camera/data_throttled_image_depth"/>
@@ -46,6 +51,7 @@
 	  <param name="Mem/NotLinkedNodesKept"    type="string" value="false"/>
 	  <param name="Optimizer/Slam2D"          type="string" value="true"/>
 	  <param name="Reg/Force3DoF"             type="string" value="true"/>   
+	  <param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
     </node>
     
     <!-- Visualisation RTAB-Map -->
@@ -101,5 +107,11 @@
     <param name="rgb/image_transport"              type="string" value="compressed"/>
     <param name="depth_registered/image_transport" type="string" value="compressedDepth"/>
   </node>
+  
+  <!-- Save objects to database example -->
+  <node if="$(arg save_objects)" name="save_objects_example" pkg="rtabmap_ros" type="save_objects_example" output="screen">
+    <remap from="mapData" to="/rtabmap/mapData"/>
+    <param name="frame_id" value="base_footprint"/>
+  </node>
 
 </launch>

package.xml

@@ -40,6 +40,7 @@
   <build_depend>costmap_2d</build_depend>
   <build_depend>octomap_ros</build_depend>
   <build_depend>image_geometry</build_depend>
+  <build_depend>find_object_2d</build_depend>
 
   <run_depend>cv_bridge</run_depend>
   <run_depend>roscpp</run_depend>
@@ -71,6 +72,7 @@
   <run_depend>costmap_2d</run_depend>
   <run_depend>octomap_ros</run_depend>
   <run_depend>image_geometry</run_depend>
+  <run_depend>find_object_2d</run_depend>
 
   <build_depend>libpcl-all-dev</build_depend>
 

src/CoreWrapper.cpp

@@ -1117,6 +1117,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 	if(userDataMsg.get())
 	{
 		userData = rtabmap_ros::userDataFromROS(*userDataMsg);
+		UScopeMutex lock(userDataMutex_);
 		if(!userData_.empty())
 		{
 			NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
@@ -1125,6 +1126,7 @@ void CoreWrapper::commonDepthCallbackImpl(
 	}
 	else
 	{
+		UScopeMutex lock(userDataMutex_);
 		userData = userData_;
 		userData_ = cv::Mat();
 	}
@@ -1363,6 +1365,7 @@ void CoreWrapper::commonStereoCallback(
 	if(userDataMsg.get())
 	{
 		userData = rtabmap_ros::userDataFromROS(*userDataMsg);
+		UScopeMutex lock(userDataMutex_);
 		if(!userData_.empty())
 		{
 			NODELET_WARN("Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!");
@@ -1371,6 +1374,7 @@ void CoreWrapper::commonStereoCallback(
 	}
 	else
 	{
+		UScopeMutex lock(userDataMutex_);
 		userData = userData_;
 		userData_ = cv::Mat();
 	}
@@ -1663,6 +1667,7 @@ void CoreWrapper::userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & da
 {
 	if(!paused_)
 	{
+		UScopeMutex lock(userDataMutex_);
 		if(!userData_.empty())
 		{
 			ROS_WARN("Overwriting previous user data set. Asynchronous user "
@@ -1670,10 +1675,7 @@ void CoreWrapper::userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & da
 					"lower rate than map update rate (current %s=%f).",
 					Parameters::kRtabmapDetectionRate().c_str(), rate_);
 		}
-		else
+		userData_ = rtabmap_ros::userDataFromROS(*dataMsg);
-		{
-			userData_ = rtabmap_ros::userDataFromROS(*dataMsg);
-		}
 	}
 }
 
@@ -1912,8 +1914,10 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
 	latestNodeWasReached_ = false;
 	mapsManager_.clear();
 	previousStamp_ = ros::Time(0);
-	userData_ = cv::Mat();
 	globalPose_.header.stamp = ros::Time(0);
+	userDataMutex_.lock();
+	userData_ = cv::Mat();
+	userDataMutex_.unlock();
 	return true;
 }
 
@@ -1967,7 +1971,9 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
 	currentMetricGoal_.setNull();
 	lastPublishedMetricGoal_.setNull();
 	latestNodeWasReached_ = false;
+	userDataMutex_.lock();
 	userData_ = cv::Mat();
+	userDataMutex_.unlock();
 	globalPose_.header.stamp = ros::Time(0);
 
 	NODELET_INFO("Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str());

src/SaveObjectsExample.cpp

@@ -0,0 +1,257 @@
+/*
+Copyright (c) 2011-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+#include <find_object_2d/ObjectsStamped.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <opencv2/core.hpp>
+#include <rtabmap_ros/UserData.h>
+#include <rtabmap_ros/MsgConversion.h>
+#include <rtabmap/core/Compression.h>
+#include <visualization_msgs/MarkerArray.h>
+
+class SaveObjectsExample
+{
+public:
+	SaveObjectsExample() :
+        objFramePrefix_("object"),
+        frameId_("base_link")
+    {
+        ros::NodeHandle pnh("~");
+        pnh.param("object_prefix", objFramePrefix_, objFramePrefix_);
+        pnh.param("frame_id", frameId_, frameId_);
+
+        ros::NodeHandle nh;
+        subObjects_ = nh.subscribe("objectsStamped", 1, &SaveObjectsExample::objectsDetectedCallback, this);
+        subsMapData_ = nh.subscribe("mapData", 1, &SaveObjectsExample::mapDataCallback, this);
+        pub_ = nh.advertise<rtabmap_ros::UserData>("objectsData", 1);
+        pubMarkers_ = nh.advertise<visualization_msgs::MarkerArray>("objectsMarkers", 1);
+    }
+
+    // from find_object_2d to rtabmap
+    void objectsDetectedCallback(const find_object_2d::ObjectsStampedConstPtr & msg)
+    {
+        if(msg->objects.data.size())
+        {
+            cv::Mat data(msg->objects.data.size()/12, 9, CV_64FC1);
+            for(unsigned int i=0; i<msg->objects.data.size(); i+=12)
+            {
+                // get data
+                int id = (int)msg->objects.data[i];
+                std::string objectFrameId = uFormat("%s_%d", objFramePrefix_.c_str(),id); // "object_1", "object_2"
+
+                // get pose of the object in base frame
+                tf::StampedTransform pose;
+                try
+                {
+                    tfListener_.lookupTransform(frameId_, objectFrameId, msg->header.stamp, pose);
+                }
+                catch(tf::TransformException & ex)
+                {
+                    ROS_WARN("%s",ex.what());
+                    return;
+                }
+
+                data.at<double>(i, 0) = double(id);
+                data.at<double>(i, 1) = ros::Time(msg->header.stamp).toSec();
+                data.at<double>(i, 2) = pose.getOrigin().x();
+                data.at<double>(i, 3) = pose.getOrigin().y();
+                data.at<double>(i, 4) = pose.getOrigin().z();
+                data.at<double>(i, 5) = pose.getRotation().x();
+                data.at<double>(i, 6) = pose.getRotation().y();
+                data.at<double>(i, 7) = pose.getRotation().z();
+                data.at<double>(i, 8) = pose.getRotation().w();
+            }
+
+            rtabmap_ros::UserData dataMsg;
+            dataMsg.header.frame_id = msg->header.frame_id;
+            dataMsg.header.stamp = msg->header.stamp;
+            rtabmap_ros::userDataToROS(data, dataMsg, false);
+            pub_.publish(dataMsg);
+        }
+    }
+
+    // from rtabmap to rviz visualization
+    void mapDataCallback(const rtabmap_ros::MapDataConstPtr & msg)
+    {
+        std::map<double, int> nodeStamps; // <stamp, id>
+        std::map<int, rtabmap::Signature> signatures;
+        std::map<int, rtabmap::Transform> poses;
+        std::multimap<int, rtabmap::Link> links;
+        rtabmap::Transform mapToOdom;
+        rtabmap_ros::mapDataFromROS(*msg, poses, links, signatures, mapToOdom);
+        for(std::map<int, rtabmap::Signature>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
+        {
+            int id = iter->first;
+            rtabmap::Signature & node = iter->second;
+            if(!node.sensorData().userDataCompressed().empty() && nodeToObjects_.find(id)==nodeToObjects_.end())
+            {
+                cv::Mat data = rtabmap::uncompressData(node.sensorData().userDataCompressed());
+                ROS_ASSERT(data.cols == 9 && data.type() == CV_64FC1);
+                ROS_INFO("Node %d has %d object(s)", id, data.rows);
+                nodeToObjects_.insert(std::make_pair(id, data));
+            }
+            // Sort stamps by stamps->id
+            nodeStamps.insert(std::make_pair(iter->second.getStamp(), iter->first));
+        }
+
+        // Publish markers accordingly to current optimized graph
+        std::map<int, float> objectsAdded;
+        visualization_msgs::MarkerArray markers;
+
+        if(nodeToObjects_.size())
+        {
+            for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+            {
+                if(nodeToObjects_.find(iter->first) != nodeToObjects_.end())
+                {
+                    cv::Mat data = nodeToObjects_.at(iter->first);
+                    for(int i=0; i<data.rows; ++i)
+                    {
+                        int objId = int(data.at<double>(i,0));
+                        double stamp = data.at<double>(i,1);
+
+                        // The object detection may have been taken between two nodes, interpolate its position.
+                        std::map<double, int>::iterator previousNode = nodeStamps.lower_bound(stamp); // lower bound of the stamp
+                        if(previousNode!=nodeStamps.end() && previousNode->first > stamp && previousNode != nodeStamps.begin())
+                        {
+                            --previousNode;
+                        }
+                        std::map<double, int>::iterator nextNode = nodeStamps.upper_bound(stamp); // upper bound of the stamp
+
+                        if(previousNode != nodeStamps.end() &&
+                           nextNode != nodeStamps.end() &&
+                           previousNode->second != nextNode->second &&
+                           poses.find(previousNode->second)!=poses.end() && poses.find(nextNode->second)!=poses.end())
+                        {
+                            rtabmap::Transform poseA = poses.at(previousNode->second);
+                            rtabmap::Transform poseB = poses.at(nextNode->second);
+                            double stampA = previousNode->first;
+                            double stampB = nextNode->first;
+                            UASSERT(stamp>=stampA && stamp <=stampB);
+
+                            rtabmap::Transform v = poseA.inverse() * poseB;
+                            double ratio = (stamp-stampA)/(stampB-stampA);
+                            rtabmap::Transform robotPose = poseA.interpolate(ratio, poseB);
+
+                            // transform object pose in map frame
+                            rtabmap::Transform objPose(
+                                    data.at<double>(i,2), data.at<double>(i,3), data.at<double>(i,4),
+                                    data.at<double>(i,5), data.at<double>(i,6), data.at<double>(i,7), data.at<double>(i,8));
+                            float distanceFromNode = objPose.getNorm();
+                            objPose = robotPose * objPose;
+
+                            if(objectsAdded.find(objId) == objectsAdded.end())
+                            {
+                                visualization_msgs::Marker marker;
+                                marker.header.frame_id = msg->header.frame_id;
+                                marker.header.stamp = msg->header.stamp;
+                                marker.ns = "objects";
+                                marker.id = objId;
+                                marker.action = visualization_msgs::Marker::ADD;
+                                marker.pose.position.x = objPose.x();
+                                marker.pose.position.y = objPose.y();
+                                marker.pose.position.z = objPose.z();
+                                Eigen::Quaterniond q = objPose.getQuaterniond();
+                                marker.pose.orientation.x = q.x();
+                                marker.pose.orientation.y = q.y();
+                                marker.pose.orientation.z = q.z();
+                                marker.pose.orientation.w = q.w();
+                                marker.scale.x = 0.3;
+                                marker.scale.y = 0.3;
+                                marker.scale.z = 0.3;
+                                marker.color.a = 0.8;
+                                marker.color.r = 0.0;
+                                marker.color.g = 1.0;
+                                marker.color.b = 0.0;
+
+                                // cube
+                                marker.type = visualization_msgs::Marker::CUBE;
+                                markers.markers.push_back(marker);
+
+                                // text
+                                marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+                                marker.text = uFormat("%s_%d", objFramePrefix_.c_str(), objId);
+                                marker.id = -objId;
+                                marker.color.a = 1.0;
+                                marker.pose.position.z += 0.3; // text over the cube
+                                markers.markers.push_back(marker);
+
+                                objectsAdded.insert(std::make_pair(objId, distanceFromNode));
+                            }
+                            else
+                            {
+                                // update pose of the marker only if current observation is closer to robot
+                                if(distanceFromNode < objectsAdded.at(objId))
+                                {
+                                    for(unsigned int j=0; j<markers.markers.size(); ++j)
+                                    {
+                                        if(markers.markers[j].id == objId || markers.markers[j].id == -objId)
+                                        {
+                                            markers.markers[j].pose.position.x = objPose.x();
+                                            markers.markers[j].pose.position.y = objPose.y();
+                                            markers.markers[j].pose.position.z = objPose.z() + (markers.markers[j].id<0?0.3:0);
+                                            Eigen::Quaterniond q = objPose.getQuaterniond();
+                                            markers.markers[j].pose.orientation.x = q.x();
+                                            markers.markers[j].pose.orientation.y = q.y();
+                                            markers.markers[j].pose.orientation.z = q.z();
+                                            markers.markers[j].pose.orientation.w = q.w();
+                                        }
+                                    }
+                                    objectsAdded.at(objId) = distanceFromNode;
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+            if(!markers.markers.empty())
+            {
+                pubMarkers_.publish(markers);
+            }
+        }
+    }
+
+private:
+    std::string objFramePrefix_;
+    ros::Subscriber subObjects_;
+    ros::Subscriber subsMapData_;
+    tf::TransformListener tfListener_;
+    ros::Publisher pub_;
+    ros::Publisher pubMarkers_;
+    std::map<int, cv::Mat> nodeToObjects_;
+    std::string frameId_;
+};
+
+int main(int argc, char * argv[])
+{
+    ros::init(argc, argv, "save_objects_example");
+
+    SaveObjectsExample sync;
+    ros::spin();
+}