diff --git a/streamer/CMakeLists.txt b/streamer/CMakeLists.txt index 4b8b3c45b7..8fa50af257 100644 --- a/streamer/CMakeLists.txt +++ b/streamer/CMakeLists.txt @@ -53,3 +53,4 @@ target_link_libraries(fd_streamer ${FASTDEPLOY_LIBS} ${DEPEND_LIBS}) add_subdirectory(src/gstreamer/meta) add_subdirectory(src/gstreamer/plugin/fdinfer) +add_subdirectory(src/gstreamer/plugin/fdtracker) diff --git a/streamer/src/gstreamer/plugin/fdtracker/CMakeLists.txt b/streamer/src/gstreamer/plugin/fdtracker/CMakeLists.txt new file mode 100644 index 0000000000..1d61baae9c --- /dev/null +++ b/streamer/src/gstreamer/plugin/fdtracker/CMakeLists.txt @@ -0,0 +1,18 @@ +# Copyright (c) 2022 PaddlePaddle Authors. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +file(GLOB_RECURSE ALL_SRCS ${PROJECT_SOURCE_DIR}/*.cc ${PROJECT_SOURCE_DIR}/src/*.cpp) + +add_library(gstfdtracker SHARED ${ALL_SRCS} ) +target_link_libraries(gstfdtracker ${DEPEND_LIBS} ${FASTDEPLOY_LIBS}) diff --git a/streamer/src/gstreamer/plugin/fdtracker/gstfdtracker.cc b/streamer/src/gstreamer/plugin/fdtracker/gstfdtracker.cc new file mode 100644 index 0000000000..08f58cf64b --- /dev/null +++ b/streamer/src/gstreamer/plugin/fdtracker/gstfdtracker.cc @@ -0,0 +1,270 @@ +// Copyright (c) 2022 PaddlePaddle Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gstfdtracker.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "gstnvdsmeta.h" + +GST_DEBUG_CATEGORY_STATIC(gst_fdtracker_debug_category); +#define GST_CAT_DEFAULT gst_fdtracker_debug_category + +/* prototypes */ + +static void gst_fdtracker_set_property(GObject *object, guint property_id, + const GValue *value, GParamSpec *pspec); +static void gst_fdtracker_get_property(GObject *object, guint property_id, + GValue *value, GParamSpec *pspec); +static gboolean gst_fdtracker_set_caps(GstBaseTransform *trans, GstCaps *incaps, + GstCaps *outcaps); +static gboolean gst_fdtracker_start(GstBaseTransform *trans); +static gboolean gst_fdtracker_stop(GstBaseTransform *trans); +static GstFlowReturn gst_fdtracker_transform_ip(GstBaseTransform *trans, + GstBuffer *buf); + +enum { PROP_0 }; + +/* pad templates */ +#define VIDEO_CAPS \ + GST_VIDEO_CAPS_MAKE("{ NV12, RGBA }") \ + ";" GST_VIDEO_CAPS_MAKE_WITH_FEATURES("memory:NVMM", "{ NV12, RGBA }") + +static GstStaticPadTemplate gst_fdtracker_src_template = + GST_STATIC_PAD_TEMPLATE("src", GST_PAD_SRC, GST_PAD_ALWAYS, + GST_STATIC_CAPS(GST_VIDEO_CAPS_MAKE_WITH_FEATURES( + "memory:NVMM", "{ NV12, RGBA }"))); + +static GstStaticPadTemplate gst_fdtracker_sink_template = + GST_STATIC_PAD_TEMPLATE("sink", GST_PAD_SINK, GST_PAD_ALWAYS, + GST_STATIC_CAPS(GST_VIDEO_CAPS_MAKE_WITH_FEATURES( + "memory:NVMM", "{ NV12, RGBA }"))); + +/* class initialization */ +G_DEFINE_TYPE_WITH_CODE( + GstFdtracker, gst_fdtracker, GST_TYPE_BASE_TRANSFORM, + GST_DEBUG_CATEGORY_INIT(gst_fdtracker_debug_category, "fdtracker", 0, + "debug category for fdtracker element")); + +static void gst_fdtracker_class_init(GstFdtrackerClass *klass) { + std::cout << "gst_fdtracker_class_init" << std::endl; + GObjectClass *gobject_class = G_OBJECT_CLASS(klass); + GstBaseTransformClass *base_transform_class = GST_BASE_TRANSFORM_CLASS(klass); + + /* Setting up pads and setting metadata should be moved to + base_class_init if you intend to subclass this class. */ + gst_element_class_add_pad_template( + GST_ELEMENT_CLASS(klass), + gst_static_pad_template_get(&gst_fdtracker_src_template)); + gst_element_class_add_pad_template( + GST_ELEMENT_CLASS(klass), + gst_static_pad_template_get(&gst_fdtracker_sink_template)); + + gst_element_class_set_static_metadata( + GST_ELEMENT_CLASS(klass), "FIXME Long name", "Generic", + "FIXME Description", "FIXME "); + + gobject_class->set_property = gst_fdtracker_set_property; + gobject_class->get_property = gst_fdtracker_get_property; + base_transform_class->set_caps = GST_DEBUG_FUNCPTR(gst_fdtracker_set_caps); + base_transform_class->start = GST_DEBUG_FUNCPTR(gst_fdtracker_start); + base_transform_class->stop = GST_DEBUG_FUNCPTR(gst_fdtracker_stop); + base_transform_class->transform_ip = + GST_DEBUG_FUNCPTR(gst_fdtracker_transform_ip); +} + +static void gst_fdtracker_init(GstFdtracker *fdtracker) { + /* We will not be generating a new buffer. Just adding / updating + * metadata. */ + gst_base_transform_set_in_place(GST_BASE_TRANSFORM(fdtracker), TRUE); + /* We do not want to change the input caps. Set to passthrough. transform_ip + * is still called. */ + gst_base_transform_set_passthrough(GST_BASE_TRANSFORM(fdtracker), TRUE); +} + +void gst_fdtracker_set_property(GObject *object, guint property_id, + const GValue *value, GParamSpec *pspec) { + GstFdtracker *fdtracker = GST_FDTRACKER(object); + + GST_DEBUG_OBJECT(fdtracker, "set_property"); + + switch (property_id) { + default: + G_OBJECT_WARN_INVALID_PROPERTY_ID(object, property_id, pspec); + break; + } +} + +void gst_fdtracker_get_property(GObject *object, guint property_id, + GValue *value, GParamSpec *pspec) { + GstFdtracker *fdtracker = GST_FDTRACKER(object); + + GST_DEBUG_OBJECT(fdtracker, "get_property"); + + switch (property_id) { + default: + G_OBJECT_WARN_INVALID_PROPERTY_ID(object, property_id, pspec); + break; + } +} + +static gboolean gst_fdtracker_set_caps(GstBaseTransform *trans, GstCaps *incaps, + GstCaps *outcaps) { + GstFdtracker *fdtracker = GST_FDTRACKER(trans); + + GST_DEBUG_OBJECT(fdtracker, "set_caps"); + + return TRUE; +} + +/* states */ +static gboolean gst_fdtracker_start(GstBaseTransform *trans) { + GstFdtracker *fdtracker = GST_FDTRACKER(trans); + + GST_DEBUG_OBJECT(fdtracker, "start"); + + fdtracker->tracker_per_class = new std::map; + return TRUE; +} + +static gboolean gst_fdtracker_stop(GstBaseTransform *trans) { + GstFdtracker *fdtracker = GST_FDTRACKER(trans); + + GST_DEBUG_OBJECT(fdtracker, "stop"); + + delete fdtracker->tracker_per_class; + return TRUE; +} + +static GstFlowReturn gst_fdtracker_transform_ip(GstBaseTransform *trans, + GstBuffer *buf) { + GstFdtracker *fdtracker = GST_FDTRACKER(trans); + + NvDsObjectMeta *obj_meta = NULL; + NvDsMetaList *l_frame = NULL; + NvDsMetaList *l_obj = NULL; + NvBbox_Coords detector_bbox; + + NvDsBatchMeta *batch_meta = gst_buffer_get_nvds_batch_meta(buf); + + std::cout << "This is a new frame!" << std::endl; + for (l_frame = batch_meta->frame_meta_list; l_frame != NULL; + l_frame = l_frame->next) { + std::map> bbox_per_class; + NvDsFrameMeta *frame_meta = (NvDsFrameMeta *)(l_frame->data); + + for (l_obj = frame_meta->obj_meta_list; l_obj != NULL; + l_obj = l_obj->next) { + obj_meta = (NvDsObjectMeta *)(l_obj->data); + if (bbox_per_class.find(obj_meta->class_id) == bbox_per_class.end()) { + std::vector vec; + bbox_per_class[obj_meta->class_id] = vec; + } + detector_bbox = obj_meta->detector_bbox_info.org_bbox_coords; + bbox_per_class[obj_meta->class_id].emplace_back(obj_meta->class_id); + bbox_per_class[obj_meta->class_id].emplace_back(obj_meta->confidence); + bbox_per_class[obj_meta->class_id].emplace_back(detector_bbox.left); + bbox_per_class[obj_meta->class_id].emplace_back(detector_bbox.top); + bbox_per_class[obj_meta->class_id].emplace_back(detector_bbox.left + + detector_bbox.width); + bbox_per_class[obj_meta->class_id].emplace_back(detector_bbox.top + + detector_bbox.height); + } + + nvds_clear_obj_meta_list(frame_meta, frame_meta->obj_meta_list); + + for (std::map>::iterator iter = + bbox_per_class.begin(); + iter != bbox_per_class.end(); iter++) { + std::map *octracker = fdtracker->tracker_per_class; + if (octracker->find(iter->first) == octracker->end()) { + octracker->insert( + std::make_pair(iter->first, new OcSortTracker(iter->first))); + } + cv::Mat dets(iter->second.size() / 6, 6, CV_32FC1, cv::Scalar(0)); + memcpy(dets.data, iter->second.data(), + iter->second.size() * sizeof(float)); + (*octracker)[iter->first]->update(dets, true, false); + cv::Mat trackers = (*octracker)[iter->first]->get_trackers(); + for (int i = 0; i < trackers.rows; i++) { + NvDsBatchMeta *batch_meta_pool = frame_meta->base_meta.batch_meta; + NvDsObjectMeta *object_meta = + nvds_acquire_obj_meta_from_pool(batch_meta_pool); + NvOSD_RectParams &rect_params = object_meta->rect_params; + NvOSD_TextParams &text_params = object_meta->text_params; + detector_bbox = object_meta->detector_bbox_info.org_bbox_coords; + detector_bbox.left = *(trackers.ptr(i, 2)); + detector_bbox.top = *(trackers.ptr(i, 3)); + detector_bbox.width = + *(trackers.ptr(i, 4)) - *(trackers.ptr(i, 2)); + detector_bbox.height = + *(trackers.ptr(i, 5)) - *(trackers.ptr(i, 3)); + rect_params.left = detector_bbox.left; + rect_params.top = detector_bbox.top; + rect_params.width = detector_bbox.width; + rect_params.height = detector_bbox.height; + /* Font to be used for label text. */ + static gchar font_name[] = "Serif"; + /* Semi-transparent yellow background. */ + rect_params.has_bg_color = 0; + rect_params.bg_color = (NvOSD_ColorParams){1, 1, 0, 0.4}; + /* Red border of width 6. */ + rect_params.border_width = 3; + rect_params.border_color = (NvOSD_ColorParams){1, 0, 0, 1}; + object_meta->class_id = *(trackers.ptr(i, 0)); + object_meta->object_id = *(trackers.ptr(i, 1)); + std::string text = std::to_string(object_meta->object_id); + text_params.display_text = g_strdup(text.c_str()); + /* Display text above the left top corner of the object. */ + text_params.x_offset = rect_params.left; + text_params.y_offset = rect_params.top - 10; + /* Set black background for the text. */ + text_params.set_bg_clr = 1; + text_params.text_bg_clr = (NvOSD_ColorParams){0, 0, 0, 1}; + /* Font face, size and color. */ + text_params.font_params.font_name = font_name; + text_params.font_params.font_size = 11; + text_params.font_params.font_color = (NvOSD_ColorParams){1, 1, 1, 1}; + nvds_add_obj_meta_to_frame(frame_meta, object_meta, + object_meta->parent); + } + } + } + GST_DEBUG_OBJECT(fdtracker, "transform_ip"); + + return GST_FLOW_OK; +} + +static gboolean plugin_init(GstPlugin *plugin) { + return gst_element_register(plugin, "fdtracker", GST_RANK_PRIMARY, + GST_TYPE_FDTRACKER); +} + +#define VERSION "0.0.1" +#define PACKAGE "fdtracker" +#define PACKAGE_NAME "PaddlePaddle FastDeploy Streamer FDInfer plugin" +#define GST_PACKAGE_ORIGIN "https://github.com/PaddlePaddle/FastDeploy" + +// GSTreamer is under LGPL license, while FastDeploy is under Apache-2.0 +// license, +// so please follow both when using this plugin. +GST_PLUGIN_DEFINE(GST_VERSION_MAJOR, GST_VERSION_MINOR, fdtracker, + "FIXME plugin description", plugin_init, VERSION, "LGPL", + PACKAGE_NAME, GST_PACKAGE_ORIGIN) diff --git a/streamer/src/gstreamer/plugin/fdtracker/gstfdtracker.h b/streamer/src/gstreamer/plugin/fdtracker/gstfdtracker.h new file mode 100644 index 0000000000..7b202ceb0d --- /dev/null +++ b/streamer/src/gstreamer/plugin/fdtracker/gstfdtracker.h @@ -0,0 +1,54 @@ +// Copyright (c) 2022 PaddlePaddle Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef STREAMER_SRC_GSTREAMER_PLUGIN_FDTRACKER_GSTFDTRACKER_H_ +#define STREAMER_SRC_GSTREAMER_PLUGIN_FDTRACKER_GSTFDTRACKER_H_ + +#include +#include +#include +#include +#include +#include "include/ocsort.h" + +G_BEGIN_DECLS + +#define GST_TYPE_FDTRACKER (gst_fdtracker_get_type()) +#define GST_FDTRACKER(obj) G_TYPE_CHECK_INSTANCE_CAST(obj) \ + (G_TYPE_CHECK_INSTANCE_CAST((obj), GST_TYPE_FDTRACKER, GstFdtracker) +#define GST_FDTRAKERCLASS(klass) G_TYPE_CHECK_CLASS_CAST(klass) \ + (G_TYPE_CHECK_CLASS_CAST(klass), GST_TYPE_FDTRAKERGstFdtrackerClass) +#define GST_IS_FDTRACKER(obj) G_TYPE_CHECK_INSTANCE_TYPE(obj) \ + (G_TYPE_CHECK_INSTANCE_TYPE(obj), GST_TYPE_FDTRACKER) +#define GST_IS_FDTRACKER_CLASS(obj) G_TYPE_CHECK_CLASS_TYPE(klass) \ + (G_TYPE_CHECK_CLASS_TYPE(klass), GST_TYPE_FDTRACKER) + +typedef struct _GstFdtracker GstFdtracker; +typedef struct _GstFdtrackerClass GstFdtrackerClass; + +struct _GstFdtracker { + GstBaseTransform base_fdtracker; + + std::map* tracker_per_class; +}; + +struct _GstFdtrackerClass { + GstBaseTransformClass base_fdtracker_class; +}; + +GType gst_fdtracker_get_type(void); + +G_END_DECLS + +#endif // STREAMER_SRC_GSTREAMER_PLUGIN_FDTRACKER_GSTFDTRACKER_H_ diff --git a/streamer/src/gstreamer/plugin/fdtracker/include/kalmantracker.h b/streamer/src/gstreamer/plugin/fdtracker/include/kalmantracker.h new file mode 100755 index 0000000000..309463dc63 --- /dev/null +++ b/streamer/src/gstreamer/plugin/fdtracker/include/kalmantracker.h @@ -0,0 +1,219 @@ +// Copyright (c) 2022 PaddlePaddle Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Part of the following code in this file refs to +// https://github.com/CnybTseng/JDE/blob/master/platforms/common/trajectory.h +// +// Copyright (c) 2020 CnybTseng +// Licensed under The MIT License + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#define mat2vec4f(m) \ + cv::Vec4f(*m.ptr(0, 0), \ + *m.ptr(0, 1), \ + *m.ptr(0, 2), \ + *m.ptr(0, 3)) + +typedef enum { New = 0, Tracked = 1, Lost = 2, Removed = 3 } TrajectoryState; + +inline void printmat(cv::Mat &mat) { + std::string str; + for (int i=0; i < mat.rows; i++) { + for (int j=0; j < mat.cols; j++) { + str += std::to_string(*(mat.ptr(i, j)))+" "; + } + } + printf("\nmat rows: %d, cols: %d, str: %s", mat.rows, mat.cols, str.c_str()); +} + +class TKalmanFilter : public cv::KalmanFilter { + public: + TKalmanFilter(void); + virtual ~TKalmanFilter(void) {} + virtual void init(const cv::Mat &measurement); + virtual const cv::Mat &predict(); + virtual const cv::Mat &correct(const cv::Mat &measurement); + virtual void project(cv::Mat *mean, cv::Mat *covariance) const; + + private: + float std_weight_position; + float std_weight_velocity; +}; + +inline TKalmanFilter::TKalmanFilter(void) : cv::KalmanFilter(8, 4) { + cv::KalmanFilter::transitionMatrix = cv::Mat::eye(8, 8, CV_32F); + for (int i = 0; i < 4; ++i) + cv::KalmanFilter::transitionMatrix.at(i, i + 4) = 1; + cv::KalmanFilter::measurementMatrix = cv::Mat::eye(4, 8, CV_32F); + std_weight_position = 1 / 20.f; + std_weight_velocity = 1 / 160.f; +} + +class KalmanTracker : public TKalmanFilter { + public: + KalmanTracker(); + KalmanTracker(const cv::Vec4f <rb, float score); + KalmanTracker(const KalmanTracker &other); + KalmanTracker &operator=(const KalmanTracker &rhs); + virtual ~KalmanTracker(void) {} + + void alloc_id(void); + virtual const cv::Mat &predict(void); + virtual void update(cv::Vec4f dets, bool angle_cost); + virtual void activate(int timestamp); + virtual void reactivate(KalmanTracker *traj, int timestamp, + bool newid = false); + virtual void mark_lost(void); + virtual void mark_removed(void); + virtual int get_length(void); + const cv::Mat get_state(void); + + TrajectoryState state; + cv::Vec4f ltrb; + cv::Vec4f last_observation{-1, -1, -1, -1}; + cv::Vec2f velocity; + std::map observations; + int id; + bool is_activated = false; + int time_since_update = 0; + int timestamp; + int starttime; + float score; + int age = 0; + int delta_t = 3; + int hits = 0; + int hit_streak = 0; + + private: + static int count; + cv::Vec4f xyah; + float eta; + int length = 0; +}; + +inline cv::Vec4f ltrb2xyah(const cv::Vec4f <rb) { + cv::Vec4f xyah; + xyah[0] = (ltrb[0] + ltrb[2]) * 0.5f; + xyah[1] = (ltrb[1] + ltrb[3]) * 0.5f; + xyah[3] = ltrb[3] - ltrb[1]; + xyah[2] = (ltrb[2] - ltrb[0]) / xyah[3]; + return xyah; +} + +inline cv::Mat xyah2ltrb(const cv::Mat &xyah_in) { + cv::Vec4f ltrb; + cv::Vec4f xyah = mat2vec4f(xyah_in); + ltrb[0] = xyah[0] - (xyah[3]*xyah[2])*0.5f; + ltrb[1] = xyah[1] - (xyah[3]/xyah[2])*0.5f; + ltrb[2] = xyah[0] + (xyah[3]*xyah[2])*0.5f; + ltrb[3] = xyah[1] + (xyah[3]/xyah[2])*0.5f; + return cv::Mat(ltrb).t(); +} + +inline KalmanTracker::KalmanTracker() + : state(New), + ltrb(cv::Vec4f()), + id(0), + is_activated(false), + timestamp(0), + starttime(0), + time_since_update(0), + score(0), + eta(0.9), + length(0) { + xyah = ltrb2xyah(ltrb); + alloc_id(); + TKalmanFilter::init(cv::Mat(xyah)); + } + +inline KalmanTracker::KalmanTracker(const cv::Vec4f <rb_, + float score_) + : state(New), + ltrb(ltrb_), + last_observation(ltrb_), + id(0), + age(0), + is_activated(false), + timestamp(0), + starttime(0), + time_since_update(0), + score(score_), + eta(0.9), + length(1) { + this->observations[this->age] = ltrb_; + xyah = ltrb2xyah(ltrb); + alloc_id(); + TKalmanFilter::init(cv::Mat(xyah)); +} + +inline KalmanTracker::KalmanTracker(const KalmanTracker &other) + : state(other.state), + ltrb(other.ltrb), + id(other.id), + is_activated(other.is_activated), + timestamp(other.timestamp), + starttime(other.starttime), + xyah(other.xyah), + score(other.score), + eta(other.eta), + length(other.length) { + // copy state in KalmanFilter + + other.statePre.copyTo(cv::KalmanFilter::statePre); + other.statePost.copyTo(cv::KalmanFilter::statePost); + other.errorCovPre.copyTo(cv::KalmanFilter::errorCovPre); + other.errorCovPost.copyTo(cv::KalmanFilter::errorCovPost); +} + +inline KalmanTracker &KalmanTracker::operator=(const KalmanTracker &rhs) { + this->state = rhs.state; + this->ltrb = rhs.ltrb; + this->id = rhs.id; + this->is_activated = rhs.is_activated; + this->timestamp = rhs.timestamp; + this->starttime = rhs.starttime; + this->xyah = rhs.xyah; + this->score = rhs.score; + this->eta = rhs.eta; + this->length = rhs.length; + + // copy state in KalmanFilter + + rhs.statePre.copyTo(cv::KalmanFilter::statePre); + rhs.statePost.copyTo(cv::KalmanFilter::statePost); + rhs.errorCovPre.copyTo(cv::KalmanFilter::errorCovPre); + rhs.errorCovPost.copyTo(cv::KalmanFilter::errorCovPost); + + return *this; +} + +inline void KalmanTracker::alloc_id() { + this->id = count; + ++count; +} + +inline void KalmanTracker::mark_lost(void) { state = Lost; } + +inline void KalmanTracker::mark_removed(void) { state = Removed; } + +inline int KalmanTracker::get_length(void) { return length; } diff --git a/streamer/src/gstreamer/plugin/fdtracker/include/lapjv.h b/streamer/src/gstreamer/plugin/fdtracker/include/lapjv.h new file mode 100755 index 0000000000..2dc95c3228 --- /dev/null +++ b/streamer/src/gstreamer/plugin/fdtracker/include/lapjv.h @@ -0,0 +1,62 @@ +// Copyright (c) 2022 PaddlePaddle Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Part of the following code in this file refs to +// https://github.com/gatagat/lap/blob/master/lap/lapjv.h +// +// Copyright (c) 2012-2017, Tomas Kazmar +// Licensed under The BSD 2-Clause "Simplified" License + +#ifndef STREAMER_SRC_GSTREAMER_PLUGIN_FDTRACKER_INCLUDE_LAPJV_H_ +#define STREAMER_SRC_GSTREAMER_PLUGIN_FDTRACKER_INCLUDE_LAPJV_H_ +#define LARGE 1000000 + +#if !defined TRUE +#define TRUE 1 +#endif +#if !defined FALSE +#define FALSE 0 +#endif + +#define NEW(x, t, n) \ + if ((x = reinterpret_cast(malloc(sizeof(t) * (n)))) == 0) { \ + return -1; \ + } +#define FREE(x) \ + if (x != 0) { \ + free(x); \ + x = 0; \ + } +#define SWAP_INDICES(a, b) \ + { \ + int_t _temp_index = a; \ + a = b; \ + b = _temp_index; \ + } +#include + +typedef signed int int_t; +typedef unsigned int uint_t; +typedef double cost_t; +typedef char boolean; +typedef enum fp_t { FP_1 = 1, FP_2 = 2, FP_DYNAMIC = 3 } fp_t; + +int lapjv_internal(const cv::Mat &cost, + const bool extend_cost, + const float cost_limit, + int *x, + int *y); + + +#endif // STREAMER_SRC_GSTREAMER_PLUGIN_FDTRACKER_INCLUDE_LAPJV_H_ diff --git a/streamer/src/gstreamer/plugin/fdtracker/include/ocsort.h b/streamer/src/gstreamer/plugin/fdtracker/include/ocsort.h new file mode 100755 index 0000000000..96aa164584 --- /dev/null +++ b/streamer/src/gstreamer/plugin/fdtracker/include/ocsort.h @@ -0,0 +1,55 @@ +// Copyright (c) 2022 PaddlePaddle Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include +#include +#include +#include "gstreamer/plugin/fdtracker/include/kalmantracker.h" + +class OcSortTracker { + public: + explicit OcSortTracker(int classid); + virtual ~OcSortTracker(void) {} + // static OcSortTracker *instance(void); + virtual bool update(const cv::Mat &dets, bool use_byte, bool use_angle_cost); + cv::Mat get_trackers(void); + void associate_only_iou(cv::Mat detections, cv::Mat trackers, + float iou_threshold, std::map &matches, + std::vector &mismatch_row, + std::vector &mismatch_col); + void associate(cv::Mat detections, cv::Mat trackers, float iou_threshold, + cv::Mat velocities, cv::Mat previous_obs, float vdc_weight, + std::map &matches, std::vector &mismatch_row, + std::vector &mismatch_col); + std::vector trackers; + std::vector unused_trackers; + + private: + int timestamp; + float lambda; + float det_thresh = 0.1; + int delta_t = 3; + int inertia; + float iou_threshold = 0.5; + float score_thresh = 0.3; + int max_age = 30; + int min_hits = 0; + int frame_count = 0; + int classid = 0; +}; diff --git a/streamer/src/gstreamer/plugin/fdtracker/include/trajectory.h b/streamer/src/gstreamer/plugin/fdtracker/include/trajectory.h new file mode 100755 index 0000000000..ed88652011 --- /dev/null +++ b/streamer/src/gstreamer/plugin/fdtracker/include/trajectory.h @@ -0,0 +1,48 @@ +// Copyright (c) 2022 PaddlePaddle Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include "gstreamer/plugin/fdtracker/include/ocsort.h" + +typedef enum { horizontal = 0, vertical = 1} region_type; + +class Trajectory { + public: + Trajectory(void) {} + ~Trajectory(void) {} + + std::vector> entrance_count(OcSortTracker* octracker); + std::vector breakin_count(OcSortTracker* octracker); + bool set_region(region_type inout_type, std::vector region); + bool set_area(std::vector area); + void clearset(void); + + std::vector get_region() {return this->region;} + std::vector> get_breakarea() {return this->breakarea;} + std::vector get_count() {return std::vector{this->countin, + this->countout};} + + private: + int countin = 0; + int countout = 0; + int num_pts = 0; + std::vector breakin; + std::vector breakout; + std::vector region; + std::vector> breakarea = {{0.f, 0.f}, {0.f, 0.f}, + {0.f, 0.f}}; + region_type inout_type = horizontal; +}; diff --git a/streamer/src/gstreamer/plugin/fdtracker/src/kalmantracker.cpp b/streamer/src/gstreamer/plugin/fdtracker/src/kalmantracker.cpp new file mode 100644 index 0000000000..e957af7da1 --- /dev/null +++ b/streamer/src/gstreamer/plugin/fdtracker/src/kalmantracker.cpp @@ -0,0 +1,176 @@ +// Copyright (c) 2022 PaddlePaddle Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Part of the following code in this file refs to +// https://github.com/CnybTseng/JDE/blob/master/platforms/common/trajectory.h +// +// Copyright (c) 2020 CnybTseng +// Licensed under The MIT License + +#include +#include "gstreamer/plugin/fdtracker/include/trajectory.h" + +void TKalmanFilter::init(const cv::Mat &measurement) { + measurement.copyTo(statePost(cv::Rect(0, 0, 1, 4))); + statePost(cv::Rect(0, 4, 1, 4)).setTo(0); + statePost.copyTo(statePre); + + float varpos = std_weight_position * (*measurement.ptr(3)); + varpos *= varpos; + float varvel = 100 * std_weight_velocity * (*measurement.ptr(3)); + varvel *= varvel; + + errorCovPost.setTo(0); + *errorCovPost.ptr(0, 0) = varpos; + *errorCovPost.ptr(1, 1) = varpos; + *errorCovPost.ptr(2, 2) = 1e-4f; + *errorCovPost.ptr(3, 3) = varpos; + *errorCovPost.ptr(4, 4) = varvel; + *errorCovPost.ptr(5, 5) = varvel; + *errorCovPost.ptr(6, 6) = 1e-10f; + *errorCovPost.ptr(7, 7) = varvel; + errorCovPost.copyTo(errorCovPre); +} + +const cv::Mat &TKalmanFilter::predict() { + float varpos = std_weight_position * (*statePre.ptr(3)); + varpos *= varpos; + float varvel = 100 * std_weight_velocity * (*statePre.ptr(3)); + varvel *= varvel; + + processNoiseCov.setTo(0); + *processNoiseCov.ptr(0, 0) = varpos; + *processNoiseCov.ptr(1, 1) = varpos; + *processNoiseCov.ptr(2, 2) = 1e-4f; + *processNoiseCov.ptr(3, 3) = varpos; + *processNoiseCov.ptr(4, 4) = varvel; + *processNoiseCov.ptr(5, 5) = varvel; + *processNoiseCov.ptr(6, 6) = 1e-10f; + *processNoiseCov.ptr(7, 7) = varvel; + + return cv::KalmanFilter::predict(); +} + +const cv::Mat &TKalmanFilter::correct(const cv::Mat &measurement) { + float varpos = std_weight_position * (*measurement.ptr(3)); + varpos *= varpos; + + measurementNoiseCov.setTo(0); + *measurementNoiseCov.ptr(0, 0) = varpos; + *measurementNoiseCov.ptr(1, 1) = varpos; + *measurementNoiseCov.ptr(2, 2) = 0; + *measurementNoiseCov.ptr(3, 3) = varpos; + + return cv::KalmanFilter::correct(measurement); +} + +void TKalmanFilter::project(cv::Mat *mean, cv::Mat *covariance) const { + float varpos = std_weight_position * (*statePost.ptr(3)); + varpos *= varpos; + + cv::Mat measurementNoiseCov_ = cv::Mat::eye(4, 4, CV_32F); + *measurementNoiseCov_.ptr(0, 0) = varpos; + *measurementNoiseCov_.ptr(1, 1) = varpos; + *measurementNoiseCov_.ptr(2, 2) = 0; + *measurementNoiseCov_.ptr(3, 3) = varpos; + + *mean = measurementMatrix * statePost; + cv::Mat temp = measurementMatrix * errorCovPost; + gemm(temp, measurementMatrix, 1, measurementNoiseCov_, 1, *covariance, + cv::GEMM_2_T); +} + +int KalmanTracker::count = 0; + +const cv::Mat &KalmanTracker::predict(void) { + this->age += 1; + if (this->time_since_update > 0) { + this->hit_streak = 0; + } + this->time_since_update += 1; + // if (state != Tracked) { + // *cv::KalmanFilter::statePost.ptr(4) = 0; + // *cv::KalmanFilter::statePost.ptr(5) = 0; + // *cv::KalmanFilter::statePost.ptr(6) = 0; + // *cv::KalmanFilter::statePost.ptr(7) = 0; + // } + return TKalmanFilter::predict(); +} + +const cv::Mat KalmanTracker::get_state(void) { + return xyah2ltrb(statePost(cv::Rect(0, 0, 1, 4))); + // return cv::Mat(this->ltrb); +} + +cv::Vec2f speed_direction(cv::Vec4f bbox1, cv::Vec4f bbox2) { + cv::Vec2f center1, center2, speed; + center1[0] = (bbox1[0] + bbox1[2]) / 2.0; + center1[1] = (bbox1[1] + bbox1[3]) / 2.0; + center2[0] = (bbox2[0] + bbox2[2]) / 2.0; + center2[1] = (bbox2[1] + bbox2[3]) / 2.0; + speed[0] = center1[0] - center2[0]; + speed[1] = center1[1] - center2[1]; + float norm = sqrt(speed[0] * speed[0] + speed[1] * speed[1]) + 1e-6; + speed /= norm; + return speed; +} + +void KalmanTracker::update(cv::Vec4f dets, bool angle_cost) { + if (angle_cost && this->last_observation[0] > 0) { + cv::Vec4f previous_box(-1, -1, -1, -1); + for (int i = 0; i < this->delta_t; i++) { + int dt = this->delta_t - i; + if (this->observations.find(this->age - dt) != this->observations.end()) { + previous_box = this->observations[this->age - dt]; + break; + } + } + if (previous_box[0] < 0) { + previous_box = this->last_observation; + } + this->velocity = speed_direction(dets, previous_box); + } + + auto det_xyah = ltrb2xyah(dets); + TKalmanFilter::correct(cv::Mat(det_xyah)); + this->last_observation = dets; + this->observations[this->age] = dets; + this->length += 1; + this->state = Tracked; + this->is_activated = true; + this->time_since_update = 0; + this->hits += 1; + this->hit_streak += 1; +} + +void KalmanTracker::activate(int timestamp_) { + TKalmanFilter::init(cv::Mat(xyah)); + length = 0; + state = Tracked; + if (timestamp_ == 1) { + is_activated = true; + } + timestamp = timestamp_; + starttime = timestamp_; +} + +void KalmanTracker::reactivate(KalmanTracker *traj, int timestamp_, + bool newid) { + TKalmanFilter::correct(cv::Mat(traj->xyah)); + length = 0; + state = Tracked; + is_activated = true; + timestamp = timestamp_; + if (newid) alloc_id(); +} diff --git a/streamer/src/gstreamer/plugin/fdtracker/src/lapjv.cpp b/streamer/src/gstreamer/plugin/fdtracker/src/lapjv.cpp new file mode 100644 index 0000000000..2b123e8657 --- /dev/null +++ b/streamer/src/gstreamer/plugin/fdtracker/src/lapjv.cpp @@ -0,0 +1,382 @@ +// Copyright (c) 2022 PaddlePaddle Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Part of the following code in this file refs to +// https://github.com/gatagat/lap/blob/master/lap/lapjv.h +// +// Copyright (c) 2012-2017, Tomas Kazmar +// Licensed under The BSD 2-Clause "Simplified" License + +#include +#include +#include + +#include "gstreamer/plugin/fdtracker/include/lapjv.h" + +/** Column-reduction and reduction transfer for a dense cost matrix. + */ +int _ccrrt_dense(const int n, float *cost[], int *free_rows, int *x, int *y, + float *v) { + int n_free_rows; + bool *unique; + + for (int i = 0; i < n; i++) { + x[i] = -1; + v[i] = LARGE; + y[i] = 0; + } + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + const float c = cost[i][j]; + if (c < v[j]) { + v[j] = c; + y[j] = i; + } + } + } + NEW(unique, bool, n); + memset(unique, TRUE, n); + { + int j = n; + do { + j--; + const int i = y[j]; + if (x[i] < 0) { + x[i] = j; + } else { + unique[i] = FALSE; + y[j] = -1; + } + } while (j > 0); + } + n_free_rows = 0; + for (int i = 0; i < n; i++) { + if (x[i] < 0) { + free_rows[n_free_rows++] = i; + } else if (unique[i]) { + const int j = x[i]; + float min = LARGE; + for (int j2 = 0; j2 < n; j2++) { + if (j2 == static_cast(j)) { + continue; + } + const float c = cost[i][j2] - v[j2]; + if (c < min) { + min = c; + } + } + v[j] -= min; + } + } + FREE(unique); + return n_free_rows; +} + +/** Augmenting row reduction for a dense cost matrix. + */ +int _carr_dense(const int n, float *cost[], const int n_free_rows, + int *free_rows, int *x, int *y, float *v) { + int current = 0; + int new_free_rows = 0; + int rr_cnt = 0; + while (current < n_free_rows) { + int i0; + int j1, j2; + float v1, v2, v1_new; + bool v1_lowers; + + rr_cnt++; + const int free_i = free_rows[current++]; + j1 = 0; + v1 = cost[free_i][0] - v[0]; + j2 = -1; + v2 = LARGE; + for (int j = 1; j < n; j++) { + const float c = cost[free_i][j] - v[j]; + if (c < v2) { + if (c >= v1) { + v2 = c; + j2 = j; + } else { + v2 = v1; + v1 = c; + j2 = j1; + j1 = j; + } + } + } + i0 = y[j1]; + v1_new = v[j1] - (v2 - v1); + v1_lowers = v1_new < v[j1]; + if (rr_cnt < current * n) { + if (v1_lowers) { + v[j1] = v1_new; + } else if (i0 >= 0 && j2 >= 0) { + j1 = j2; + i0 = y[j2]; + } + if (i0 >= 0) { + if (v1_lowers) { + free_rows[--current] = i0; + } else { + free_rows[new_free_rows++] = i0; + } + } + } else { + if (i0 >= 0) { + free_rows[new_free_rows++] = i0; + } + } + x[free_i] = j1; + y[j1] = free_i; + } + return new_free_rows; +} + +/** Find columns with minimum d[j] and put them on the SCAN list. + */ +int _find_dense(const int n, int lo, float *d, int *cols, int *y) { + int hi = lo + 1; + float mind = d[cols[lo]]; + for (int k = hi; k < n; k++) { + int j = cols[k]; + if (d[j] <= mind) { + if (d[j] < mind) { + hi = lo; + mind = d[j]; + } + cols[k] = cols[hi]; + cols[hi++] = j; + } + } + return hi; +} + +// Scan all columns in TODO starting from arbitrary column in SCAN +// and try to decrease d of the TODO columns using the SCAN column. +int _scan_dense(const int n, float *cost[], int *plo, int *phi, float *d, + int *cols, int *pred, int *y, float *v) { + int lo = *plo; + int hi = *phi; + float h, cred_ij; + + while (lo != hi) { + int j = cols[lo++]; + const int i = y[j]; + const float mind = d[j]; + h = cost[i][j] - v[j] - mind; + // For all columns in TODO + for (int k = hi; k < n; k++) { + j = cols[k]; + cred_ij = cost[i][j] - v[j] - h; + if (cred_ij < d[j]) { + d[j] = cred_ij; + pred[j] = i; + if (cred_ij == mind) { + if (y[j] < 0) { + return j; + } + cols[k] = cols[hi]; + cols[hi++] = j; + } + } + } + } + *plo = lo; + *phi = hi; + return -1; +} + +/** Single iteration of modified Dijkstra shortest path algorithm as explained + * in the JV paper. + * + * This is a dense matrix version. + * + * \return The closest free column index. + */ +int find_path_dense(const int n, float *cost[], const int start_i, int *y, + float *v, int *pred) { + int lo = 0, hi = 0; + int final_j = -1; + int n_ready = 0; + int *cols; + float *d; + + NEW(cols, int, n); + NEW(d, float, n); + + for (int i = 0; i < n; i++) { + cols[i] = i; + pred[i] = start_i; + d[i] = cost[start_i][i] - v[i]; + } + while (final_j == -1) { + // No columns left on the SCAN list. + if (lo == hi) { + n_ready = lo; + hi = _find_dense(n, lo, d, cols, y); + for (int k = lo; k < hi; k++) { + const int j = cols[k]; + if (y[j] < 0) { + final_j = j; + } + } + } + if (final_j == -1) { + final_j = _scan_dense(n, cost, &lo, &hi, d, cols, pred, y, v); + } + } + + { + const float mind = d[cols[lo]]; + for (int k = 0; k < n_ready; k++) { + const int j = cols[k]; + v[j] += d[j] - mind; + } + } + + FREE(cols); + FREE(d); + + return final_j; +} + +/** Augment for a dense cost matrix. + */ +int _ca_dense(const int n, float *cost[], const int n_free_rows, int *free_rows, + int *x, int *y, float *v) { + int *pred; + + NEW(pred, int, n); + + for (int *pfree_i = free_rows; pfree_i < free_rows + n_free_rows; pfree_i++) { + int i = -1, j; + int k = 0; + + j = find_path_dense(n, cost, *pfree_i, y, v, pred); + while (i != *pfree_i) { + i = pred[j]; + y[j] = i; + SWAP_INDICES(j, x[i]); + k++; + } + } + FREE(pred); + return 0; +} + +/** Solve dense sparse LAP. + */ +int lapjv_internal(const cv::Mat &cost, const bool extend_cost, + const float cost_limit, int *x, int *y) { + int n_rows = cost.rows; + int n_cols = cost.cols; + int n; + if (n_rows == n_cols) { + n = n_rows; + } else if (!extend_cost) { + throw std::invalid_argument( + "Square cost array expected. If cost is intentionally non-square, pass " + "extend_cost=True."); + } + + // Get extend cost + if (extend_cost || cost_limit < LARGE) { + n = n_rows + n_cols; + } + cv::Mat cost_expand(n, n, CV_32F); + float expand_value; + if (cost_limit < LARGE) { + expand_value = cost_limit / 2; + } else { + double max_v; + minMaxLoc(cost, nullptr, &max_v); + expand_value = static_cast(max_v) + 1.; + } + + for (int i = 0; i < n; ++i) { + for (int j = 0; j < n; ++j) { + cost_expand.at(i, j) = expand_value; + if (i >= n_rows && j >= n_cols) { + cost_expand.at(i, j) = 0; + } else if (i < n_rows && j < n_cols) { + cost_expand.at(i, j) = cost.at(i, j); + } + } + } + + // Convert Mat to pointer array + float **cost_ptr; + NEW(cost_ptr, float *, n); + for (int i = 0; i < n; ++i) { + NEW(cost_ptr[i], float, n); + } + for (int i = 0; i < n; ++i) { + for (int j = 0; j < n; ++j) { + cost_ptr[i][j] = cost_expand.at(i, j); + } + } + + int ret; + int *free_rows; + float *v; + int *x_c; + int *y_c; + + NEW(free_rows, int, n); + NEW(v, float, n); + NEW(x_c, int, n); + NEW(y_c, int, n); + + ret = _ccrrt_dense(n, cost_ptr, free_rows, x_c, y_c, v); + int i = 0; + while (ret > 0 && i < 2) { + ret = _carr_dense(n, cost_ptr, ret, free_rows, x_c, y_c, v); + i++; + } + if (ret > 0) { + ret = _ca_dense(n, cost_ptr, ret, free_rows, x_c, y_c, v); + } + FREE(v); + FREE(free_rows); + for (int i = 0; i < n; ++i) { + FREE(cost_ptr[i]); + } + FREE(cost_ptr); + if (ret != 0) { + if (ret == -1) { + throw "Out of memory."; + } + throw "Unknown error (lapjv_internal)"; + } + // Get output of x, y, opt + for (int i = 0; i < n; ++i) { + if (i < n_rows) { + x[i] = x_c[i]; + if (x[i] >= n_cols) { + x[i] = -1; + } + } + if (i < n_cols) { + y[i] = y_c[i]; + if (y[i] >= n_rows) { + y[i] = -1; + } + } + } + + FREE(x_c); + FREE(y_c); + return ret; +} diff --git a/streamer/src/gstreamer/plugin/fdtracker/src/ocsort.cpp b/streamer/src/gstreamer/plugin/fdtracker/src/ocsort.cpp new file mode 100644 index 0000000000..0013c900ab --- /dev/null +++ b/streamer/src/gstreamer/plugin/fdtracker/src/ocsort.cpp @@ -0,0 +1,320 @@ +// Copyright (c) 2022 PaddlePaddle Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "gstreamer/plugin/fdtracker/include/kalmantracker.h" +#include "gstreamer/plugin/fdtracker/include/lapjv.h" +#include "gstreamer/plugin/fdtracker/include/ocsort.h" + +bool isnan_in_pred(cv::Mat &mat) { + for (int i = 0; i < mat.rows; i++) { + for (int j = 0; j < mat.cols; j++) { + if (isnan(*(mat.ptr(i, j)))) { + return true; + } + } + } + + return false; +} + +cv::Vec4f k_previous_obs(KalmanTracker *trajectory, int cur_age, int delta) { + if (trajectory->observations.size() == 0) { + return cv::Vec4f{-1, -1, -1, -1}; + } + for (int i = 0; i < delta; i++) { + int idx = delta - i; + if (trajectory->observations.find(idx) != trajectory->observations.end()) { + return trajectory->observations[idx]; + } + } + return trajectory->observations.rbegin()->second; +} + +OcSortTracker::OcSortTracker(int classid) + : timestamp(0), + max_age(30), + lambda(0.98f), + score_thresh(0.3f), + iou_threshold(0.3f), + delta_t(3), + classid(classid) { + std::cout << "Init Octracker: " << classid << std::endl; +} + +bool OcSortTracker::update(const cv::Mat &dets, bool use_byte = true, + bool use_angle_cost = false) { + ++timestamp; + + cv::Mat candidates_first, candidates_second; + for (int i = 0; i < dets.rows; ++i) { + float score = *dets.ptr(i, 1); + if (score > this->score_thresh) { + candidates_first.push_back(dets.row(i)); + } else if (score > 0.1) { + candidates_second.push_back(dets.row(i)); + } + } + + cv::Mat trks; + std::vector to_del; + for (int i = 0; i < this->trackers.size(); i++) { + // cv::Mat pos_pred; + cv::Mat pos_pred = xyah2ltrb(this->trackers[i]->predict()); + if (isnan_in_pred(pos_pred)) { + to_del.push_back(i); + } else { + trks.push_back(pos_pred); + } + } + + for (int i = to_del.size() - 1; i >= 0; i--) { + this->unused_trackers.push_back(this->trackers[i]); + this->trackers.erase(this->trackers.begin() + i); + } + + cv::Mat last_boxes; + for (auto tracker : this->trackers) { + last_boxes.push_back(cv::Mat(tracker->last_observation).t()); + } + + // First round of association + std::map matches; + std::vector mismatch_row, mismatch_col; + std::vector mismatch_row_temp, mismatch_col_temp; + if (use_angle_cost) { + cv::Mat velocities, k_observations; + for (auto tracker : this->trackers) { + if (!tracker->velocity[0] == -1) { + velocities.push_back(tracker->velocity); + } else { + velocities.push_back(cv::Vec2f{0, 0}); + } + + k_observations.push_back( + k_previous_obs(tracker, tracker->age, this->delta_t)); + } + associate(candidates_first, trks, this->iou_threshold, velocities, + k_observations, this->inertia, matches, mismatch_row, + mismatch_col); + } else { + associate_only_iou(candidates_first, trks, this->iou_threshold, matches, + mismatch_row, mismatch_col); + } + + for (auto item : matches) { + this->trackers[item.second]->update( + candidates_first.row(item.first).colRange(2, 6), use_angle_cost); + } + + // Second round of associaton + if (use_byte && candidates_second.rows > 0 && mismatch_col.size() > 0) { + cv::Mat u_trks; + for (auto idx : mismatch_col) { + u_trks.push_back(trks.row(idx)); + } + associate_only_iou(candidates_second, u_trks, this->iou_threshold, matches, + mismatch_row_temp, mismatch_col_temp); + for (auto pair : matches) { + this->trackers[mismatch_col[pair.second]]->update( + candidates_second.row(pair.first).colRange(2, 6), use_angle_cost); + } + std::vector mismatch_col_copy(mismatch_col); + mismatch_col.clear(); + for (auto mistrk : mismatch_col_temp) { + mismatch_col.push_back(mismatch_col_copy[mistrk]); + } + } + + if (mismatch_col.size() > 0 && mismatch_row.size() > 0) { + cv::Mat left_dets, left_trks; + for (auto det_idx : mismatch_row) { + left_dets.push_back(candidates_first.row(det_idx)); + } + for (auto trk_idx : mismatch_col) { + left_trks.push_back(last_boxes.row(trk_idx)); + } + associate_only_iou(left_dets, left_trks, this->iou_threshold, matches, + mismatch_row_temp, mismatch_col_temp); + for (auto pair : matches) { + this->trackers[mismatch_col[pair.second]]->update( + left_dets.row(pair.first).colRange(2, 6), use_angle_cost); + } + + std::vector mismatch_row_copy(mismatch_row); + mismatch_row.clear(); + for (auto mistrk : mismatch_row_temp) { + mismatch_row.push_back(mismatch_row_copy[mistrk]); + } + std::vector mismatch_col_copy(mismatch_col); + mismatch_col.clear(); + for (auto mistrk : mismatch_col_temp) { + mismatch_col.push_back(mismatch_col_copy[mistrk]); + } + } + + for (auto idx : mismatch_row) { + cv::Mat rect = candidates_first.row(idx); + float score = *rect.ptr(0, 1); + cv::Vec4f ltrb = mat2vec4f(rect.colRange(2, 6)); + printf( + "find new obj with score: %.2f,ltrb xmin: %.2f, ymin: %.2f,, xmax: " + "%.2f, ymax: %.2f,", + score, ltrb[0], ltrb[1], ltrb[2], ltrb[3]); + KalmanTracker *tracker = new KalmanTracker(ltrb, score); + this->trackers.push_back( + tracker); // todo, is the tracker memory will be released out this {} + } + + std::vector> tracker_res; + int tracker_num = this->trackers.size(); + for (int idx = tracker_num - 1; idx >= 0; idx--) { + if (this->trackers[idx]->time_since_update > this->max_age) { + this->unused_trackers.push_back(this->trackers[idx]); + this->trackers.erase(this->trackers.begin() + idx); + } + } + return true; +} + +cv::Mat OcSortTracker::get_trackers(void) { + // return [class, id, xmin, ymin, xmax, ymax] + cv::Mat tracker_res; + int tracker_num = this->trackers.size(); + for (int idx = tracker_num - 1; idx >= 0; idx--) { + cv::Mat rect(1, 6, CV_32FC1); + if (this->trackers[idx]->last_observation[0] < 0) { + rect(cv::Rect(2, 0, 4, 1)) = this->trackers[idx]->get_state(); + } else { + *rect.ptr(0, 2) = this->trackers[idx]->last_observation[0]; + *rect.ptr(0, 3) = this->trackers[idx]->last_observation[1]; + *rect.ptr(0, 4) = this->trackers[idx]->last_observation[2]; + *rect.ptr(0, 5) = this->trackers[idx]->last_observation[3]; + } + if (this->trackers[idx]->time_since_update < this->max_age && + (this->trackers[idx]->hit_streak >= this->min_hits)) { + *rect.ptr(0, 1) = this->trackers[idx]->id; + *rect.ptr(0, 0) = this->classid; + tracker_res.push_back(rect); + } + } + return tracker_res; +} + +void speed_direction_batch(const cv::Mat &detections, + const cv::Mat &previous_obs, cv::Mat &directx, + cv::Mat &directy) { + cv::Mat det_center = + (detections.colRange(1, 3) + detections.colRange(3, 5)) / 2.0; + cv::Mat pre_center = + (previous_obs.colRange(1, 3) + previous_obs.colRange(3, 5)) / 2.0; + for (int i = 0; i < previous_obs.rows; i++) { + cv::Mat dist = det_center - pre_center.rowRange(i, i + 1); + sqrt(dist.colRange(1, 2) * dist.colRange(1, 2) + + dist.colRange(2, 3) * dist.colRange(2, 3), + dist); + directx(cv::Rect(0, i, detections.cols, 1)) = + (det_center.colRange(1, 2) - + pre_center.rowRange(i, i + 1).colRange(1, 2)) / + dist; + directy(cv::Rect(0, i, detections.cols, 1)) = + (det_center.colRange(2, 3) - + pre_center.rowRange(i, i + 1).colRange(2, 3)) / + dist; + } +} + +void iou_batch(cv::Mat &bboxes1, cv::Mat &bboxes2, cv::Mat &iou_matrix) { + for (int row = 0; row < bboxes1.rows; row++) { + for (int col = 0; col < bboxes2.rows; col++) { + cv::Mat box1 = bboxes1(cv::Rect(2, row, 4, 1)); + cv::Mat box2 = bboxes2(cv::Rect(0, col, 4, 1)); + float inner_xmin = std::max(box1.at(0, 0), box2.at(0, 0)); + float inner_ymin = std::max(box1.at(0, 1), box2.at(0, 1)); + float inner_xmax = std::min(box1.at(0, 2), box2.at(0, 2)); + float inner_ymax = std::min(box1.at(0, 3), box2.at(0, 3)); + float inner_area = (inner_xmax - inner_xmin) * (inner_ymax - inner_ymin); + + float outer_xmin = std::min(box1.at(0, 0), box2.at(0, 0)); + float outer_ymin = std::min(box1.at(0, 1), box2.at(0, 1)); + float outer_xmax = std::max(box1.at(0, 2), box2.at(0, 2)); + float outer_ymax = std::max(box1.at(0, 3), box2.at(0, 3)); + float outer_area = (outer_xmax - outer_xmin) * (outer_ymax - outer_ymin); + + float iou = inner_area / outer_area; + iou_matrix.at(row, col) = iou; + } + } +} + +void OcSortTracker::associate_only_iou(cv::Mat detections, cv::Mat trackers, + float iou_threshold, + std::map &matches, + std::vector &mismatch_row, + std::vector &mismatch_col) { + matches.clear(); + mismatch_row.clear(); + mismatch_col.clear(); + if (detections.empty() || trackers.empty()) { + for (int i = 0; i < detections.rows; ++i) mismatch_row.push_back(i); + for (int i = 0; i < trackers.rows; ++i) mismatch_col.push_back(i); + return; + } + + cv::Mat iou_matrix(detections.rows, trackers.rows, CV_32FC1); + iou_batch(detections, trackers, iou_matrix); + + cv::Mat x(iou_matrix.rows, 1, CV_32S, cv::Scalar(-1)); + cv::Mat y(iou_matrix.cols, 1, CV_32S, cv::Scalar(-1)); + float maxValue = + *std::max_element(iou_matrix.begin(), iou_matrix.end()); + if (maxValue > iou_threshold) { + lapjv_internal(-iou_matrix, true, 1000.f, reinterpret_cast(x.data), + reinterpret_cast(y.data)); + } + + for (int i = 0; i < x.rows; ++i) { + int j = *x.ptr(i); + if (j >= 0 && iou_matrix.at(i, j) >= iou_threshold) + matches.insert({i, j}); + else + mismatch_row.push_back(i); + } + + for (int i = 0; i < y.rows; ++i) { + int j = *y.ptr(i); + if (j < 0 || iou_matrix.at(j, i) < iou_threshold) + mismatch_col.push_back(i); + } + return; +} + +void OcSortTracker::associate(cv::Mat detections, cv::Mat trackers, + float iou_threshold, cv::Mat velocities, + cv::Mat previous_obs, float vdc_weight, + std::map &matches, + std::vector &mismatch_row, + std::vector &mismatch_col) { + if (trackers.empty()) { + return; + } + cv::Mat directx(detections.rows, previous_obs.rows, CV_32SC1), + directy(detections.rows, previous_obs.rows, CV_32SC1); + speed_direction_batch(detections, previous_obs, directx, directy); +} diff --git a/streamer/src/gstreamer/plugin/fdtracker/src/trajectory.cpp b/streamer/src/gstreamer/plugin/fdtracker/src/trajectory.cpp new file mode 100644 index 0000000000..0fd763c5fe --- /dev/null +++ b/streamer/src/gstreamer/plugin/fdtracker/src/trajectory.cpp @@ -0,0 +1,152 @@ +// Copyright (c) 2022 PaddlePaddle Authors. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "gstreamer/plugin/fdtracker/include/trajectory.h" + +std::vector> Trajectory::entrance_count( + OcSortTracker* octracker) { + std::vector breakin, breakout; + std::vector> res; + for (auto tracker : octracker->trackers) { + if (tracker->observations.size() < 2) { + continue; + } + if (this->inout_type == horizontal) { + auto precenter = tracker->observations[tracker->age - 1][3]; + auto curcenter = tracker->observations[tracker->age][3]; + if (precenter <= this->region[1] && curcenter > this->region[1]) { + breakin.emplace_back(tracker->id); + } else if (precenter >= this->region[1] && curcenter < this->region[1]) { + breakout.emplace_back(tracker->id); + } + } else { + auto precenter = (tracker->observations[tracker->age - 1][0] + + tracker->observations[tracker->age - 1][2]) / + 2.0; + auto curcenter = (tracker->observations[tracker->age][0] + + tracker->observations[tracker->age][2]) / + 2.0; + if (precenter <= this->region[0] && curcenter > this->region[0]) { + breakin.emplace_back(tracker->id); + } else if (precenter >= this->region[0] && curcenter < this->region[0]) { + breakout.emplace_back(tracker->id); + } + } + } + res.emplace_back(breakin); + res.emplace_back(breakout); + countin += breakin.size(); + countout += breakout.size(); + return res; +} + +bool checkinarea(cv::Point point, std::vector> area, + int num_pts) { + if (area.size() == 0) { + return false; + } + cv::Point points[1][num_pts]; + int maxw = 0; + int maxh = 0; + for (int i = 0; i < num_pts; i++) { + if (area[i][0] > maxw) { + maxw = area[i][0]; + } + if (area[i][1] > maxh) { + maxh = area[i][1]; + } + points[0][i] = cv::Point(area[i][0], area[i][1]); + } + maxw = std::max(maxw, point.x); + maxh = std::max(maxh, point.y); + + cv::Mat img = cv::Mat::zeros(maxh + 1, maxw + 1, CV_32FC1); + // img.setTo(0); + + const cv::Point* ppt[] = {points[0]}; + int npt[] = {num_pts}; + cv::fillPoly(img, ppt, npt, 1, cv::Scalar(1.0)); + if (img.at(point) > 0.5) { + return true; + } else { + return false; + } +} + +std::vector Trajectory::breakin_count(OcSortTracker* octracker) { + std::vector res; + for (auto tracker : octracker->trackers) { + float locx = + (tracker->last_observation[0] + tracker->last_observation[1]) / 2.0; + float locy = tracker->last_observation[3]; + cv::Point2f loc(locx, locy); + if (checkinarea(loc, this->breakarea, this->num_pts)) { + res.push_back(tracker->id); + } + } + return res; +} + +bool Trajectory::set_region(region_type inout_type, std::vector region) { + if (region.size() != 2) { + printf("illegal region set, the region should be a vector of size=2(x,y)"); + return false; + } + this->inout_type = inout_type; + if (inout_type == horizontal) { + this->region = region; + } else { + this->region = region; + } + return true; +} + +bool Trajectory::set_area(std::vector area) { + if (area.size() < 6) { + printf( + "illegal area set, the area should include at least 3 points, so the " + "vector should have a size>=6"); + return false; + } + if (area.size() % 2 != 0) { + printf( + "illegal area set, the area vector should have even nunmbers, while " + "got number of %d", + static_cast(area.size())); + return false; + } + this->breakarea.clear(); + + this->num_pts = area.size() / 2; + for (int i = 0; i < this->num_pts; i++) { + std::vector temp; + temp.push_back(area[i * 2]); + temp.push_back(area[i * 2 + 1]); + this->breakarea.push_back(temp); + } + + return true; +} + +void Trajectory::clearset(void) { + this->countin = 0; + this->countout = 0; + this->breakin.clear(); + this->region.clear(); + this->breakarea.clear(); + this->inout_type = horizontal; +}