Support on-camera apriltag detections

multisense_ros/CMakeLists.txt

@@ -50,7 +50,10 @@ add_message_files(DIRECTORY msg
                   RawLidarCal.msg
                   Histogram.msg
                   DeviceStatus.msg
-                  StampedPps.msg)
+                  StampedPps.msg
+                  AprilTagCornerPoint.msg
+                  AprilTagDetection.msg
+                  AprilTagDetectionArray.msg)
 
 generate_messages(DEPENDENCIES sensor_msgs)
 

multisense_ros/cfg/multisense.cfg

@@ -25,6 +25,7 @@ class SensorConfig(object):
 class SupportedFeatures(Enum):
     NONE = 0
     GROUND_SURFACE = 1
+    APRILTAG = 2
 
 sensorConfigList = []
 sensorConfigList.append(SensorConfig(name="sl_bm_cmv2000",      sgm=False, motor=True,  imu=False, lighting=True , aux=False, ar0234=False, features=False))
@@ -230,6 +231,10 @@ for feature_name, feature in SupportedFeatures.__members__.items():
 
             if feature == SupportedFeatures.GROUND_SURFACE:
                 gen.add("ground_surface_profile", bool_t, 0, "GroundSurfaceProfile", False);
+
+            if feature == SupportedFeatures.APRILTAG:
+                gen.add("apriltag_profile", bool_t, 0, "AprilTagProfile", False);
+
             #endif
         #endif
 
@@ -331,6 +336,26 @@ for feature_name, feature in SupportedFeatures.__members__.items():
             gen.add("ground_surface_spline_draw_resolution", double_t, 0, "Resolution to draw resulting B-Spline model with in RVIZ", 0.1, 0.01, 1.0)
         #endif
 
+        if feature == SupportedFeatures.APRILTAG:
+            apriltag_family_enum = gen.enum([gen.const("tag25h9", str_t, "tag25h9", "tag25h9"),
+                                             gen.const("tag16h5", str_t, "tag16h5", "tag16h5"),
+                                             gen.const("tag36h10", str_t, "tag36h10", "tag36h10"),
+                                             gen.const("tag36h11", str_t, "tag36h11", "tag36h11"),
+                                             gen.const("tagCircle21h7", str_t, "tagCircle21h7", "tagCircle21h7"),
+                                             gen.const("tagCircle49h12", str_t, "tagCircle49h12", "tagCircle49h12"),
+                                             gen.const("tagCustom48h12", str_t, "tagCustom48h12", "tagCustom48h12"),
+                                             gen.const("tagStandard41h12", str_t, "tagStandard41h12", "tagStandard41h12"),
+                                             gen.const("tagStandard52h13", str_t, "tagStandard52h13", "tagStandard52h13")],
+                                "Available apriltag families")
+            gen.add("apriltag_family", str_t, 0, "Apriltag family to detect", "tagStandard52h13", edit_method=apriltag_family_enum)
+            gen.add("apriltag_max_hamming", int_t, 0, "The maximum hamming correction that will be applied while detecting codes (value of 1 causes the detector to pause for a while)", 0, 0, 1)
+            gen.add("apriltag_quad_detection_blur_sigma", double_t, 0, "Sigma of the Gaussian blur applied to the image before quad_detection, specified in full resolution pixels. Kernel size = 4*sigma, rounded up to the next odd number. (<0.5 results in no blur)", 0.75, 0.0, 5.0)
+            gen.add("apriltag_quad_detection_decimate", double_t, 0, "Amount to decimate image before detecting quads. Values < 1.0. (0.5 decimation reduces height/width by half)", 1.0, 0.0, 1.0)
+            gen.add("apriltag_min_border_width", int_t, 0, "Minimum border width that can be considered a valid tag. Used to filter contours based on area and perimeter. Units are in undecimated image pixels.", 5, 0, 10)
+            gen.add("apriltag_refine_quad_edges", bool_t, 0, "Whether or not to refine the edges before attempting to decode", False)
+            gen.add("apriltag_decode_sharpening", double_t, 0, "How much to sharpen the quad before sampling the pixels. 0 to turn off, large values are stronger sharpening", 0.25, 0.0, 1.0)
+        #endif
+
         # Generate package name based on camera cfg and supported features
         if feature == SupportedFeatures.NONE:
             package_name = cfg.name

multisense_ros/include/multisense_ros/camera.h

@@ -77,6 +77,7 @@ class Camera {
     void colorizeCallback(const crl::multisense::image::Header& header);
     void groundSurfaceCallback(const crl::multisense::image::Header& header);
     void groundSurfaceSplineCallback(const crl::multisense::ground_surface::Header& header);
+    void apriltagCallback(const crl::multisense::apriltag::Header& header);
 
     void borderClipChanged(const BorderClip &borderClipType, double borderClipValue);
 
@@ -140,6 +141,7 @@ class Camera {
     static constexpr char GROUND_SURFACE_IMAGE_TOPIC[] = "image";
     static constexpr char GROUND_SURFACE_INFO_TOPIC[] = "camera_info";
     static constexpr char GROUND_SURFACE_POINT_SPLINE_TOPIC[] = "spline";
+    static constexpr char APRILTAG_DETECTION_TOPIC[] = "apriltag_detections";
 
 
     //
@@ -169,6 +171,7 @@ class Camera {
     ros::NodeHandle aux_nh_;
     ros::NodeHandle calibration_nh_;
     ros::NodeHandle ground_surface_nh_;
+    ros::NodeHandle apriltag_nh_;
 
     //
     // Image transports
@@ -222,6 +225,7 @@ class Camera {
     ros::Publisher                   aux_rect_cam_info_pub_;
     ros::Publisher                   aux_rgb_rect_cam_info_pub_;
     ros::Publisher                   ground_surface_info_pub_;
+    ros::Publisher                   apriltag_detection_pub_;
 
     ros::Publisher                   luma_point_cloud_pub_;
     ros::Publisher                   color_point_cloud_pub_;

multisense_ros/include/multisense_ros/reconfigure.h

@@ -53,8 +53,11 @@
 #include <multisense_ros/mono_cmv4000Config.h>
 #include <multisense_ros/s27_sgm_AR0234Config.h>
 #include <multisense_ros/s27_sgm_AR0234_ground_surfaceConfig.h>
+#include <multisense_ros/s27_sgm_AR0234_apriltagConfig.h>
 #include <multisense_ros/ks21_sgm_AR0234Config.h>
 #include <multisense_ros/ks21_sgm_AR0234_ground_surfaceConfig.h>
+#include <multisense_ros/ks21_sgm_AR0234_apriltagConfig.h>
+
 #include <multisense_ros/camera_utilities.h>
 #include <multisense_ros/ground_surface_utilities.h>
 
@@ -94,6 +97,8 @@ class Reconfigure {
 
     void callback_s27_AR0234_ground_surface        (multisense_ros::s27_sgm_AR0234_ground_surfaceConfig&     dyn, uint32_t level);
     void callback_ks21_AR0234_ground_surface       (multisense_ros::ks21_sgm_AR0234_ground_surfaceConfig&    dyn, uint32_t level);
+    void callback_s27_AR0234_apriltag              (multisense_ros::s27_sgm_AR0234_apriltagConfig&           dyn, uint32_t level);
+    void callback_ks21_AR0234_apriltag             (multisense_ros::ks21_sgm_AR0234_apriltagConfig&          dyn, uint32_t level);
 
     //
     // Internal helper functions
@@ -114,8 +119,10 @@ class Reconfigure {
     template<class T> void configurePtp(const T& dyn);
     template<class T> void configureStereoProfile(crl::multisense::image::Config &cfg, const T& dyn);
     template<class T> void configureStereoProfileWithGroundSurface(crl::multisense::image::Config &cfg, const T& dyn);
+    template<class T> void configureStereoProfileWithApriltag(crl::multisense::image::Config &cfg, const T& dyn);
     template<class T> void configureExtrinsics(const T& dyn);
     template<class T> void configureGroundSurfaceParams(const T& dyn);
+    template<class T> void configureApriltagParams(const T& dyn);
 
     //
     // CRL sensor API
@@ -156,6 +163,8 @@ class Reconfigure {
     std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::ks21_sgm_AR0234Config> >    server_ks21_sgm_AR0234_;
     std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234_ground_surfaceConfig> >     server_s27_AR0234_ground_surface_;
     std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::ks21_sgm_AR0234_ground_surfaceConfig> >    server_ks21_sgm_AR0234_ground_surface_;
+    std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::s27_sgm_AR0234_apriltagConfig> >           server_s27_AR0234_apriltag_;
+    std::shared_ptr< dynamic_reconfigure::Server<multisense_ros::ks21_sgm_AR0234_apriltagConfig> >          server_ks21_sgm_AR0234_apriltag_;
 
     //
     // Cached values for supported sub-systems (these may be unavailable on
@@ -192,7 +201,7 @@ class Reconfigure {
     std::function<void (crl::multisense::system::ExternalCalibration)> extrinsics_callback_;
 
     //
-    // Extrinsics callback to modify pointcloud
+    // Callback to modify spline drawing parameters
 
     std::function<void (ground_surface_utilities::SplineDrawParameters)> spline_draw_parameters_callback_;
 };

multisense_ros/msg/AprilTagCornerPoint.msg

@@ -0,0 +1,3 @@
+# Apriltag corner point in pixels
+float64 x
+float64 y

multisense_ros/msg/AprilTagDetection.msg

@@ -0,0 +1,28 @@
+# The family of the tag
+string family
+
+# The ID of the tag
+uint32 id
+
+# The hamming distance between the detection and the real code
+uint8 hamming
+
+# The quality/confidence of the binary decoding process
+# average difference between intensity of data bit vs decision thresh.
+# Higher is better. Only useful for small tags
+float32 decisionMargin
+
+# The 3x3 homography matrix (in row-major order) describing the projection
+# from an "ideal" tag (with corners at (-1,1), (1,1), (1,-1), and (-1,-1))
+# to pixels in the image
+float64[9] tagToImageHomography
+
+# The 2D position of the origin of the tag in the image
+float64[2] center
+
+# The 4 tag corner pixel locations in the order:
+# point 0: [-squareLength / 2, squareLength / 2]
+# point 1: [ squareLength / 2, squareLength / 2]
+# point 2: [ squareLength / 2, -squareLength / 2]
+# point 3: [-squareLength / 2, -squareLength / 2]
+AprilTagCornerPoint[4] corners

multisense_ros/msg/AprilTagDetectionArray.msg

@@ -0,0 +1,16 @@
+std_msgs/Header header
+
+# The frame ID of the image that the apriltags were detected on
+int64 frameId
+
+# The frame timestamp (nanoseconds) of the image that the apriltags were detected on
+int64 timestamp
+
+# Success flag to indicate whether for the apriltag algorithm ran successfully
+uint8 success
+
+# The number of apriltags that were detected
+uint32 numDetections
+
+# The collection of apriltag detections
+AprilTagDetection[] detections

multisense_ros/src/camera.cpp

@@ -50,6 +50,10 @@
 #include <multisense_ros/DeviceInfo.h>
 #include <multisense_ros/Histogram.h>
 #include <multisense_ros/point_cloud_utilities.h>
+#include <multisense_ros/AprilTagCornerPoint.h>
+#include <multisense_ros/AprilTagDetection.h>
+#include <multisense_ros/AprilTagDetectionArray.h>
+
 
 using namespace crl::multisense;
 
@@ -114,6 +118,8 @@ void groundSurfaceCB(const image::Header& header, void* userDataP)
 { reinterpret_cast<Camera*>(userDataP)->groundSurfaceCallback(header); }
 void groundSurfaceSplineCB(const ground_surface::Header& header, void* userDataP)
 { reinterpret_cast<Camera*>(userDataP)->groundSurfaceSplineCallback(header); }
+void apriltagCB(const apriltag::Header& header, void* userDataP)
+{ reinterpret_cast<Camera*>(userDataP)->apriltagCallback(header); }
 
 
 bool isValidReprojectedPoint(const Eigen::Vector3f& pt, double squared_max_range)
@@ -265,6 +271,7 @@ constexpr char Camera::COST_CAMERA_INFO_TOPIC[];
 constexpr char Camera::GROUND_SURFACE_IMAGE_TOPIC[];
 constexpr char Camera::GROUND_SURFACE_INFO_TOPIC[];
 constexpr char Camera::GROUND_SURFACE_POINT_SPLINE_TOPIC[];
+constexpr char Camera::APRILTAG_DETECTION_TOPIC[];
 
 Camera::Camera(Channel* driver, const std::string& tf_prefix) :
     driver_(driver),
@@ -274,6 +281,7 @@ Camera::Camera(Channel* driver, const std::string& tf_prefix) :
     aux_nh_(device_nh_, AUX),
     calibration_nh_(device_nh_, CALIBRATION),
     ground_surface_nh_(device_nh_, GROUND_SURFACE),
+    apriltag_nh_(device_nh_, LEFT),
     left_mono_transport_(left_nh_),
     right_mono_transport_(right_nh_),
     left_rect_transport_(left_nh_),
@@ -344,21 +352,39 @@ Camera::Camera(Channel* driver, const std::string& tf_prefix) :
     histogram_pub_      = device_nh_.advertise<multisense_ros::Histogram>(HISTOGRAM_TOPIC, 5);
 
     //
-    // Create spline-based ground surface publishers for S27/S30 cameras
+    // Create publishers for algorithms which are available
+
+    std::vector<system::DeviceMode> device_modes;
+    status = driver_->getDeviceModes(device_modes);
+    if (Status_Ok != status) {
+        ROS_ERROR("Reconfigure: failed to query device modes: %s", Channel::statusString(status));
+        return;
+    }
 
-    const bool can_support_ground_surface =
-        system::DeviceInfo::HARDWARE_REV_MULTISENSE_C6S2_S27 == device_info_.hardwareRevision ||
-        system::DeviceInfo::HARDWARE_REV_MULTISENSE_S30 == device_info_.hardwareRevision;
+    const bool ground_surface_supported =
+        std::any_of(device_modes.begin(), device_modes.end(), [](const auto &mode) {
+            return (mode.supportedDataSources & Source_Ground_Surface_Spline_Data) &&
+                   (mode.supportedDataSources & Source_Ground_Surface_Class_Image); });
 
-    if (can_support_ground_surface) {
+    if (ground_surface_supported) {
         ground_surface_cam_pub_ = ground_surface_transport_.advertise(GROUND_SURFACE_IMAGE_TOPIC, 5,
                                 std::bind(&Camera::connectStream, this, Source_Ground_Surface_Class_Image),
                                 std::bind(&Camera::disconnectStream, this, Source_Ground_Surface_Class_Image));
+
+        ground_surface_info_pub_ = ground_surface_nh_.advertise<sensor_msgs::CameraInfo>(GROUND_SURFACE_INFO_TOPIC, 1, true);
+
+        ground_surface_spline_pub_ = ground_surface_nh_.advertise<sensor_msgs::PointCloud2>(GROUND_SURFACE_POINT_SPLINE_TOPIC, 5, true);
     }
 
-    ground_surface_info_pub_ = ground_surface_nh_.advertise<sensor_msgs::CameraInfo>(GROUND_SURFACE_INFO_TOPIC, 1, true);
+    const bool apriltag_supported =
+    std::any_of(device_modes.begin(), device_modes.end(), [](const auto &mode) {
+        return mode.supportedDataSources & Source_AprilTag_Detections; });
 
-    ground_surface_spline_pub_ = ground_surface_nh_.advertise<sensor_msgs::PointCloud2>(GROUND_SURFACE_POINT_SPLINE_TOPIC, 5, true);
+    if (apriltag_supported) {
+        apriltag_detection_pub_ = apriltag_nh_.advertise<AprilTagDetectionArray>(APRILTAG_DETECTION_TOPIC, 5,
+                                  std::bind(&Camera::connectStream, this, Source_AprilTag_Detections),
+                                  std::bind(&Camera::disconnectStream, this, Source_AprilTag_Detections));
+    }
 
     //
     // Create topic publishers (TODO: color topics should not be advertised if the device can't support it)
@@ -672,7 +698,7 @@ Camera::Camera(Channel* driver, const std::string& tf_prefix) :
     static_tf_broadcaster_.sendTransform(stamped_transforms);
 
     //
-    // Update our internal image config and publish intitial camera info
+    // Update our internal image config and publish initial camera info
 
     updateConfig(image_config);
 
@@ -715,13 +741,17 @@ Camera::Camera(Channel* driver, const std::string& tf_prefix) :
     driver_->addIsolatedCallback(histCB, allImageSources, this);
 
     //
-    // Add ground surface callbacks for S27/S30 cameras
+    // Add algorithm callbacks
 
-    if (can_support_ground_surface) {
+    if (ground_surface_supported) {
         driver_->addIsolatedCallback(groundSurfaceCB, Source_Ground_Surface_Class_Image, this);
         driver_->addIsolatedCallback(groundSurfaceSplineCB, this);
     }
 
+    if (apriltag_supported) {
+        driver_->addIsolatedCallback(apriltagCB, this);
+    }
+
     //
     // Disable color point cloud strict frame syncing, if desired
 
@@ -761,6 +791,7 @@ Camera::~Camera()
 
     driver_->removeIsolatedCallback(groundSurfaceCB);
     driver_->removeIsolatedCallback(groundSurfaceSplineCB);
+    driver_->removeIsolatedCallback(apriltagCB);
 }
 
 void Camera::borderClipChanged(const BorderClip &borderClipType, double borderClipValue)
@@ -2080,6 +2111,61 @@ void Camera::groundSurfaceSplineCallback(const ground_surface::Header& header)
     ground_surface_spline_pub_.publish(ground_surface_utilities::eigenToPointcloud(eigen_pcl, frame_id_origin_));
 }
 
+void Camera::apriltagCallback(const apriltag::Header& header)
+{
+    // Convert camera timestamp to ROS timestamp
+    int64_t seconds = std::floor(header.timestamp / 1e9);
+    int64_t nanoseconds = header.timestamp - seconds * 1e9;
+    const ros::Time ros_time(static_cast<uint32_t>(seconds), static_cast<uint32_t>(nanoseconds));
+
+    // Publish Apriltags as ROS message
+    AprilTagDetectionArray tag_detection_array;
+
+    tag_detection_array.header.seq = static_cast<uint32_t>(header.frameId);
+    tag_detection_array.header.stamp = ros_time;
+    tag_detection_array.header.frame_id = frame_id_rectified_left_;
+    tag_detection_array.frameId = header.frameId;
+    tag_detection_array.timestamp = header.timestamp;
+    tag_detection_array.success = header.success;
+    tag_detection_array.numDetections = header.numDetections;
+
+    for (auto &d : header.detections)
+    {
+        AprilTagDetection tag_detection;
+
+        tag_detection.family = d.family;
+        tag_detection.id = d.id;
+        tag_detection.hamming = d.hamming;
+        tag_detection.decisionMargin = d.decisionMargin;
+
+        for (unsigned int col = 0; col < 3; col++)
+        {
+            for (unsigned int row = 0; row < 3; row++)
+            {
+                tag_detection.tagToImageHomography[row + col * 3] = d.tagToImageHomography[row][col];
+            }
+        }
+
+        for (unsigned int i = 0; i < 2; i++)
+        {
+            tag_detection.center[i] = d.center[i];
+        }
+
+        for (unsigned int i = 0; i < 4; i++)
+        {
+            AprilTagCornerPoint point;
+            point.x = d.corners[i][0];
+            point.y = d.corners[i][1];
+
+            tag_detection.corners[i] = point;
+        }
+
+        tag_detection_array.detections.push_back(tag_detection);
+    }
+
+    apriltag_detection_pub_.publish(tag_detection_array);
+}
+
 void Camera::updateConfig(const image::Config& config)
 {
     stereo_calibration_manager_->updateConfig(config);