Deeptag decode integration

zebROS_ws/src/deeptag_ros/CMakeLists.txt

@@ -201,6 +201,10 @@ target_link_libraries(run_inference_benchmark PUBLIC deeptag)
 set_target_properties(run_inference_benchmark PROPERTIES CUDA_ARCHITECTURES "86")
 set_property(TARGET run_inference_benchmark PROPERTY CUDA_SEPARABLE_COMPILATION ON)
 
+add_executable(decoder_test src/decoder_test.cpp)
+target_link_libraries(decoder_test PUBLIC deeptag)
+set_target_properties(decoder_test PROPERTIES CUDA_ARCHITECTURES "86")
+set_property(TARGET decoder_test PROPERTY CUDA_SEPARABLE_COMPILATION ON)
 # add_executable(softmax_test src/softmax_test.cpp)
 # target_link_libraries(softmax_test PUBLIC deeptag)
 # set_target_properties(softmax_test PROPERTIES CUDA_ARCHITECTURES "86")

zebROS_ws/src/deeptag_ros/include/deeptag_ros/confidence_filter.h

@@ -1,7 +1,7 @@
 #ifndef CONFIDENCE_FILTER_INC__
 #define CONFIDENCE_FILTER_INC__
 
-#include "span.hpp"
+#include "deeptag_ros/span.hpp"
 
 #ifndef __host__
 #define __host__

zebROS_ws/src/deeptag_ros/include/deeptag_ros/cuda_image_tile.h

@@ -3,7 +3,7 @@
 #include <cstdint>
 
 #include "device_types.h"
-#include "image_format.h"
+#include "deeptag_ros/image_format.h"
 
 cudaError_t cudaImageTileRGB(const void *input,
 							 const imageFormat format,

zebROS_ws/src/deeptag_ros/include/deeptag_ros/cuda_ssd_preprocess.h

@@ -1,7 +1,7 @@
 #ifndef CUDA_SSD_PREPROCESSING_H__
 #define CUDA_SSD_PREPROCESSING_H__
 
-#include "image_format.h"
+#include "deeptag_ros/image_format.h"
 
 /*
  * Downsample and apply pixel normalization, NCHW format

zebROS_ws/src/deeptag_ros/include/deeptag_ros/decoded_tag.h

@@ -3,7 +3,7 @@
 #include <array>
 #include <cstdint>
 #include "opencv2/core.hpp"
-#include "points_and_ids.h"
+#include "deeptag_ros/points_and_ids.h"
 
 template <size_t GRID_SIZE>
 class DecodedTag
@@ -13,10 +13,7 @@ class DecodedTag
     cv::Mat m_HCrop;
     int m_tagId;
     uint64_t m_binaryId;
-    std::array<PointsAndIDs, (GRID_SIZE + 2) * (GRID_SIZE + 2)> m_keypointsWithIds;
-    // TODO : this is just the points from the previous field copied into a different var
-    // see about combining them
-    std::array<cv::Point2d, (GRID_SIZE + 2) * (GRID_SIZE + 2)> m_keypointsInImage;
+    PointsAndIDs<GRID_SIZE + 2> m_keypointsWithIds;
     std::array<cv::Point2d, 4> m_roi;
 
     int m_mainIdx{0};

zebROS_ws/src/deeptag_ros/include/deeptag_ros/decoder_engine.h

@@ -1,10 +1,10 @@
 #ifndef DECODER_ENGINE_INC__
 #define DECODER_ENGINE_INC__
 
-#include "span.hpp"
+#include "deeptag_ros/span.hpp"
 
-#include "decoder_preprocess.h"
-#include "engine.h"
+#include "deeptag_ros/decoder_preprocess.h"
+#include "deeptag_ros/engine.h"
 
 class DecoderEngineCalibrator : public Int8EntropyCalibrator2
 {

zebROS_ws/src/deeptag_ros/include/deeptag_ros/decoder_preprocess.h

@@ -3,7 +3,7 @@
 #include <array>
 #include "cuda_runtime.h"  // for cudaError
 
-#include "image_format.h"
+#include "deeptag_ros/image_format.h"
 
 class DecoderPreprocess
 {

zebROS_ws/src/deeptag_ros/include/deeptag_ros/decoder_softmax.h

@@ -2,8 +2,8 @@
 #define DECODER_SOFTMAX_
 
 #include <cuda_runtime.h>
-#include "decoder_softmax_results.h"
-#include "span.hpp"
+#include "deeptag_ros/decoder_softmax_results.h"
+#include "deeptag_ros/span.hpp"
 
 
 // Class to handle DecoderSoftmax ops.

zebROS_ws/src/deeptag_ros/include/deeptag_ros/deeptag_impls.h

@@ -1,13 +1,13 @@
 #ifndef DEEPTAG_IMPLS_INC__
 #define DEEPTAG_IMPLS_INC__
 
-#include "cuda_event_timing.h"
-#include "deeptag.h"
-#include "detection_engine.h"
-#include "marker_dict.h"
-#include "pose_estimator.h"
-#include "stag_decoder.h"
-#include "stag_detector.h"
+#include "deeptag_ros/cuda_event_timing.h"
+#include "deeptag_ros/deeptag.h"
+#include "deeptag_ros/detection_engine.h"
+#include "deeptag_ros/marker_dict.h"
+#include "deeptag_ros/pose_estimator.h"
+#include "deeptag_ros/stag_decoder.h"
+#include "deeptag_ros/stag_detector.h"
 
 class DeepTagImplBase
 {

zebROS_ws/src/deeptag_ros/include/deeptag_ros/detection_engine.h

@@ -1,7 +1,7 @@
 #ifndef DETECTION_ENGINE_INC__
 #define DETECTION_ENGINE_INC__
 
-#include "engine.h"
+#include "deeptag_ros/engine.h"
 class DetectionEngineCalibrator : public Int8EntropyCalibrator2
 {
 public:

zebROS_ws/src/deeptag_ros/include/deeptag_ros/distorted_h_transform.h

@@ -4,9 +4,9 @@
 #include <array>
 #include "opencv2/core.hpp"
 #include "opencv2/calib3d.hpp"
-#include "warp_perspective_points.h"
+#include "deeptag_ros/warp_perspective_points.h"
 #undef  DEBUG
-#include "debug.h"
+#include "deeptag_ros/debug.h"
 
 template <size_t N>
 std::array<cv::Point2d, N> fromCropToUndistorted(const std::array<cv::Point2d, N> &pointsInCrop,

zebROS_ws/src/deeptag_ros/include/deeptag_ros/engine.h

@@ -16,7 +16,7 @@
 #include "NvInferRuntimeCommon.h"  // for Dims, ILogger
 #include "driver_types.h"          // for cudaStream_t, CUstream_st, CUgraph...
 
-#include "cuda_event_timing.h"     // for Timings
+#include "deeptag_ros/cuda_event_timing.h" // for Timings
 class GpuImageWrapper;
 
 // Utility methods

zebROS_ws/src/deeptag_ros/include/deeptag_ros/gpu_image_wrapper.h

@@ -3,7 +3,7 @@
 #include <cstddef>
 #include "driver_types.h"  // for cudaStream_t
 
-#include "image_format.h"
+#include "deeptag_ros/image_format.h"
 namespace cv
 {
     class Mat;

zebROS_ws/src/deeptag_ros/include/deeptag_ros/grid_prior.h

@@ -1,8 +1,8 @@
 #ifndef GRID_PRIOR_INC__
 #define GRID_PRIOR_INC__
 #include <cuda_runtime.h>
-#include "grid_prior_value.h"
-#include "span.hpp"
+#include "deeptag_ros/grid_prior_value.h"
+#include "deeptag_ros/span.hpp"
 
 // Class to handle all of the GridPrior ops.
 // Stores the grid priors (a map from model output indexes to image coords) along

zebROS_ws/src/deeptag_ros/include/deeptag_ros/image_format.h

@@ -4,6 +4,7 @@
 enum class imageFormat
 {
     IMAGE_MONO8,
+    IMAGE_MONO32F,
     IMAGE_RGB8,
     IMAGE_BGR8,
     IMAGE_RGBA8,

zebROS_ws/src/deeptag_ros/include/deeptag_ros/points_and_ids.h

@@ -2,25 +2,27 @@
 #define POINTS_AND_IDS_INC__
 #include "opencv2/core.hpp"
 
+template <size_t GRID_SIZE>
 struct PointsAndIDs
 {
     PointsAndIDs()
-        : m_point{0., 0.}
-        , m_id{-1}
-        , m_score{0.}
     {
+        std::ranges::fill(m_id, -1);
+        std::ranges::fill(m_score, -1);
     }
-    PointsAndIDs(const double x, const double y, const int id, const double score = 0)
-        : m_point{cv::Point2d{x, y}}, m_id{id}, m_score{score}
-    {
-    }
-    cv::Point2d m_point;
-    int m_id; 
-    double m_score;
+
+    std::array<cv::Point2d, GRID_SIZE * GRID_SIZE> m_point{};
+    std::array<int, GRID_SIZE * GRID_SIZE> m_id{};
+    std::array<double, GRID_SIZE * GRID_SIZE> m_score{};
+
+    size_t size(void) const { return m_point.size(); } // all arrays are the same size
 
     friend std::ostream& operator<<(std::ostream &os, const PointsAndIDs &pid)
     {
-        os << pid.m_point.x << ", " << pid.m_point.y << " id = " << pid.m_id << " score = " << pid.m_score;
+        for (size_t ii = 0; ii < pid.m_point.size(); ii++)
+        {
+            os << pid.m_point[ii].x << ", " << pid.m_point[ii].y << " id = " << pid.m_id[ii] << " score = " << pid.m_score[ii] << std::endl;
+        }
         return os;
     }
 };

zebROS_ws/src/deeptag_ros/include/deeptag_ros/pose_estimator.h

@@ -8,7 +8,7 @@
 template <bool BOOL> struct BoolTag;
 
 #undef  DEBUG
-#include "debug.h"
+#include "deeptag_ros/debug.h"
 
 template <size_t GRID_SIZE, class UNIT_TAG_TYPE, size_t STEP_ELEM_NUM, bool IS_RANSAC_SOLVEPNP, bool IS_INVERSE_X = true>
 class PoseEstimator

zebROS_ws/src/deeptag_ros/include/deeptag_ros/roi_generator.h

@@ -3,7 +3,7 @@
 
 #include <vector>
 #include <opencv2/core/types.hpp>  // for Point2d
-#include "span.hpp"
+#include "deeptag_ros/span.hpp"
 
 template <uint32_t BATCH_SIZE> class Stage1GridGroup;
 class Stage1SSDGroup;

zebROS_ws/src/deeptag_ros/include/deeptag_ros/softmax.h

@@ -3,7 +3,7 @@
 
 #include <stdint.h>        // for int32_t
 #include "driver_types.h"  // for cudaStream_t
-#include "span.hpp"        // for span
+#include "deeptag_ros/span.hpp"        // for span
 
 // Class to handle Softmax ops.
 // Input is result from model, all bg scores (h x w) followed by all fg scores for grid

zebROS_ws/src/deeptag_ros/include/deeptag_ros/ssd_grid_prior.h

@@ -6,8 +6,8 @@
 #include <vector>
 #include <cuda_runtime.h>
 
-#include "span.hpp"
-#include "ssd_box.h"
+#include "deeptag_ros/span.hpp"
+#include "deeptag_ros/ssd_box.h"
 
 // Class to handle all of the SSDGridPrior ops.
 // Stores the grid priors (a map from model output indexes to image coords) along

zebROS_ws/src/deeptag_ros/include/deeptag_ros/ssd_tag_keypoint.h

@@ -9,7 +9,7 @@
 #endif
 
 #include <cstdint>
-#include "ssd_box.h"
+#include "deeptag_ros/ssd_box.h"
 class SSDTagKeypoint
 {
 public:

zebROS_ws/src/deeptag_ros/include/deeptag_ros/stag_decoder.h

@@ -8,22 +8,22 @@
 #include <opencv2/core/types.hpp>            // for Point2d
 #include <string>                            // for string
 #include <vector>                            // for vector
-#include "decoded_tag.h"                     // for DecodedTag
-#include "decoder_engine.h"                  // for DecoderEngine
-#include "decoder_softmax.h"                 // for DecoderSoftmax
-#include "grid_prior.h"                      // for GridPrior
-#include "confidence_filter.h"               // for ConfidenceFilter
-#include "span.hpp"                          // for span
-#include "stage2_corners.h"                  // for Stage2Corners
-#include "stage2_keypoint_trust.h"           // for Stage2KeypointTrust
-#include "suppress_and_average_keypoints.h"  // for SuppressAndAverageKeypoints
+#include "deeptag_ros/decoded_tag.h"                     // for DecodedTag
+#include "deeptag_ros/decoder_engine.h"                  // for DecoderEngine
+#include "deeptag_ros/decoder_softmax.h"                 // for DecoderSoftmax
+#include "deeptag_ros/grid_prior.h"                      // for GridPrior
+#include "deeptag_ros/confidence_filter.h"               // for ConfidenceFilter
+#include "deeptag_ros/span.hpp"                          // for span
+#include "deeptag_ros/stage2_corners.h"                  // for Stage2Corners
+#include "deeptag_ros/stage2_keypoint_trust.h"           // for Stage2KeypointTrust
+#include "deeptag_ros/suppress_and_average_keypoints.h"  // for SuppressAndAverageKeypoints
 
 class GpuImageWrapper;
 struct GridPriorValue;
 class Stage2Keypoint;
 class Stage2KeypointGroup;
 class Timings;
-struct PointsAndIDs;
+template <size_t GRID_SIZE> struct PointsAndIDs;
 
 template <class MARKER_DICT, size_t GRID_SIZE>
 class STagDecoder
@@ -46,13 +46,14 @@ class STagDecoder
     void   setMinGridMatchRatio(const double minGridMatchRatio);
     double getMinGridMatchRatio(void) const;
     ushort2 getModelSize(void) const;
+    cudaStream_t getCudaStream(void);
 
 private:
     void runInference(std::vector<std::vector<Stage2KeypointGroup>> &stage2KeypointGroupss,
-                      std::vector<std::vector<float2>> &stage2Corners,
+                      std::vector<std::array<float2, 4>> &stage2Corners,
                       const std::vector<std::vector<GpuImageWrapper>> &detectInputs,
                       const tcb::span<const std::array<cv::Point2d, 4>> &rois);
-    void fillEmptyIds(std::array<PointsAndIDs, (GRID_SIZE + 2) * (GRID_SIZE + 2)> &orderedFineGridPointsIds,
+    void fillEmptyIds(PointsAndIDs<GRID_SIZE + 2> &orderedFineGridPointsIds,
                       const tcb::span<const Stage2KeypointGroup> &fineGridPointsWithIdsCandidates) const;
 
     const MARKER_DICT &m_markerDict;

zebROS_ws/src/deeptag_ros/include/deeptag_ros/stag_detector.h

@@ -7,14 +7,14 @@
 #include <memory>                            // for unique_ptr:w
 #include <string>                            // for string
 #include <vector>                            // for vector
-#include "confidence_filter.h"               // for Stage1Predicate, Confide...
+#include "deeptag_ros/confidence_filter.h"   // for Stage1Predicate, Confide...
 #include "driver_types.h"                    // for CUevent_st, cudaEvent_t
-#include "grid_prior.h"                      // for GridPrior
-#include "softmax.h"                         // for Softmax
-#include "span.hpp"                          // for span
-#include "ssd_grid_prior.h"                  // for SSDGridPrior
-#include "suppress_and_average_keypoints.h"  // for SuppressAndAverageKeypoints
-#include "tag_detect_info.h"                 // for TagDetectInfo
+#include "deeptag_ros/grid_prior.h"          // for GridPrior
+#include "deeptag_ros/softmax.h"             // for Softmax
+#include "deeptag_ros/span.hpp"              // for span
+#include "deeptag_ros/ssd_grid_prior.h"      // for SSDGridPrior
+#include "deeptag_ros/suppress_and_average_keypoints.h"  // for SuppressAndAverageKeypoints
+#include "deeptag_ros/tag_detect_info.h"     // for TagDetectInfo
 #include "vector_types.h"                    // for ushort2
 template <size_t NUM_TILES, bool USE_SCALED_IMAGE> class DetectionEngine;
 class GpuImageWrapper;

zebROS_ws/src/deeptag_ros/include/deeptag_ros/stage1_grid.h

@@ -3,7 +3,7 @@
 #include <cstdint>
 
 #include "vector_types.h"
-#include "grid_prior_value.h"
+#include "deeptag_ros/grid_prior_value.h"
 
 #ifndef __host__
 #define __host__

zebROS_ws/src/deeptag_ros/include/deeptag_ros/stage1_ssd_group.h

@@ -6,7 +6,7 @@
 #ifndef __device__
 #define __device__
 #endif
-#include "ssd_box.h"
+#include "deeptag_ros/ssd_box.h"
 
 // Used to store merged Stage1Grid similar data as a single
 // group rather than deal with duplicates of individual results

zebROS_ws/src/deeptag_ros/include/deeptag_ros/stage2_corners.h

@@ -2,8 +2,8 @@
 #define STAGE2_CORNERS_INC__
 
 #include "cuda_runtime.h"
-#include "grid_prior_value.h"
-#include "span.hpp"
+#include "deeptag_ros/grid_prior_value.h"
+#include "deeptag_ros/span.hpp"
 
 class Stage2Corners
 {
@@ -18,7 +18,7 @@ class Stage2Corners
 
     virtual ~Stage2Corners();
     void compute(const float *input, const tcb::span<const GridPriorValue> &priors, const float centerVariance, cudaStream_t cudaStream);
-    const tcb::span<const float2> getDeviceOutput(void);
+    // const tcb::span<const float2> getDeviceOutput(void);
     const tcb::span<const float2> getHostOutput(void);
 
 private:

zebROS_ws/src/deeptag_ros/include/deeptag_ros/stage2_keypoint.h

@@ -1,7 +1,7 @@
 #ifndef STAGE2_KEYPOINT_INC__
 #define STAGE2_KEYPOINT_INC__
 #include <cstdint>
-#include "grid_prior_value.h"
+#include "deeptag_ros/grid_prior_value.h"
 
 #ifndef __host__
 #define __host__

zebROS_ws/src/deeptag_ros/include/deeptag_ros/stage2_keypoint_trust.h

@@ -1,8 +1,8 @@
 #ifndef STAGE2_KEYPOINT_TRUST_INC__
 #define STAGE2_KEYPOINT_TRUST_INC__
 #include <cuda_runtime.h>
-#include "stage2_keypoint.h"
-#include "span.hpp"
+#include "deeptag_ros/stage2_keypoint.h"
+#include "deeptag_ros/span.hpp"
 
 // Given a list of keypoints, check that enough of them
 // have a high enough confidence that we trust that the group

zebROS_ws/src/deeptag_ros/include/deeptag_ros/suppress_and_average_keypoints.h

@@ -1,7 +1,7 @@
 #ifndef SUPPRESS_AND_AVERAGE_KEYPOINTS_H__
 #define SUPPRESS_AND_AVERAGE_KEYPOINTS_H__
 
-#include "span.hpp"
+#include "deeptag_ros/span.hpp"
 
 template <class INPUT, class OUTPUT>
 class SuppressAndAverageKeypoints