updated demo_find_object.launch with save_objects_as_landmarks argument. Added script objects_to_tags.py. Scripts are now installed in binaries. Updated tag conversion to use frame_id/stamp included in detection array instead of parent if set.

CMakeLists.txt

@@ -428,10 +428,14 @@ set_target_properties(rtabmap_pointcloud_to_depthimage PROPERTIES OUTPUT_NAME "p
 
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
+install(PROGRAMS
-#   scripts/my_python_script
+  scripts/patrol.py
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  scripts/objects_to_tags.py
-# )
+  scripts/point_to_tf.py
+  scripts/transform_to_tf.py
+  scripts/yaml_to_camera_info.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 ## Mark executables and/or libraries for installation
 IF(RTABMAP_GUI)

launch/config/demo_find_object.rviz

@@ -7,7 +7,7 @@ Panels:
         - /Global Options1
         - /TF1/Frames1
       Splitter Ratio: 0.434783012
-    Tree Height: 590
+    Tree Height: 592
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -27,6 +27,8 @@ Panels:
     Name: Time
     SyncMode: 0
     SyncSource: LaserScan
+Toolbars:
+  toolButtonStyle: 2
 Visualization Manager:
   Class: ""
   Displays:
@@ -65,7 +67,7 @@ Visualization Manager:
       Max Color: 255; 255; 255
       Max Intensity: 2.34176998e-38
       Min Color: 0; 0; 0
-      Min Intensity: 9.21942286e-41
+      Min Intensity: 0
       Name: PointCloud2
       Position Transformer: XYZ
       Queue Size: 10
@@ -101,6 +103,16 @@ 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:
@@ -137,6 +149,15 @@ 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:
@@ -155,8 +176,8 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 4.7127676
+        Max Value: -0.662045956
-        Min Value: 0.0444712937
+        Min Value: -5.20883799
         Value: true
       Axis: X
       Channel Name: intensity
@@ -230,6 +251,7 @@ Visualization Manager:
       Channel Name: intensity
       Class: rtabmap_ros/MapCloud
       Cloud decimation: 4
+      Cloud from scan: false
       Cloud max depth (m): 3
       Cloud min depth (m): 0
       Cloud voxel size (m): 0.0199999996
@@ -271,6 +293,32 @@ Visualization Manager:
         {}
       Queue Size: 100
       Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /rtabmap/labels
+      Name: MarkerArray
+      Namespaces:
+        ids: false
+        labels: false
+        landmarks: true
+      Queue Size: 100
+      Value: true
+    - Alpha: 1
+      Arrow Length: 0.300000012
+      Axes Length: 0.300000012
+      Axes Radius: 0.100000001
+      Class: rviz/PoseArray
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.0700000003
+      Head Radius: 0.0299999993
+      Name: PoseArray
+      Shaft Length: 0.230000004
+      Shaft Radius: 0.00999999978
+      Shape: Axes
+      Topic: /rtabmap/landmarks
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -292,35 +340,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rtabmap_ros/OrbitOriented
-      Distance: 7.89955997
+      Distance: 7.50918865
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.0199225005
+        X: 0.0776291937
-        Y: 0.235159993
+        Y: 0.0206621774
-        Z: -0.0896359012
+        Z: 0.317859411
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.819796979
+      Pitch: 0.909796953
       Target Frame: base_link
       Value: OrbitOriented (rtabmap)
-      Yaw: 3.14374995
+      Yaw: 3.73874164
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 763
+  Height: 765
   Hide Left Dock: false
   Hide Right Dock: false
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000001b0000002ddfc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000000000002dd000000d700fffffffb0000000a0049006d006100670065000000022f000000ae0000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000377000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000459000002dd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001b0000002dffc0200000007fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000000000002df000000d700fffffffb0000000a0049006d006100670065000000022f000000ae0000001600fffffffb0000000a0049006d006100670065010000027d000000fa0000000000000000000000010000010f00000377fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000000000000377000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000002ed000002df00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -329,6 +377,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1551
+  Width: 1187
-  X: 49
+  X: 699
-  Y: 107
+  Y: 344

launch/demo/demo_find_object.launch

@@ -6,6 +6,7 @@
   <arg name="rtabmapviz" default="false" /> 
   <arg name="save_objects" default="false"/>
   <arg name="localization" default="false"/>
+  <arg name="save_objects_as_landmarks" default="false"/> <!-- apriltag_ros package should be installed and rtabmap_ros built with it -->
   <arg     if="$(arg localization)" name="rtabmap_args" default=""/>
   <arg unless="$(arg localization)" name="rtabmap_args" default="--delete_db_on_start"/>
 
@@ -32,6 +33,8 @@
       <param name="depth/image_transport" type="string" value="compressedDepth"/>
 	
 	  <param name="queue_size" type="int" value="10"/>
+	  <param name="landmark_linear_variance" type="double" value="0.1"/>
+	  <param name="landmark_angular_variance" type="double" value="0.5"/>
 	
 	  <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
 	  <param name="RGBD/NeighborLinkRefining" type="string" value="true"/>  <!-- Do odometry correction with consecutive laser scans -->
@@ -114,4 +117,10 @@
     <param name="frame_id" value="base_footprint"/>
   </node>
   
+  <!-- Convert objects to tags -->
+  <node if="$(arg save_objects_as_landmarks)" name="objects_to_tags" pkg="rtabmap_ros" type="objects_to_tags.py" output="screen">
+    <remap from="tag_detections" to="/rtabmap/tag_detections"/>
+    <param name="distance_max" value="2.0"/>
+  </node>
+
 </launch>

launch/tests/test_apriltag_ros.launch

@@ -2,11 +2,11 @@
   <!-- Print on the file tag_bundle1.png (with GIMP: set size in Image Settings to 160x240mm) -->
   <!-- Definition of the tag bundle is tags.yaml, make sure the camera is calibrated or adjust the size of the tag if needed -->
   <!-- The TF published by apriltag_ros should match the point cloud created by the camera -->
-  <!-- Change Optimizer/Strategy below between 1 (g2o) and 2 (GTSAM), and tag_angular_variance between 0.005 (optimize rotation) and 9999 (optimize only tag's XYZ) -->
+  <!-- Change Optimizer/Strategy below between 1 (g2o) and 2 (GTSAM), and landmark_angular_variance between 0.005 (optimize rotation) and 9999 (optimize only tag's XYZ) -->
 
   <!-- $ roslaunch realsense2_camera rs_camera.launch align_depth:=true -->
   <!-- $ roslaunch rtabmap_ros test_apriltag_ros.launch rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info -->
-  <!-- $ roslaunch rtabmap_ros rtabmap.launch depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info rviz:=true rtabmapviz:=false args:="-d -Optimizer/Strategy 1" tag_angular_variance:=9999 -->
+  <!-- $ roslaunch rtabmap_ros rtabmap.launch depth_topic:=/camera/aligned_depth_to_color/image_raw rgb_topic:=/camera/color/image_raw camera_info_topic:=/camera/color/camera_info rviz:=true rtabmapviz:=false args:="-d -Optimizer/Strategy 1" landmark_angular_variance:=9999 -->
 
   <arg name="camera_frame_id"         default="camera_color_optical_frame"/>
   <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />

scripts/objects_to_tags.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+import rospy
+from apriltag_ros.msg import AprilTagDetectionArray
+from apriltag_ros.msg import AprilTagDetection
+from find_object_2d.msg import ObjectsStamped
+import tf
+import geometry_msgs.msg
+
+objFramePrefix_ = "object"
+distanceMax_ = 0.0
+
+def callback(data):
+    global objFramePrefix_
+    global distanceMax_
+    if len(data.objects.data) > 0:
+        output = AprilTagDetectionArray()
+        output.header = data.header
+        for i in range(0,len(data.objects.data),12):
+            try:
+                objId = data.objects.data[i]
+                (trans,quat) = listener.lookupTransform(data.header.frame_id, objFramePrefix_+'_'+str(int(objId)), data.header.stamp)
+                tag = AprilTagDetection()
+                tag.id.append(objId)
+                tag.pose.pose.pose.position.x = trans[0]
+                tag.pose.pose.pose.position.y = trans[1]
+                tag.pose.pose.pose.position.z = trans[2]
+                tag.pose.pose.pose.orientation.x = quat[0]
+                tag.pose.pose.pose.orientation.y = quat[1]
+                tag.pose.pose.pose.orientation.z = quat[2]
+                tag.pose.pose.pose.orientation.w = quat[3]
+                tag.pose.header = output.header
+                if distanceMax_ <= 0.0 or trans[2] < distanceMax_:
+                    output.detections.append(tag)
+            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+                continue
+        if len(output.detections) > 0:
+            pub.publish(output)
+
+if __name__ == '__main__':
+    pub = rospy.Publisher('tag_detections', AprilTagDetectionArray, queue_size=10)
+    rospy.init_node('objects_to_tags', anonymous=True)
+    rospy.Subscriber("objectsStamped", ObjectsStamped, callback)
+    objFramePrefix_ = rospy.get_param('~object_prefix', objFramePrefix_)
+    distanceMax_ = rospy.get_param('~distance_max', distanceMax_)
+    listener = tf.TransformListener()
+    rospy.spin()

src/CoreWrapper.cpp

@@ -2009,7 +2009,15 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_ros::AprilTagDetecti
 			if(tagDetections.detections[i].id.size() >= 1)
 			{
 				geometry_msgs::PoseWithCovarianceStamped p = tagDetections.detections[i].pose;
-				p.header.frame_id = tagDetections.header.frame_id; // make sure we pass frame_id of parent
+				p.header = tagDetections.header;
+				if(!tagDetections.detections[i].pose.header.frame_id.empty())
+				{
+					p.header.frame_id = tagDetections.detections[i].pose.header.frame_id;
+				}
+				if(!tagDetections.detections[i].pose.header.stamp.isZero())
+				{
+					p.header.stamp = tagDetections.detections[i].pose.header.stamp;
+				}
 				uInsert(tags_, std::make_pair(tagDetections.detections[i].id[0], p));
 			}
 		}