diff --git a/deploy/TensorRT/cpp/BEV_INTEGRATION.md b/deploy/TensorRT/cpp/BEV_INTEGRATION.md new file mode 100644 index 000000000..0e44f9f11 --- /dev/null +++ b/deploy/TensorRT/cpp/BEV_INTEGRATION.md @@ -0,0 +1,31 @@ +# BEV mapping call location in `BYTETracker::update` + +The BEV mapping is now called **inside `BYTETracker::update`**, right after: + +```cpp +this->lost_stracks.assign(resb.begin(), resb.end()); +``` + +Then: + +```cpp +if (bev_renderer.get() != nullptr) +{ + bev_frame_index++; + bev_renderer->renderFrame(this->tracked_stracks, this->lost_stracks, bev_frame_index); +} +``` + +So each frame maps directly from tracker state vectors: + +- `vector tracked_stracks` +- `vector lost_stracks` + +## Enable mapping from `bytetrack.cpp` + +```cpp +BYTETracker tracker(fps, 30); +tracker.enable_bev_mapping("bev_outputs", -30.0f, 60.0f, 1000, 1000); +``` + +`STrack::gp` is used directly and expected as `std::vector{x, y}`. diff --git a/deploy/TensorRT/cpp/include/BYTETracker.h b/deploy/TensorRT/cpp/include/BYTETracker.h index 112480050..3e0f25651 100644 --- a/deploy/TensorRT/cpp/include/BYTETracker.h +++ b/deploy/TensorRT/cpp/include/BYTETracker.h @@ -1,24 +1,36 @@ -#pragma once - -#include "STrack.h" - -struct Object -{ - cv::Rect_ rect; - int label; - float prob; -}; - -class BYTETracker -{ -public: - BYTETracker(int frame_rate = 30, int track_buffer = 30); - ~BYTETracker(); - - vector update(const vector& objects); - Scalar get_color(int idx); - -private: +#pragma once + +#include "STrack.h" +#include + +class BirdEyeViewRenderer; + +struct Object +{ + cv::Rect_ rect; + int label; + float prob; +}; + +class BYTETracker +{ +public: + BYTETracker(int frame_rate = 30, int track_buffer = 30); + virtual ~BYTETracker(); + + virtual vector update(const vector& objects); + Scalar get_color(int idx); + const vector& get_tracked_stracks() const; + const vector& get_lost_stracks() const; + void enable_bev_mapping(const string& output_dir, + float y_min_m, + float y_max_m, + int canvas_width = 900, + int canvas_height = 900, + int padding_px = 50, + int grid_step_m = 10); + +private: vector joint_stracks(vector &tlista, vector &tlistb); vector joint_stracks(vector &tlista, vector &tlistb); @@ -42,8 +54,10 @@ class BYTETracker int frame_id; int max_time_lost; - vector tracked_stracks; - vector lost_stracks; - vector removed_stracks; - byte_kalman::KalmanFilter kalman_filter; -}; \ No newline at end of file + vector tracked_stracks; + vector lost_stracks; + vector removed_stracks; + byte_kalman::KalmanFilter kalman_filter; + std::unique_ptr bev_renderer; + int bev_frame_index; +}; diff --git a/deploy/TensorRT/cpp/include/BirdEyeViewRenderer.h b/deploy/TensorRT/cpp/include/BirdEyeViewRenderer.h new file mode 100644 index 000000000..45579ff44 --- /dev/null +++ b/deploy/TensorRT/cpp/include/BirdEyeViewRenderer.h @@ -0,0 +1,65 @@ +#pragma once + +#include + +#include +#include +#include + +#include "STrack.h" + +struct BEVRenderedTrackInfo { + int frame_index = -1; + int track_id = -1; + int class_id = -1; + std::vector gp = std::vector(2, 0.0F); + cv::Point2i bev_pixel = cv::Point2i(-1, -1); + bool inside_bev = false; + int track_state = TrackState::Tracked; +}; + +class BirdEyeViewRenderer { +public: + BirdEyeViewRenderer(const std::string& output_dir, + float y_min_m, + float y_max_m, + int canvas_width = 900, + int canvas_height = 900, + int padding_px = 50, + int grid_step_m = 10); + + void renderFrame(const std::vector& tracked_stracks, + const std::vector& lost_stracks, + int frame_index); + + cv::Point2i worldToImage(float x_m, float y_m, bool* inside = nullptr) const; + + const std::vector& latestRenderedTracks() const; + +private: + struct Range { + float min_x; + float max_x; + float min_y; + float max_y; + }; + + bool ensureOutputDirectory(const std::string& dir) const; + std::string frameOutputPath(int frame_index) const; + cv::Mat buildCanvas() const; + void drawGridAndAxes(cv::Mat& canvas) const; + void drawTrackPoint(cv::Mat& canvas, const STrack& track, int frame_index); + cv::Scalar getTrackColor(int track_id); + std::string resolveClassText(const STrack& track) const; + +private: + std::string output_dir_; + Range world_range_; + int canvas_width_; + int canvas_height_; + int padding_px_; + int grid_step_m_; + + std::unordered_map track_color_table_; + std::vector latest_rendered_tracks_; +}; diff --git a/deploy/TensorRT/cpp/include/BirdEyeViewTracker.h b/deploy/TensorRT/cpp/include/BirdEyeViewTracker.h new file mode 100644 index 000000000..ae7c375f6 --- /dev/null +++ b/deploy/TensorRT/cpp/include/BirdEyeViewTracker.h @@ -0,0 +1,26 @@ +#pragma once + +#include "BYTETracker.h" +#include "BirdEyeViewRenderer.h" + +class BirdEyeViewTracker : public BYTETracker { +public: + BirdEyeViewTracker(int frame_rate, + int track_buffer, + const std::string& bev_output_dir, + float y_min_m, + float y_max_m, + int canvas_width = 900, + int canvas_height = 900, + int padding_px = 50, + int grid_step_m = 10); + + vector update(const vector& objects) override; + + const BirdEyeViewRenderer& renderer() const; + BirdEyeViewRenderer& renderer(); + +private: + BirdEyeViewRenderer bev_renderer_; + int bev_frame_index_; +}; diff --git a/deploy/TensorRT/cpp/include/STrack.h b/deploy/TensorRT/cpp/include/STrack.h index 4bd9d000c..eda3335c6 100644 --- a/deploy/TensorRT/cpp/include/STrack.h +++ b/deploy/TensorRT/cpp/include/STrack.h @@ -10,8 +10,8 @@ enum TrackState { New = 0, Tracked, Lost, Removed }; class STrack { -public: - STrack(vector tlwh_, float score); +public: + STrack(vector tlwh_, float score, int class_id = -1, vector gp = vector(2, 0.0F)); ~STrack(); vector static tlbr_to_tlwh(vector &tlbr); @@ -43,8 +43,10 @@ class STrack KAL_MEAN mean; KAL_COVA covariance; - float score; - -private: + float score; + int class_id; + vector gp; + +private: byte_kalman::KalmanFilter kalman_filter; -}; \ No newline at end of file +}; diff --git a/deploy/TensorRT/cpp/src/BYTETracker.cpp b/deploy/TensorRT/cpp/src/BYTETracker.cpp index 7e036e996..8f81af180 100644 --- a/deploy/TensorRT/cpp/src/BYTETracker.cpp +++ b/deploy/TensorRT/cpp/src/BYTETracker.cpp @@ -1,5 +1,6 @@ -#include "BYTETracker.h" -#include +#include "BYTETracker.h" +#include "BirdEyeViewRenderer.h" +#include BYTETracker::BYTETracker(int frame_rate, int track_buffer) { @@ -7,14 +8,15 @@ BYTETracker::BYTETracker(int frame_rate, int track_buffer) high_thresh = 0.6; match_thresh = 0.8; - frame_id = 0; - max_time_lost = int(frame_rate / 30.0 * track_buffer); - cout << "Init ByteTrack!" << endl; -} + frame_id = 0; + bev_frame_index = 0; + max_time_lost = int(frame_rate / 30.0 * track_buffer); + cout << "Init ByteTrack!" << endl; +} -BYTETracker::~BYTETracker() -{ -} +BYTETracker::~BYTETracker() +{ +} vector BYTETracker::update(const vector& objects) { @@ -51,7 +53,7 @@ vector BYTETracker::update(const vector& objects) float score = objects[i].prob; - STrack strack(STrack::tlbr_to_tlwh(tlbr_), score); + STrack strack(STrack::tlbr_to_tlwh(tlbr_), score, objects[i].label); if (score >= track_thresh) { detections.push_back(strack); @@ -227,15 +229,43 @@ vector BYTETracker::update(const vector& objects) this->tracked_stracks.clear(); this->tracked_stracks.assign(resa.begin(), resa.end()); - this->lost_stracks.clear(); - this->lost_stracks.assign(resb.begin(), resb.end()); - - for (int i = 0; i < this->tracked_stracks.size(); i++) - { + this->lost_stracks.clear(); + this->lost_stracks.assign(resb.begin(), resb.end()); + + if (bev_renderer.get() != nullptr) + { + bev_frame_index++; + bev_renderer->renderFrame(this->tracked_stracks, this->lost_stracks, bev_frame_index); + } + + for (int i = 0; i < this->tracked_stracks.size(); i++) + { if (this->tracked_stracks[i].is_activated) { output_stracks.push_back(this->tracked_stracks[i]); } } - return output_stracks; -} \ No newline at end of file + return output_stracks; +} + +const vector& BYTETracker::get_tracked_stracks() const +{ + return tracked_stracks; +} + +const vector& BYTETracker::get_lost_stracks() const +{ + return lost_stracks; +} + +void BYTETracker::enable_bev_mapping(const string& output_dir, + float y_min_m, + float y_max_m, + int canvas_width, + int canvas_height, + int padding_px, + int grid_step_m) +{ + bev_renderer.reset(new BirdEyeViewRenderer(output_dir, y_min_m, y_max_m, canvas_width, canvas_height, padding_px, grid_step_m)); + bev_frame_index = 0; +} diff --git a/deploy/TensorRT/cpp/src/BirdEyeViewRenderer.cpp b/deploy/TensorRT/cpp/src/BirdEyeViewRenderer.cpp new file mode 100644 index 000000000..d2d7b6e1c --- /dev/null +++ b/deploy/TensorRT/cpp/src/BirdEyeViewRenderer.cpp @@ -0,0 +1,267 @@ +#include "BirdEyeViewRenderer.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace { +cv::Scalar hsvToBgr(float h_deg, float s, float v) { + cv::Mat hsv(1, 1, CV_32FC3, cv::Scalar(h_deg, s, v)); + cv::Mat bgr; + cv::cvtColor(hsv, bgr, cv::COLOR_HSV2BGR); + const cv::Vec3f color = bgr.at(0, 0); + return cv::Scalar(color[0] * 255.0F, color[1] * 255.0F, color[2] * 255.0F); +} + +float clamp01(float v) { + return std::max(0.0F, std::min(1.0F, v)); +} +} // namespace + +BirdEyeViewRenderer::BirdEyeViewRenderer(const std::string& output_dir, + float y_min_m, + float y_max_m, + int canvas_width, + int canvas_height, + int padding_px, + int grid_step_m) + : output_dir_(output_dir), + canvas_width_(canvas_width), + canvas_height_(canvas_height), + padding_px_(padding_px), + grid_step_m_(grid_step_m) { + world_range_.min_x = -40.0F; + world_range_.max_x = 50.0F; + world_range_.min_y = std::min(y_min_m, y_max_m); + world_range_.max_y = std::max(y_min_m, y_max_m); + + ensureOutputDirectory(output_dir_); +} + +void BirdEyeViewRenderer::renderFrame(const std::vector& tracked_stracks, + const std::vector& lost_stracks, + int frame_index) { + cv::Mat canvas = buildCanvas(); + latest_rendered_tracks_.clear(); + latest_rendered_tracks_.reserve(tracked_stracks.size() + lost_stracks.size()); + + for (size_t i = 0; i < tracked_stracks.size(); ++i) { + drawTrackPoint(canvas, tracked_stracks[i], frame_index); + } + for (size_t i = 0; i < lost_stracks.size(); ++i) { + drawTrackPoint(canvas, lost_stracks[i], frame_index); + } + + const std::string output_path = frameOutputPath(frame_index); + cv::imwrite(output_path, canvas); +} + +cv::Point2i BirdEyeViewRenderer::worldToImage(float x_m, float y_m, bool* inside) const { + const float x_span = world_range_.max_x - world_range_.min_x; + const float y_span = world_range_.max_y - world_range_.min_y; + const float usable_w = static_cast(canvas_width_ - 2 * padding_px_); + const float usable_h = static_cast(canvas_height_ - 2 * padding_px_); + + const float nx = (x_m - world_range_.min_x) / x_span; + const float ny = (y_m - world_range_.min_y) / y_span; + + const int px = static_cast(std::round(padding_px_ + clamp01(nx) * usable_w)); + const int py = static_cast(std::round(canvas_height_ - padding_px_ - clamp01(ny) * usable_h)); + + const bool is_inside = (x_m >= world_range_.min_x && x_m <= world_range_.max_x && y_m >= world_range_.min_y && + y_m <= world_range_.max_y); + if (inside != nullptr) { + *inside = is_inside; + } + + return cv::Point2i(px, py); +} + +const std::vector& BirdEyeViewRenderer::latestRenderedTracks() const { + return latest_rendered_tracks_; +} + +bool BirdEyeViewRenderer::ensureOutputDirectory(const std::string& dir) const { + if (dir.empty()) { + return false; + } + + std::string current; + current.reserve(dir.size()); + for (size_t i = 0; i < dir.size(); ++i) { + current.push_back(dir[i]); + if (dir[i] == '/' || i + 1 == dir.size()) { + if (current.empty() || current == "/") { + continue; + } + if (mkdir(current.c_str(), 0755) != 0 && errno != EEXIST) { + return false; + } + } + } + return true; +} + +std::string BirdEyeViewRenderer::frameOutputPath(int frame_index) const { + std::ostringstream oss; + oss << output_dir_ << "/bev_" << std::setfill('0') << std::setw(6) << frame_index << ".jpg"; + return oss.str(); +} + +cv::Mat BirdEyeViewRenderer::buildCanvas() const { + cv::Mat canvas(canvas_height_, canvas_width_, CV_8UC3, cv::Scalar(245, 245, 245)); + drawGridAndAxes(canvas); + return canvas; +} + +void BirdEyeViewRenderer::drawGridAndAxes(cv::Mat& canvas) const { + const cv::Point2i top_left(padding_px_, padding_px_); + const cv::Point2i bottom_right(canvas_width_ - padding_px_, canvas_height_ - padding_px_); + + cv::rectangle(canvas, top_left, bottom_right, cv::Scalar(190, 190, 190), 1, cv::LINE_AA); + + for (int x = static_cast(std::ceil(world_range_.min_x)); x <= static_cast(std::floor(world_range_.max_x)); + x += grid_step_m_) { + bool inside = false; + const int px = worldToImage(static_cast(x), world_range_.min_y, &inside).x; + if (!inside && (x < world_range_.min_x || x > world_range_.max_x)) { + continue; + } + cv::line(canvas, + cv::Point(px, padding_px_), + cv::Point(px, canvas_height_ - padding_px_), + cv::Scalar(220, 220, 220), + 1, + cv::LINE_AA); + cv::putText(canvas, + std::to_string(x) + "m", + cv::Point(px + 2, canvas_height_ - padding_px_ + 18), + cv::FONT_HERSHEY_SIMPLEX, + 0.35, + cv::Scalar(100, 100, 100), + 1, + cv::LINE_AA); + } + + for (int y = static_cast(std::ceil(world_range_.min_y)); y <= static_cast(std::floor(world_range_.max_y)); + y += grid_step_m_) { + bool inside = false; + const int py = worldToImage(world_range_.min_x, static_cast(y), &inside).y; + if (!inside && (y < world_range_.min_y || y > world_range_.max_y)) { + continue; + } + cv::line(canvas, + cv::Point(padding_px_, py), + cv::Point(canvas_width_ - padding_px_, py), + cv::Scalar(220, 220, 220), + 1, + cv::LINE_AA); + cv::putText(canvas, + std::to_string(y) + "m", + cv::Point(8, py - 2), + cv::FONT_HERSHEY_SIMPLEX, + 0.35, + cv::Scalar(100, 100, 100), + 1, + cv::LINE_AA); + } + + cv::putText(canvas, + "BEV map (meters)", + cv::Point(padding_px_, 25), + cv::FONT_HERSHEY_SIMPLEX, + 0.6, + cv::Scalar(60, 60, 60), + 2, + cv::LINE_AA); + + cv::putText(canvas, + "x range: [-40, 50] m", + cv::Point(canvas_width_ - 210, 25), + cv::FONT_HERSHEY_SIMPLEX, + 0.45, + cv::Scalar(80, 80, 80), + 1, + cv::LINE_AA); +} + +void BirdEyeViewRenderer::drawTrackPoint(cv::Mat& canvas, const STrack& track, int frame_index) { + const float gp_x = track.gp.size() > 0 ? track.gp[0] : 0.0F; + const float gp_y = track.gp.size() > 1 ? track.gp[1] : 0.0F; + bool inside = false; + const cv::Point2i pixel = worldToImage(gp_x, gp_y, &inside); + const cv::Scalar color = getTrackColor(track.track_id); + + BEVRenderedTrackInfo rendered; + rendered.frame_index = frame_index; + rendered.track_id = track.track_id; + rendered.class_id = track.class_id; + rendered.gp = track.gp; + rendered.bev_pixel = pixel; + rendered.inside_bev = inside; + rendered.track_state = track.track_state; + latest_rendered_tracks_.push_back(rendered); + + if (!inside) { + return; + } + + if (track.track_state == TrackState::Lost) { + cv::drawMarker(canvas, pixel, color, cv::MARKER_TILTED_CROSS, 14, 2, cv::LINE_AA); + } else { + cv::circle(canvas, pixel, 5, color, cv::FILLED, cv::LINE_AA); + cv::circle(canvas, pixel, 9, color, 1, cv::LINE_AA); + } + + const std::string class_text = resolveClassText(track); + std::ostringstream oss; + oss << "id:" << track.track_id; + if (!class_text.empty()) { + oss << " cls:" << class_text; + } + if (track.track_state == TrackState::Lost) { + oss << " LOST"; + } + + cv::putText(canvas, + oss.str(), + cv::Point(pixel.x + 8, pixel.y - 8), + cv::FONT_HERSHEY_SIMPLEX, + 0.42, + color, + 2, + cv::LINE_AA); + cv::putText(canvas, + oss.str(), + cv::Point(pixel.x + 8, pixel.y - 8), + cv::FONT_HERSHEY_SIMPLEX, + 0.42, + cv::Scalar(255, 255, 255), + 1, + cv::LINE_AA); +} + +cv::Scalar BirdEyeViewRenderer::getTrackColor(int track_id) { + const std::unordered_map::const_iterator it = track_color_table_.find(track_id); + if (it != track_color_table_.end()) { + return it->second; + } + + const int hue_seed = (track_id * 37) % 180; + const cv::Scalar color = hsvToBgr(static_cast(hue_seed), 0.85F, 0.95F); + track_color_table_[track_id] = color; + return color; +} + +std::string BirdEyeViewRenderer::resolveClassText(const STrack& track) const { + if (track.class_id >= 0) { + return std::to_string(track.class_id); + } + return ""; +} diff --git a/deploy/TensorRT/cpp/src/BirdEyeViewTracker.cpp b/deploy/TensorRT/cpp/src/BirdEyeViewTracker.cpp new file mode 100644 index 000000000..6f37b5c3b --- /dev/null +++ b/deploy/TensorRT/cpp/src/BirdEyeViewTracker.cpp @@ -0,0 +1,32 @@ +#include "BirdEyeViewTracker.h" + +BirdEyeViewTracker::BirdEyeViewTracker(int frame_rate, + int track_buffer, + const std::string& bev_output_dir, + float y_min_m, + float y_max_m, + int canvas_width, + int canvas_height, + int padding_px, + int grid_step_m) + : BYTETracker(frame_rate, track_buffer), + bev_renderer_(bev_output_dir, y_min_m, y_max_m, canvas_width, canvas_height, padding_px, grid_step_m), + bev_frame_index_(0) { +} + +vector BirdEyeViewTracker::update(const vector& objects) { + vector output_tracks = BYTETracker::update(objects); + + ++bev_frame_index_; + bev_renderer_.renderFrame(get_tracked_stracks(), get_lost_stracks(), bev_frame_index_); + + return output_tracks; +} + +const BirdEyeViewRenderer& BirdEyeViewTracker::renderer() const { + return bev_renderer_; +} + +BirdEyeViewRenderer& BirdEyeViewTracker::renderer() { + return bev_renderer_; +} diff --git a/deploy/TensorRT/cpp/src/STrack.cpp b/deploy/TensorRT/cpp/src/STrack.cpp index 0e8653e8f..dd93ef2b1 100644 --- a/deploy/TensorRT/cpp/src/STrack.cpp +++ b/deploy/TensorRT/cpp/src/STrack.cpp @@ -1,7 +1,7 @@ #include "STrack.h" -STrack::STrack(vector tlwh_, float score) -{ +STrack::STrack(vector tlwh_, float score, int class_id, vector gp) +{ _tlwh.resize(4); _tlwh.assign(tlwh_.begin(), tlwh_.end()); @@ -16,9 +16,13 @@ STrack::STrack(vector tlwh_, float score) static_tlbr(); frame_id = 0; tracklet_len = 0; - this->score = score; - start_frame = 0; -} + this->score = score; + this->class_id = class_id; + this->gp = gp; + if (this->gp.size() < 2) + this->gp.assign(2, 0.0F); + start_frame = 0; +} STrack::~STrack() { @@ -77,10 +81,12 @@ void STrack::re_activate(STrack &new_track, int frame_id, bool new_id) this->state = TrackState::Tracked; this->is_activated = true; this->frame_id = frame_id; - this->score = new_track.score; - if (new_id) - this->track_id = next_id(); -} + this->score = new_track.score; + this->class_id = new_track.class_id; + this->gp = new_track.gp; + if (new_id) + this->track_id = next_id(); +} void STrack::update(STrack &new_track, int frame_id) { @@ -104,8 +110,10 @@ void STrack::update(STrack &new_track, int frame_id) this->state = TrackState::Tracked; this->is_activated = true; - this->score = new_track.score; -} + this->score = new_track.score; + this->class_id = new_track.class_id; + this->gp = new_track.gp; +} void STrack::static_tlwh() { @@ -191,4 +199,4 @@ void STrack::multi_predict(vector &stracks, byte_kalman::KalmanFilter & stracks[i]->static_tlwh(); stracks[i]->static_tlbr(); } -} \ No newline at end of file +} diff --git a/deploy/TensorRT/cpp/src/bytetrack.cpp b/deploy/TensorRT/cpp/src/bytetrack.cpp index 5b99eed8f..fe0f32bea 100644 --- a/deploy/TensorRT/cpp/src/bytetrack.cpp +++ b/deploy/TensorRT/cpp/src/bytetrack.cpp @@ -445,6 +445,7 @@ int main(int argc, char** argv) { Mat img; BYTETracker tracker(fps, 30); + tracker.enable_bev_mapping("bev_outputs", -30.0F, 60.0F, 1000, 1000); int num_frames = 0; int total_ms = 0; while (true)