From aeeb493028200e524ab10c52fa76f88fbeeb013f Mon Sep 17 00:00:00 2001 From: Nino Wegleitner Date: Sat, 13 May 2023 22:05:43 +0000 Subject: [PATCH 1/9] Add TrackletConverter --- depthai_bridge/CMakeLists.txt | 1 + .../depthai_bridge/TrackletConverter.hpp | 37 ++++++++ depthai_bridge/src/TrackletConverter.cpp | 88 +++++++++++++++++++ 3 files changed, 126 insertions(+) create mode 100644 depthai_bridge/include/depthai_bridge/TrackletConverter.hpp create mode 100644 depthai_bridge/src/TrackletConverter.cpp diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt index 9d5a11f3..098dafbf 100644 --- a/depthai_bridge/CMakeLists.txt +++ b/depthai_bridge/CMakeLists.txt @@ -47,6 +47,7 @@ FILE(GLOB LIB_SRC "src/ImageConverter.cpp" "src/ImgDetectionConverter.cpp" "src/SpatialDetectionConverter.cpp" +"src/TrackletConverter.cpp" "src/ImuConverter.cpp" ) diff --git a/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp b/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp new file mode 100644 index 00000000..f1663dae --- /dev/null +++ b/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include +#include +#include + +#include "depthai/pipeline/datatype/Tracklets.hpp" +#include "depthai_ros_msgs/TrackletArray.h" +#include "ros/ros.h" +#include "vision_msgs/Detection3DArray.h" + +namespace dai { + +namespace ros { +namespace TrackletMessages = depthai_ros_msgs; +using TrackletArrayPtr = TrackletMessages::TrackletArray::Ptr; +class TrackletConverter { + public: + TrackletConverter(std::string frameName, int width, int height, bool normalized = false, bool getBaseDeviceTimestamp = false); + void toRosMsg(std::shared_ptr trackData, std::deque& trackletsMsg); + void toRosVisionMsg(std::shared_ptr trackData, std::deque& trackletsMsg); + TrackletArrayPtr toRosMsgPtr(std::shared_ptr trackData); + + private: + int _width, _height; + const std::string _frameName; + bool _normalized; + std::chrono::time_point _steadyBaseTime; + ::ros::Time _rosBaseTime; + bool _getBaseDeviceTimestamp; +}; + +} // namespace ros + +namespace rosBridge = ros; + +} // namespace dai diff --git a/depthai_bridge/src/TrackletConverter.cpp b/depthai_bridge/src/TrackletConverter.cpp new file mode 100644 index 00000000..6c91bfba --- /dev/null +++ b/depthai_bridge/src/TrackletConverter.cpp @@ -0,0 +1,88 @@ +#include "depthai_bridge/TrackletConverter.hpp" +#include "depthai_bridge/depthaiUtility.hpp" +#include +#include + +namespace dai { +namespace ros { + +TrackletConverter::TrackletConverter(std::string frameName, int width, int height, bool normalized, bool getBaseDeviceTimestamp) + : _frameName(frameName), + _width(width), + _height(height), + _normalized(normalized), + _steadyBaseTime(std::chrono::steady_clock::now()), + _getBaseDeviceTimestamp(getBaseDeviceTimestamp) { + _rosBaseTime = ::ros::Time::now(); +} + +void TrackletConverter::toRosMsg(std::shared_ptr trackData, std::deque& trackletMsgs) { + // setting the header + std::chrono::_V2::steady_clock::time_point tstamp; + if(_getBaseDeviceTimestamp) + tstamp = trackData->getTimestampDevice(); + else + tstamp = trackData->getTimestamp(); + + TrackletMessages::TrackletArray trackletsMsg; + + trackletsMsg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp); + trackletsMsg.header.frame_id = _frameName; + trackletsMsg.tracklets.resize(trackData->tracklets.size()); + + // publishing + for(int i = 0; i < trackData->tracklets.size(); ++i) { + int xMin, yMin, xMax, yMax; + if(_normalized) { + xMin = trackData->tracklets[i].srcImgDetection.xmin; + yMin = trackData->tracklets[i].srcImgDetection.ymin; + xMax = trackData->tracklets[i].srcImgDetection.xmax; + yMax = trackData->tracklets[i].srcImgDetection.ymax; + } else { + xMin = trackData->tracklets[i].srcImgDetection.xmin * _width; + yMin = trackData->tracklets[i].srcImgDetection.ymin * _height; + xMax = trackData->tracklets[i].srcImgDetection.xmax * _width; + yMax = trackData->tracklets[i].srcImgDetection.ymax * _height; + } + + float xSize = xMax - xMin; + float ySize = yMax - yMin; + float xCenter = xMin + xSize / 2; + float yCenter = yMin + ySize / 2; + + trackletsMsg.tracklets[i].roi.center.x = xCenter; + trackletsMsg.tracklets[i].roi.center.y = yCenter; + trackletsMsg.tracklets[i].roi.size_x = xSize; + trackletsMsg.tracklets[i].roi.size_y = ySize; + + trackletsMsg.tracklets[i].id = trackData->tracklets[i].id; + trackletsMsg.tracklets[i].label = trackData->tracklets[i].label; + trackletsMsg.tracklets[i].age = trackData->tracklets[i].age; + trackletsMsg.tracklets[i].status = static_cast::type>(trackData->tracklets[i].status); + + trackletsMsg.tracklets[i].srcImgDetection.confidence = trackData->tracklets[i].srcImgDetection.confidence; + trackletsMsg.tracklets[i].srcImgDetection.label = trackData->tracklets[i].srcImgDetection.label; + trackletsMsg.tracklets[i].srcImgDetection.xmin = xMin; + trackletsMsg.tracklets[i].srcImgDetection.xmax = xMax; + trackletsMsg.tracklets[i].srcImgDetection.ymin = yMin; + trackletsMsg.tracklets[i].srcImgDetection.ymax = yMax; + + // converting mm to meters since per ros rep-103 lenght should always be in meters + trackletsMsg.tracklets[i].spatialCoordinates.x = trackData->tracklets[i].spatialCoordinates.x / 1000; + trackletsMsg.tracklets[i].spatialCoordinates.y = trackData->tracklets[i].spatialCoordinates.y / 1000; + trackletsMsg.tracklets[i].spatialCoordinates.z = trackData->tracklets[i].spatialCoordinates.z / 1000; + } + + trackletMsgs.push_back(trackletsMsg); +} + +TrackletArrayPtr TrackletConverter::toRosMsgPtr(std::shared_ptr trackData) { + std::deque msgQueue; + toRosMsg(trackData, msgQueue); + auto msg = msgQueue.front(); + TrackletArrayPtr ptr = boost::make_shared(msg); + return ptr; +} + +} // namespace ros +} // namespace dai \ No newline at end of file From 4809bca657753ba72e5e5ca32d1c0a9db3ca94fa Mon Sep 17 00:00:00 2001 From: Nino Wegleitner Date: Sat, 13 May 2023 22:07:06 +0000 Subject: [PATCH 2/9] Add TrackletConverter --- depthai_bridge/include/depthai_bridge/TrackletConverter.hpp | 1 - depthai_bridge/src/TrackletConverter.cpp | 2 -- 2 files changed, 3 deletions(-) diff --git a/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp b/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp index f1663dae..d7528e73 100644 --- a/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp @@ -7,7 +7,6 @@ #include "depthai/pipeline/datatype/Tracklets.hpp" #include "depthai_ros_msgs/TrackletArray.h" #include "ros/ros.h" -#include "vision_msgs/Detection3DArray.h" namespace dai { diff --git a/depthai_bridge/src/TrackletConverter.cpp b/depthai_bridge/src/TrackletConverter.cpp index 6c91bfba..e6ce5245 100644 --- a/depthai_bridge/src/TrackletConverter.cpp +++ b/depthai_bridge/src/TrackletConverter.cpp @@ -1,7 +1,5 @@ #include "depthai_bridge/TrackletConverter.hpp" #include "depthai_bridge/depthaiUtility.hpp" -#include -#include namespace dai { namespace ros { From 798d42cc95530f82e8fe0da0a4b5107ef2e84b96 Mon Sep 17 00:00:00 2001 From: Nino Wegleitner Date: Sat, 13 May 2023 22:10:41 +0000 Subject: [PATCH 3/9] Add msg for Tracklets --- depthai_ros_msgs/CMakeLists.txt | 3 +++ depthai_ros_msgs/msg/ImgDetection.msg | 11 +++++++++++ depthai_ros_msgs/msg/Tracklet.msg | 26 ++++++++++++++++++++++++++ depthai_ros_msgs/msg/TrackletArray.msg | 5 +++++ 4 files changed, 45 insertions(+) create mode 100644 depthai_ros_msgs/msg/ImgDetection.msg create mode 100644 depthai_ros_msgs/msg/Tracklet.msg create mode 100644 depthai_ros_msgs/msg/TrackletArray.msg diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index d0338bdf..61a37113 100644 --- a/depthai_ros_msgs/CMakeLists.txt +++ b/depthai_ros_msgs/CMakeLists.txt @@ -31,6 +31,9 @@ add_message_files ( SpatialDetection.msg SpatialDetectionArray.msg ImuWithMagneticField.msg + ImgDetection.msg + Tracklet.msg + TrackletArray.msg ) ## Generate services in the 'srv' folder diff --git a/depthai_ros_msgs/msg/ImgDetection.msg b/depthai_ros_msgs/msg/ImgDetection.msg new file mode 100644 index 00000000..a63c57a8 --- /dev/null +++ b/depthai_ros_msgs/msg/ImgDetection.msg @@ -0,0 +1,11 @@ +# label +uint32 label + +# confidence +float32 confidence + +# bounding box +float32 xmin +float32 ymin +float32 xmax +float32 ymax diff --git a/depthai_ros_msgs/msg/Tracklet.msg b/depthai_ros_msgs/msg/Tracklet.msg new file mode 100644 index 00000000..957cfb13 --- /dev/null +++ b/depthai_ros_msgs/msg/Tracklet.msg @@ -0,0 +1,26 @@ +# TrackingStatus +uint8 NEW=0 +uint8 TRACKED=1 +uint8 LOST=2 +uint8 REMOVED=3 + +# Rect roi +vision_msgs/BoundingBox2D roi + +# Tracklet's ID. +int32 id + +# Tracklet's label ID. +int32 label + +# Number of frames it is being tracked for. +int32 age + +# Status of tracklet. +int32 status + +# Image detection that is tracked. +ImgDetection srcImgDetection + +# Spatial coordinates of tracklet. +geometry_msgs/Point spatialCoordinates diff --git a/depthai_ros_msgs/msg/TrackletArray.msg b/depthai_ros_msgs/msg/TrackletArray.msg new file mode 100644 index 00000000..38749ba1 --- /dev/null +++ b/depthai_ros_msgs/msg/TrackletArray.msg @@ -0,0 +1,5 @@ +# header +std_msgs/Header header + +# tracklets +Tracklet[] tracklets \ No newline at end of file From bf548e0ac747ffe6413e236c936b70beb7b0b546 Mon Sep 17 00:00:00 2001 From: Nino Wegleitner Date: Sun, 14 May 2023 16:55:16 +0000 Subject: [PATCH 4/9] remove toRosVisionMsg --- depthai_bridge/include/depthai_bridge/TrackletConverter.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp b/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp index d7528e73..13cffeae 100644 --- a/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp @@ -17,7 +17,6 @@ class TrackletConverter { public: TrackletConverter(std::string frameName, int width, int height, bool normalized = false, bool getBaseDeviceTimestamp = false); void toRosMsg(std::shared_ptr trackData, std::deque& trackletsMsg); - void toRosVisionMsg(std::shared_ptr trackData, std::deque& trackletsMsg); TrackletArrayPtr toRosMsgPtr(std::shared_ptr trackData); private: From 3341e690ebb61f91f6479d1fccd447cf9e9e9fb8 Mon Sep 17 00:00:00 2001 From: Nino Wegleitner Date: Sun, 14 May 2023 16:55:45 +0000 Subject: [PATCH 5/9] add example tracker publisher --- depthai_examples/CMakeLists.txt | 3 + .../launch/tracker_publisher.launch | 79 ++++ depthai_examples/src/tracker_publisher.cpp | 399 ++++++++++++++++++ 3 files changed, 481 insertions(+) create mode 100644 depthai_examples/launch/tracker_publisher.launch create mode 100644 depthai_examples/src/tracker_publisher.cpp diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt index 214fb9cf..2ba04c0f 100644 --- a/depthai_examples/CMakeLists.txt +++ b/depthai_examples/CMakeLists.txt @@ -111,9 +111,11 @@ dai_add_node(stereo_inertial_node src/stereo_inertial_publisher.cpp) dai_add_node(stereo_node src/stereo_publisher.cpp) dai_add_node(yolov4_spatial_node src/yolov4_spatial_publisher.cpp) +dai_add_node(tracker_node src/tracker_publisher.cpp) target_compile_definitions(mobilenet_node PRIVATE BLOB_NAME="${mobilenet_blob_name}") target_compile_definitions(yolov4_spatial_node PRIVATE BLOB_NAME="${tiny_yolo_v4_blob_name}") +target_compile_definitions(tracker_node PRIVATE BLOB_NAME="${tiny_yolo_v4_blob_name}") target_compile_definitions(stereo_inertial_node PRIVATE BLOB_NAME="${tiny_yolo_v4_blob_name}") catkin_install_python(PROGRAMS @@ -131,6 +133,7 @@ install(TARGETS stereo_inertial_node stereo_node yolov4_spatial_node + tracker_node LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/depthai_examples/launch/tracker_publisher.launch b/depthai_examples/launch/tracker_publisher.launch new file mode 100644 index 00000000..0a578c9d --- /dev/null +++ b/depthai_examples/launch/tracker_publisher.launch @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/depthai_examples/src/tracker_publisher.cpp b/depthai_examples/src/tracker_publisher.cpp new file mode 100644 index 00000000..0441c647 --- /dev/null +++ b/depthai_examples/src/tracker_publisher.cpp @@ -0,0 +1,399 @@ +#include +#include +#include +#include + +#include "camera_info_manager/camera_info_manager.h" +#include "depthai_bridge/BridgePublisher.hpp" +#include "depthai_bridge/ImageConverter.hpp" +#include "depthai_bridge/SpatialDetectionConverter.hpp" +#include "depthai_bridge/TrackletConverter.hpp" +#include "depthai_bridge/DisparityConverter.hpp" +#include "depthai_bridge/ImuConverter.hpp" +#include "depthai_bridge/depthaiUtility.hpp" +#include "ros/ros.h" + +// Inludes common necessary includes for development using depthai library +#include "depthai/device/DataQueue.hpp" +#include "depthai/device/Device.hpp" +#include "depthai/pipeline/Pipeline.hpp" +#include "depthai/pipeline/node/ColorCamera.hpp" +#include "depthai/pipeline/node/MonoCamera.hpp" +#include "depthai/pipeline/node/SpatialDetectionNetwork.hpp" +#include "depthai/pipeline/node/StereoDepth.hpp" +#include "depthai/pipeline/node/XLinkOut.hpp" +#include "depthai/pipeline/node/IMU.hpp" +#include "depthai/pipeline/node/XLinkIn.hpp" +#include "depthai/pipeline/node/ObjectTracker.hpp" + +#include "depthai_ros_msgs/SpatialDetectionArray.h" +#include "sensor_msgs/Imu.h" +#include "stereo_msgs/DisparityImage.h" +#include "sensor_msgs/Image.h" +#include "vision_msgs/Detection2DArray.h" + + +const std::vector label_map = { + "person", "bicycle", "car", "motorbike", "aeroplane", "bus", "train", "truck", "boat", + "traffic light", "fire hydrant", "stop sign", "parking meter", "bench", "bird", "cat", "dog", "horse", + "sheep", "cow", "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag", + "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite", "baseball bat", "baseball glove", + "skateboard", "surfboard", "tennis racket", "bottle", "wine glass", "cup", "fork", "knife", "spoon", + "bowl", "banana", "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza", + "donut", "cake", "chair", "sofa", "pottedplant", "bed", "diningtable", "toilet", "tvmonitor", + "laptop", "mouse", "remote", "keyboard", "cell phone", "microwave", "oven", "toaster", "sink", + "refrigerator", "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"}; + +std::tuple createPipeline(bool syncNN, + bool subpixel, + bool lrcheck, + bool extended, + std::string nnPath, + int confidence, + int LRchecktresh, + int mono_fps, + int rgb_fps, + int preview_size, + int rgbScaleNumerator, + int rgbScaleDinominator, + std::string monoResolutionStr, + std::string rgbResolutionStr) { + dai::Pipeline pipeline; + pipeline.setXLinkChunkSize(0); + + auto controlIn = pipeline.create(); + auto monoLeft = pipeline.create(); + auto monoRight = pipeline.create(); + auto colorCam = pipeline.create(); + auto stereo = pipeline.create(); + auto spatialDetectionNetwork = pipeline.create(); + auto objectTracker = pipeline.create(); + auto imu = pipeline.create(); + + // create xlink connections + auto xoutRgb = pipeline.create(); + auto xoutDepth = pipeline.create(); + auto xoutNN = pipeline.create(); + auto xoutPreview = pipeline.create(); + auto xoutTracker = pipeline.create(); + auto xoutImu = pipeline.create(); + + controlIn->setStreamName("control"); + xoutPreview->setStreamName("preview"); + xoutRgb->setStreamName("rgb"); + xoutDepth->setStreamName("depth"); + xoutNN->setStreamName("detections"); + xoutTracker->setStreamName("tracklets"); + xoutImu->setStreamName("imu"); + + // MonoCameras + dai::node::MonoCamera::Properties::SensorResolution monoResolution; + int monoWidth, monoHeight; + if(monoResolutionStr == "720p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_720_P; + monoWidth = 1280; + monoHeight = 720; + } else if(monoResolutionStr == "400p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_400_P; + monoWidth = 640; + monoHeight = 400; + } else if(monoResolutionStr == "800p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_800_P; + monoWidth = 1280; + monoHeight = 800; + } else if(monoResolutionStr == "480p") { + monoResolution = dai::node::MonoCamera::Properties::SensorResolution::THE_480_P; + monoWidth = 640; + monoHeight = 480; + } else { + ROS_ERROR("Invalid parameter. -> monoResolution: %s", monoResolutionStr.c_str()); + throw std::runtime_error("Invalid mono camera resolution."); + } + monoLeft->setResolution(monoResolution); + monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT); + monoLeft->setFps(mono_fps); + monoRight->setResolution(monoResolution); + monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT); + monoRight->setFps(mono_fps); + + // StereoDepth + stereo->initialConfig.setConfidenceThreshold(confidence); + stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout + stereo->initialConfig.setLeftRightCheckThreshold(LRchecktresh); + stereo->setSubpixel(subpixel); + stereo->setLeftRightCheck(lrcheck); + stereo->setExtendedDisparity(extended); + stereo->setDepthAlign(dai::CameraBoardSocket::RGB); + + // ObjectTracker + objectTracker->setTrackerType(dai::TrackerType::ZERO_TERM_COLOR_HISTOGRAM); + objectTracker->setTrackerIdAssignmentPolicy(dai::TrackerIdAssignmentPolicy::UNIQUE_ID); + + // IMU + imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 500); + imu->enableIMUSensor(dai::IMUSensor::GYROSCOPE_RAW, 400); + imu->setBatchReportThreshold(5); + imu->setMaxBatchReports(20); // Get one message only for now. + + // ColorCamera + int rgbWidth, rgbHeight; + dai::node::ColorCamera::Properties::SensorResolution rgbResolution; + if(rgbResolutionStr == "1080p") { + rgbResolution = dai::node::ColorCamera::Properties::SensorResolution::THE_1080_P; + rgbWidth = 1920; + rgbHeight = 1080; + } else if(rgbResolutionStr == "4K") { + rgbResolution = dai::node::ColorCamera::Properties::SensorResolution::THE_4_K; + rgbWidth = 3840; + rgbHeight = 2160; + } else if(rgbResolutionStr == "12MP") { + rgbResolution = dai::node::ColorCamera::Properties::SensorResolution::THE_12_MP; + rgbWidth = 4056; + rgbHeight = 3040; + } else if(rgbResolutionStr == "13MP") { + rgbResolution = dai::node::ColorCamera::Properties::SensorResolution::THE_13_MP; + rgbWidth = 4208; + rgbHeight = 3120; + } else { + ROS_ERROR("Invalid parameter. -> rgbResolution: %s", rgbResolutionStr.c_str()); + throw std::runtime_error("Invalid color camera resolution."); + } + colorCam->setBoardSocket(dai::CameraBoardSocket::RGB); + colorCam->setPreviewSize(preview_size, preview_size); + colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); + colorCam->setInterleaved(false); + colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); + colorCam->setFps(rgb_fps); + colorCam->setIspScale(rgbScaleNumerator, rgbScaleDinominator); + rgbWidth = rgbWidth * rgbScaleNumerator / rgbScaleDinominator; + rgbHeight = rgbHeight * rgbScaleNumerator / rgbScaleDinominator; + + // SpatialDetections + spatialDetectionNetwork->setBlobPath(nnPath); + spatialDetectionNetwork->setConfidenceThreshold(0.5f); + spatialDetectionNetwork->input.setBlocking(false); + spatialDetectionNetwork->setBoundingBoxScaleFactor(0.5); + spatialDetectionNetwork->setDepthLowerThreshold(100); + spatialDetectionNetwork->setDepthUpperThreshold(5000); + + // yolo specific parameters + spatialDetectionNetwork->setNumClasses(80); + spatialDetectionNetwork->setCoordinateSize(4); + spatialDetectionNetwork->setAnchors({10, 14, 23, 27, 37, 58, 81, 82, 135, 169, 344, 319}); + spatialDetectionNetwork->setAnchorMasks({{"side13", {3, 4, 5}}, {"side26", {1, 2, 3}}}); + spatialDetectionNetwork->setIouThreshold(0.5f); + + // Link Control + controlIn->out.link(monoRight->inputControl); + controlIn->out.link(monoLeft->inputControl); + + // Link MonoCameras + monoLeft->out.link(stereo->left); + monoRight->out.link(stereo->right); + + // Link StereoCamera + stereo->depth.link(spatialDetectionNetwork->inputDepth); + + // Link ColorCamera + colorCam->preview.link(spatialDetectionNetwork->input); + colorCam->isp.link(xoutRgb->input); + + // Link SpatialDetectionNetwork + spatialDetectionNetwork->passthroughDepth.link(xoutDepth->input); + spatialDetectionNetwork->passthrough.link(objectTracker->inputDetectionFrame); + spatialDetectionNetwork->passthrough.link(objectTracker->inputTrackerFrame); + spatialDetectionNetwork->out.link(objectTracker->inputDetections); + + // Link ObjectTracker + // objectTracker->setDetectionLabelsToTrack({47}); + objectTracker->passthroughDetectionFrame.link(xoutPreview->input); + objectTracker->passthroughDetections.link(xoutNN->input); + objectTracker->out.link(xoutTracker->input); + + // Link IMU + imu->out.link(xoutImu->input); + + return std::make_tuple(pipeline, rgbWidth, rgbHeight, monoWidth, monoHeight); +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "tracker_node"); + ros::NodeHandle pnh("~"); + + std::string tfPrefix, resourceBaseFolder, nnPath; + std::string camera_param_uri; + std::string nnName(BLOB_NAME); // Set your blob name for the model here + bool lrcheck, extended, syncNN, subpixel, enableDotProjector, enableFloodLight; + int rgbScaleNumerator, rgbScaleDinominator; + double dotProjectormA, floodLightmA; + double angularVelCovariance, linearAccelCovariance; + int imuModeParam = 1; + int badParams = 0; + int confidence = 200; + int LRchecktresh = 5; + int monoFPS = 30; + int rgbFPS = 30; + int previewSize = 416; + std::string monoResolution = "720p", rgbResolution = "1080p"; + + badParams += !pnh.getParam("tf_prefix", tfPrefix); + badParams += !pnh.getParam("camera_param_uri", camera_param_uri); + badParams += !pnh.getParam("sync_nn", syncNN); + badParams += !pnh.getParam("subpixel", subpixel); + badParams += !pnh.getParam("confidence", confidence); + badParams += !pnh.getParam("LRchecktresh", LRchecktresh); + badParams += !pnh.getParam("monoResolution", monoResolution); + badParams += !pnh.getParam("resourceBaseFolder", resourceBaseFolder); + badParams += !pnh.getParam("monoFPS", monoFPS); + badParams += !pnh.getParam("rgbFPS", rgbFPS); + badParams += !pnh.getParam("previewSize", previewSize); + badParams += !pnh.getParam("lrcheck", lrcheck); + badParams += !pnh.getParam("extended", extended); + badParams += !pnh.getParam("rgbScaleNumerator", rgbScaleNumerator); + badParams += !pnh.getParam("rgbScaleDinominator", rgbScaleDinominator); + badParams += !pnh.getParam("monoResolution", monoResolution); + badParams += !pnh.getParam("rgbResolution", rgbResolution); + + badParams += !pnh.getParam("angularVelCovariance", angularVelCovariance); + badParams += !pnh.getParam("linearAccelCovariance", linearAccelCovariance); + + badParams += !pnh.getParam("enableDotProjector", enableDotProjector); + badParams += !pnh.getParam("enableFloodLight", enableFloodLight); + badParams += !pnh.getParam("dotProjectormA", dotProjectormA); + badParams += !pnh.getParam("floodLightmA", floodLightmA); + + if(badParams > 0) { + throw std::runtime_error("Couldn't find one of the parameters"); + } + + std::string nnParam; + pnh.getParam("nnName", nnParam); + if(nnParam != "x") { + pnh.getParam("nnName", nnName); + } + + if(resourceBaseFolder.empty()) { + throw std::runtime_error("Send the path to the resouce folder containing NNBlob in \'resourceBaseFolder\' "); + } + nnPath = resourceBaseFolder + "/" + nnName; + + dai::Pipeline pipeline; + int rgbWidth, rgbHeight, monoWidth, monoHeight; + std::tie(pipeline, rgbWidth, rgbHeight, monoWidth, monoHeight) = createPipeline(syncNN, + subpixel, + lrcheck, + extended, + nnPath, + confidence, + LRchecktresh, + monoFPS, + rgbFPS, + previewSize, + rgbScaleNumerator, + rgbScaleDinominator, + monoResolution, + rgbResolution); + + // Device + std::shared_ptr device; + device = std::make_shared(pipeline); + + // Get Input/Output Queues + auto previewQueue = device->getOutputQueue("preview", 30, false); + auto colorQueue = device->getOutputQueue("rgb", 30, false); + auto detectionQueue = device->getOutputQueue("detections", 30, false); + auto depthQueue = device->getOutputQueue("depth", 30, false); + auto imuQueue = device->getOutputQueue("imu", 30, false); + auto controlQueue = device->getInputQueue("control"); + auto trackletQueue = device->getOutputQueue("tracklets", 30, false); + + auto calibrationHandler = device->readCalibration(); + + std::vector> irDrivers = device->getIrDrivers(); + if(!irDrivers.empty()) { + if(enableDotProjector) device->setIrLaserDotProjectorBrightness(dotProjectormA); // in mA, 0..1200 + if(enableFloodLight) device->setIrFloodLightBrightness(floodLightmA); // in mA, 0..1500 + } + + auto boardName = calibrationHandler.getEepromData().boardName; + if(monoHeight > 480 && boardName == "OAK-D-LITE") { + ROS_ERROR("Invalid parameter. -> monoResolution: %s", monoResolution.c_str()); + throw std::runtime_error("Invalid mono camera resolution for OAK-D-LITE."); + } + + // IMU + dai::ros::ImuSyncMethod imuMode = static_cast(imuModeParam); + dai::rosBridge::ImuConverter imuConverter(tfPrefix + "_imu_frame", imuMode, linearAccelCovariance, angularVelCovariance); + dai::rosBridge::BridgePublisher imuPublish( + imuQueue, + pnh, + std::string("imu"), + std::bind(&dai::rosBridge::ImuConverter::toRosMsg, &imuConverter, std::placeholders::_1, std::placeholders::_2), + 30, + "", + "imu"); + imuPublish.addPublisherCallback(); + + // RGB + dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false); + auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, rgbWidth, rgbHeight); + dai::rosBridge::BridgePublisher rgbPublish( + colorQueue, + pnh, + std::string("color/image"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &rgbConverter, std::placeholders::_1, std::placeholders::_2), + 30, + rgbCameraInfo, + "color"); + rgbPublish.addPublisherCallback(); + + // Depth + dai::rosBridge::ImageConverter depthConverter(tfPrefix + "_rgb_camera_optical_frame", false); + auto depthCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight); + dai::rosBridge::BridgePublisher depthPublish( + depthQueue, + pnh, + std::string("stereo/depth"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &depthConverter, std::placeholders::_1, std::placeholders::_2), + 30, + depthCameraInfo, + "stereo"); + depthPublish.addPublisherCallback(); + + // Preview + dai::rosBridge::ImageConverter previewConverter(tfPrefix + "_rgb_camera_optical_frame", false); + auto previewCameraInfo = previewConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, previewSize, previewSize); + dai::rosBridge::BridgePublisher previewPublish( + previewQueue, + pnh, + std::string("color/preview/image"), + std::bind(&dai::rosBridge::ImageConverter::toRosMsg, &previewConverter, std::placeholders::_1, std::placeholders::_2), + 30, + previewCameraInfo, + "color/preview"); + previewPublish.addPublisherCallback(); + + // SpatialDetections + dai::rosBridge::SpatialDetectionConverter detConverter(tfPrefix + "_rgb_camera_optical_frame", previewSize, previewSize, false); + dai::rosBridge::BridgePublisher detectionPublish( + detectionQueue, + pnh, + std::string("detections/yolov4_spatial"), + std::bind(&dai::rosBridge::SpatialDetectionConverter::toRosMsg, &detConverter, std::placeholders::_1, std::placeholders::_2), + 30); + detectionPublish.addPublisherCallback(); + + // Tracklets + dai::rosBridge::TrackletConverter trackConverter(tfPrefix + "_rgb_camera_optical_frame", previewSize, previewSize, false); + dai::rosBridge::BridgePublisher trackletPublish( + trackletQueue, + pnh, + std::string("tracker/tracklets"), + std::bind(&dai::rosBridge::TrackletConverter::toRosMsg, &trackConverter, std::placeholders::_1, std::placeholders::_2), + 30); + trackletPublish.addPublisherCallback(); + + ros::spin(); + + return 0; +} From 6e91b44af74b80dd3a8aadbd2a8678371a23149a Mon Sep 17 00:00:00 2001 From: Nino Wegleitner Date: Mon, 15 May 2023 10:00:09 +0000 Subject: [PATCH 6/9] replace ImgDetections with vision_msgs --- depthai_ros_msgs/CMakeLists.txt | 1 - depthai_ros_msgs/msg/ImgDetection.msg | 11 ----------- depthai_ros_msgs/msg/Tracklet.msg | 5 +++-- 3 files changed, 3 insertions(+), 14 deletions(-) delete mode 100644 depthai_ros_msgs/msg/ImgDetection.msg diff --git a/depthai_ros_msgs/CMakeLists.txt b/depthai_ros_msgs/CMakeLists.txt index 61a37113..afe29a4f 100644 --- a/depthai_ros_msgs/CMakeLists.txt +++ b/depthai_ros_msgs/CMakeLists.txt @@ -31,7 +31,6 @@ add_message_files ( SpatialDetection.msg SpatialDetectionArray.msg ImuWithMagneticField.msg - ImgDetection.msg Tracklet.msg TrackletArray.msg ) diff --git a/depthai_ros_msgs/msg/ImgDetection.msg b/depthai_ros_msgs/msg/ImgDetection.msg deleted file mode 100644 index a63c57a8..00000000 --- a/depthai_ros_msgs/msg/ImgDetection.msg +++ /dev/null @@ -1,11 +0,0 @@ -# label -uint32 label - -# confidence -float32 confidence - -# bounding box -float32 xmin -float32 ymin -float32 xmax -float32 ymax diff --git a/depthai_ros_msgs/msg/Tracklet.msg b/depthai_ros_msgs/msg/Tracklet.msg index 957cfb13..1b1ec0b1 100644 --- a/depthai_ros_msgs/msg/Tracklet.msg +++ b/depthai_ros_msgs/msg/Tracklet.msg @@ -19,8 +19,9 @@ int32 age # Status of tracklet. int32 status -# Image detection that is tracked. -ImgDetection srcImgDetection +# srcImgDetection detection that is tracked. +vision_msgs/ObjectHypothesis srcImgDetectionHypothesis +vision_msgs/BoundingBox2D srcImgDetectionBBox # Spatial coordinates of tracklet. geometry_msgs/Point spatialCoordinates From 84c176edc0a95b123047c0068644e5524be010f6 Mon Sep 17 00:00:00 2001 From: Nino Wegleitner Date: Mon, 15 May 2023 10:00:43 +0000 Subject: [PATCH 7/9] update denormalization --- .../depthai_bridge/TrackletConverter.hpp | 13 +++- depthai_bridge/src/TrackletConverter.cpp | 78 ++++++++++++------- 2 files changed, 60 insertions(+), 31 deletions(-) diff --git a/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp b/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp index 13cffeae..6e13e7df 100644 --- a/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp +++ b/depthai_bridge/include/depthai_bridge/TrackletConverter.hpp @@ -15,14 +15,21 @@ namespace TrackletMessages = depthai_ros_msgs; using TrackletArrayPtr = TrackletMessages::TrackletArray::Ptr; class TrackletConverter { public: - TrackletConverter(std::string frameName, int width, int height, bool normalized = false, bool getBaseDeviceTimestamp = false); + TrackletConverter(std::string frameName, + int width_srcImgDetection, + int height_srcImgDetection, + int width_srcImgTracker, + int height_srcImgTracker, + bool normalized_srcImgDetection = false, + bool normalized_srcImgTracker = false, + bool getBaseDeviceTimestamp = false); void toRosMsg(std::shared_ptr trackData, std::deque& trackletsMsg); TrackletArrayPtr toRosMsgPtr(std::shared_ptr trackData); private: - int _width, _height; + int _width_srcImgDetection, _height_srcImgDetection, _width_srcImgTracker, _height_srcImgTracker; const std::string _frameName; - bool _normalized; + bool _normalized_srcImgDetection, _normalized_srcImgTracker; std::chrono::time_point _steadyBaseTime; ::ros::Time _rosBaseTime; bool _getBaseDeviceTimestamp; diff --git a/depthai_bridge/src/TrackletConverter.cpp b/depthai_bridge/src/TrackletConverter.cpp index e6ce5245..8af518f3 100644 --- a/depthai_bridge/src/TrackletConverter.cpp +++ b/depthai_bridge/src/TrackletConverter.cpp @@ -4,11 +4,21 @@ namespace dai { namespace ros { -TrackletConverter::TrackletConverter(std::string frameName, int width, int height, bool normalized, bool getBaseDeviceTimestamp) +TrackletConverter::TrackletConverter(std::string frameName, + int width_srcImgDetection, + int height_srcImgDetection, + int width_srcImgTracker, + int height_srcImgTracker, + bool normalized_srcImgDetection, + bool normalized_srcImgTracker, + bool getBaseDeviceTimestamp) : _frameName(frameName), - _width(width), - _height(height), - _normalized(normalized), + _width_srcImgDetection(width_srcImgDetection), + _height_srcImgDetection(height_srcImgDetection), + _width_srcImgTracker(width_srcImgTracker), + _height_srcImgTracker(height_srcImgTracker), + _normalized_srcImgDetection(normalized_srcImgDetection), + _normalized_srcImgTracker(normalized_srcImgTracker), _steadyBaseTime(std::chrono::steady_clock::now()), _getBaseDeviceTimestamp(getBaseDeviceTimestamp) { _rosBaseTime = ::ros::Time::now(); @@ -30,40 +40,52 @@ void TrackletConverter::toRosMsg(std::shared_ptr trackData, std: // publishing for(int i = 0; i < trackData->tracklets.size(); ++i) { - int xMin, yMin, xMax, yMax; - if(_normalized) { - xMin = trackData->tracklets[i].srcImgDetection.xmin; - yMin = trackData->tracklets[i].srcImgDetection.ymin; - xMax = trackData->tracklets[i].srcImgDetection.xmax; - yMax = trackData->tracklets[i].srcImgDetection.ymax; + // srcImgDetection -> inputDetectionsFrame + int xMin_srcImgDetection, yMin_srcImgDetection, xMax_srcImgDetection, yMax_srcImgDetection; + if(_normalized_srcImgDetection) { + xMin_srcImgDetection = trackData->tracklets[i].srcImgDetection.xmin; + yMin_srcImgDetection = trackData->tracklets[i].srcImgDetection.ymin; + xMax_srcImgDetection = trackData->tracklets[i].srcImgDetection.xmax; + yMax_srcImgDetection = trackData->tracklets[i].srcImgDetection.ymax; } else { - xMin = trackData->tracklets[i].srcImgDetection.xmin * _width; - yMin = trackData->tracklets[i].srcImgDetection.ymin * _height; - xMax = trackData->tracklets[i].srcImgDetection.xmax * _width; - yMax = trackData->tracklets[i].srcImgDetection.ymax * _height; + xMin_srcImgDetection = trackData->tracklets[i].srcImgDetection.xmin * _width_srcImgDetection; + yMin_srcImgDetection = trackData->tracklets[i].srcImgDetection.ymin * _height_srcImgDetection; + xMax_srcImgDetection = trackData->tracklets[i].srcImgDetection.xmax * _width_srcImgDetection; + yMax_srcImgDetection = trackData->tracklets[i].srcImgDetection.ymax * _height_srcImgDetection; } + auto xSize_srcImgDetection = xMax_srcImgDetection - xMin_srcImgDetection; + auto ySize_srcImgDetection = yMax_srcImgDetection - yMin_srcImgDetection; + auto xCenter_srcImgDetection = xMin_srcImgDetection + xSize_srcImgDetection / 2; + auto yCenter_srcImgDetection = yMin_srcImgDetection + ySize_srcImgDetection / 2; - float xSize = xMax - xMin; - float ySize = yMax - yMin; - float xCenter = xMin + xSize / 2; - float yCenter = yMin + ySize / 2; + // roi -> inputTrackerFrame + dai::Rect roi; + if(_normalized_srcImgTracker) { + roi = trackData->tracklets[i].roi; + } else { + roi = trackData->tracklets[i].roi.denormalize(_width_srcImgTracker, _height_srcImgTracker); + } + auto xSize_srcImgTracker = roi.size().width; + auto ySize_srcImgTracker = roi.size().height; + auto xCenter_srcImgTracker = roi.topLeft().x + roi.width / 2; + auto yCenter_srcImgTracker = roi.topLeft().y + roi.height / 2; - trackletsMsg.tracklets[i].roi.center.x = xCenter; - trackletsMsg.tracklets[i].roi.center.y = yCenter; - trackletsMsg.tracklets[i].roi.size_x = xSize; - trackletsMsg.tracklets[i].roi.size_y = ySize; + trackletsMsg.tracklets[i].roi.center.x = xCenter_srcImgTracker; + trackletsMsg.tracklets[i].roi.center.y = yCenter_srcImgTracker; + trackletsMsg.tracklets[i].roi.size_x = xSize_srcImgTracker; + trackletsMsg.tracklets[i].roi.size_y = ySize_srcImgTracker; trackletsMsg.tracklets[i].id = trackData->tracklets[i].id; trackletsMsg.tracklets[i].label = trackData->tracklets[i].label; trackletsMsg.tracklets[i].age = trackData->tracklets[i].age; trackletsMsg.tracklets[i].status = static_cast::type>(trackData->tracklets[i].status); - trackletsMsg.tracklets[i].srcImgDetection.confidence = trackData->tracklets[i].srcImgDetection.confidence; - trackletsMsg.tracklets[i].srcImgDetection.label = trackData->tracklets[i].srcImgDetection.label; - trackletsMsg.tracklets[i].srcImgDetection.xmin = xMin; - trackletsMsg.tracklets[i].srcImgDetection.xmax = xMax; - trackletsMsg.tracklets[i].srcImgDetection.ymin = yMin; - trackletsMsg.tracklets[i].srcImgDetection.ymax = yMax; + trackletsMsg.tracklets[i].srcImgDetectionHypothesis.score = trackData->tracklets[i].srcImgDetection.confidence; + trackletsMsg.tracklets[i].srcImgDetectionHypothesis.id = trackData->tracklets[i].srcImgDetection.label; + trackletsMsg.tracklets[i].srcImgDetectionBBox.center.x = xCenter_srcImgDetection; + trackletsMsg.tracklets[i].srcImgDetectionBBox.center.y = yCenter_srcImgDetection; + trackletsMsg.tracklets[i].srcImgDetectionBBox.size_x = xSize_srcImgDetection; + trackletsMsg.tracklets[i].srcImgDetectionBBox.size_y = ySize_srcImgDetection; // converting mm to meters since per ros rep-103 lenght should always be in meters trackletsMsg.tracklets[i].spatialCoordinates.x = trackData->tracklets[i].spatialCoordinates.x / 1000; From d18125062111be8c8ae6249955a2d56546b803c0 Mon Sep 17 00:00:00 2001 From: Nino Wegleitner Date: Mon, 15 May 2023 10:01:20 +0000 Subject: [PATCH 8/9] update denormalization --- .../launch/tracker_publisher.launch | 6 +++-- depthai_examples/src/tracker_publisher.cpp | 23 +++++++++++-------- 2 files changed, 18 insertions(+), 11 deletions(-) diff --git a/depthai_examples/launch/tracker_publisher.launch b/depthai_examples/launch/tracker_publisher.launch index 0a578c9d..aff73741 100644 --- a/depthai_examples/launch/tracker_publisher.launch +++ b/depthai_examples/launch/tracker_publisher.launch @@ -26,7 +26,8 @@ - + + @@ -61,7 +62,8 @@ - + + diff --git a/depthai_examples/src/tracker_publisher.cpp b/depthai_examples/src/tracker_publisher.cpp index 0441c647..92ac61cb 100644 --- a/depthai_examples/src/tracker_publisher.cpp +++ b/depthai_examples/src/tracker_publisher.cpp @@ -53,7 +53,8 @@ std::tuple createPipeline(bool syncNN, int LRchecktresh, int mono_fps, int rgb_fps, - int preview_size, + int previewWidth, + int previewHeight, int rgbScaleNumerator, int rgbScaleDinominator, std::string monoResolutionStr, @@ -159,7 +160,7 @@ std::tuple createPipeline(bool syncNN, throw std::runtime_error("Invalid color camera resolution."); } colorCam->setBoardSocket(dai::CameraBoardSocket::RGB); - colorCam->setPreviewSize(preview_size, preview_size); + colorCam->setPreviewSize(previewWidth, previewHeight); colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P); colorCam->setInterleaved(false); colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::BGR); @@ -196,7 +197,7 @@ std::tuple createPipeline(bool syncNN, // Link ColorCamera colorCam->preview.link(spatialDetectionNetwork->input); - colorCam->isp.link(xoutRgb->input); + colorCam->isp.link(objectTracker->inputTrackerFrame); // Link SpatialDetectionNetwork spatialDetectionNetwork->passthroughDepth.link(xoutDepth->input); @@ -208,6 +209,7 @@ std::tuple createPipeline(bool syncNN, // objectTracker->setDetectionLabelsToTrack({47}); objectTracker->passthroughDetectionFrame.link(xoutPreview->input); objectTracker->passthroughDetections.link(xoutNN->input); + objectTracker->passthroughTrackerFrame.link(xoutRgb->input); objectTracker->out.link(xoutTracker->input); // Link IMU @@ -233,7 +235,8 @@ int main(int argc, char** argv) { int LRchecktresh = 5; int monoFPS = 30; int rgbFPS = 30; - int previewSize = 416; + int previewHeight = 416; + int previewWidth = 416; std::string monoResolution = "720p", rgbResolution = "1080p"; badParams += !pnh.getParam("tf_prefix", tfPrefix); @@ -246,7 +249,8 @@ int main(int argc, char** argv) { badParams += !pnh.getParam("resourceBaseFolder", resourceBaseFolder); badParams += !pnh.getParam("monoFPS", monoFPS); badParams += !pnh.getParam("rgbFPS", rgbFPS); - badParams += !pnh.getParam("previewSize", previewSize); + badParams += !pnh.getParam("previewHeight", previewHeight); + badParams += !pnh.getParam("previewWidth", previewWidth); badParams += !pnh.getParam("lrcheck", lrcheck); badParams += !pnh.getParam("extended", extended); badParams += !pnh.getParam("rgbScaleNumerator", rgbScaleNumerator); @@ -288,7 +292,8 @@ int main(int argc, char** argv) { LRchecktresh, monoFPS, rgbFPS, - previewSize, + previewWidth, + previewHeight, rgbScaleNumerator, rgbScaleDinominator, monoResolution, @@ -362,7 +367,7 @@ int main(int argc, char** argv) { // Preview dai::rosBridge::ImageConverter previewConverter(tfPrefix + "_rgb_camera_optical_frame", false); - auto previewCameraInfo = previewConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, previewSize, previewSize); + auto previewCameraInfo = previewConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, previewWidth, previewHeight); dai::rosBridge::BridgePublisher previewPublish( previewQueue, pnh, @@ -374,7 +379,7 @@ int main(int argc, char** argv) { previewPublish.addPublisherCallback(); // SpatialDetections - dai::rosBridge::SpatialDetectionConverter detConverter(tfPrefix + "_rgb_camera_optical_frame", previewSize, previewSize, false); + dai::rosBridge::SpatialDetectionConverter detConverter(tfPrefix + "_rgb_camera_optical_frame", previewWidth, previewHeight, false); dai::rosBridge::BridgePublisher detectionPublish( detectionQueue, pnh, @@ -384,7 +389,7 @@ int main(int argc, char** argv) { detectionPublish.addPublisherCallback(); // Tracklets - dai::rosBridge::TrackletConverter trackConverter(tfPrefix + "_rgb_camera_optical_frame", previewSize, previewSize, false); + dai::rosBridge::TrackletConverter trackConverter(tfPrefix + "_rgb_camera_optical_frame", previewWidth, previewHeight, rgbWidth, rgbHeight, false, false); dai::rosBridge::BridgePublisher trackletPublish( trackletQueue, pnh, From bba2b8c2b300bbbdce898fafc5421f09008000ff Mon Sep 17 00:00:00 2001 From: Nino Wegleitner Date: Tue, 23 May 2023 16:33:41 +0200 Subject: [PATCH 9/9] update pipeline --- depthai_examples/src/tracker_publisher.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/depthai_examples/src/tracker_publisher.cpp b/depthai_examples/src/tracker_publisher.cpp index 92ac61cb..3a77aba0 100644 --- a/depthai_examples/src/tracker_publisher.cpp +++ b/depthai_examples/src/tracker_publisher.cpp @@ -125,8 +125,11 @@ std::tuple createPipeline(bool syncNN, stereo->setLeftRightCheck(lrcheck); stereo->setExtendedDisparity(extended); stereo->setDepthAlign(dai::CameraBoardSocket::RGB); + stereo->setOutputSize(monoLeft->getResolutionWidth(), monoLeft->getResolutionHeight()); + // stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY); // ObjectTracker + // objectTracker->setDetectionLabelsToTrack({15}); // track only person objectTracker->setTrackerType(dai::TrackerType::ZERO_TERM_COLOR_HISTOGRAM); objectTracker->setTrackerIdAssignmentPolicy(dai::TrackerIdAssignmentPolicy::UNIQUE_ID); @@ -211,6 +214,8 @@ std::tuple createPipeline(bool syncNN, objectTracker->passthroughDetections.link(xoutNN->input); objectTracker->passthroughTrackerFrame.link(xoutRgb->input); objectTracker->out.link(xoutTracker->input); + objectTracker->inputTrackerFrame.setBlocking(false); + objectTracker->inputTrackerFrame.setQueueSize(2); // Link IMU imu->out.link(xoutImu->input);