Hu Chunming / src

/*
 * File: pedestrian_vehicle_retrograde.cpp
 * Created Date: Tuesday February 22nd 2022
 * Author: yangzilong (yangzilong@objecteye.com)
 * Description:
 * -----
 * Last Modified: Tuesday, 22nd February 2022 4:38:48 pm
 * Modified By: yangzilong (yangzilong@objecteye.com>)
 * -----
 * Copyright 2022
 */

#include "./pedestrian_vehicle_retrograde.hpp"
#include "../reprocessing_module/CropImg.h"
#include <cmath>

namespace ai_engine_module {
namespace pedestrian_vehicle_retrograde {

#define MAX_TRACE_NUM 5
#define MINIMUM_DISTANCE 10

static std::set<algorithm_type_t> algor_type_list_ = {
    algorithm_type_t::PEDESTRIAN_RETROGRADE,
    algorithm_type_t::VEHICLE_RETROGRADE,
};

// ############################################################ //
// !                Auxiliary Function                        ! //
// ############################################################ //

std::set<det_class_label_t> algor_type_to_det_label_set(const algorithm_type_t &algor_type) {
  if (algorithm_type_t::PEDESTRIAN_RETROGRADE == algor_type) {
    return {det_class_label_t::HUMAN};
  } else if (algorithm_type_t::VEHICLE_RETROGRADE == algor_type) {
    return {
        det_class_label_t::LARGE_CAR, det_class_label_t::MEDIUM_BUS, det_class_label_t::SMALL_CAR,
        det_class_label_t::TRUCK,     det_class_label_t::TRACTOR,

        // det_class_label_t::BICYCLE,
        // det_class_label_t::MOTOCYCLE,
        // det_class_label_t::TRICYCLE,
    };
  } else {
    return {};
  }
}

bool is_valid_box(const int &cls, const algorithm_type_t &algor_type) {
  return algor_type_to_det_label_set(algor_type).count(static_cast<det_class_label_t>(cls));
}

bool is_valid_box(const box_t &box, const algorithm_type_t &algor_type,
                  const task_param_manager::algo_param_type_t_ *params_ptr = nullptr) {
  if (!params_ptr)
    return false;

  if (!snapshot_legal_inarea(params_ptr->basic_param->algor_valid_rect, box.left, box.top, box.right, box.bottom))
    return false;

  if (params_ptr->algor_param == nullptr)
    return false;

  if (box.width() == 0 || box.height() == 0)
    return false;

  using data_t = algor_config_param_retrograde_basic;
  data_t *algor_params_ptr = (data_t *)(params_ptr->algor_param);

  if (box.score < algor_params_ptr->conf_threshold || box.width() < algor_params_ptr->minmum_width ||
      box.height() < algor_params_ptr->minmum_height)
    return false;

  return is_valid_box(box.cls, algor_type);
}

const task_param_manager::algo_param_type_t_ *get_task_param_ptr(const task_id_t &task_id,
                                                                 const algorithm_type_t &algor_type,
                                                                 task_param_manager *const task_param_manager) {
  if (!task_param_manager)
    return nullptr;

  auto &&algor_param_wrap = task_param_manager->get_task_other_param(task_id, algor_type);
  if (!algor_param_wrap)
    LOG_ERROR("{} is nullptr when get algor param from task_param", task_id);
  return algor_param_wrap;
}

std::vector<algorithm_type_t> task_id_to_algorithm_type_seq(const task_id_t &task_id,
                                                            task_param_manager *const task_param) {
  std::vector<algorithm_type_t> seq;
  auto &&algor_map = task_param->get_task_other_param(task_id);
  for (auto iter = algor_map->begin(); iter != algor_map->end(); ++iter) {
    // LOG_TRACE("task id is {} algor type is {}", task_id, int(iter->first));
    if (algor_type_list_.count(iter->first) > 0)
      seq.emplace_back(iter->first);
  }
  return seq; // N(RVO)
}

// ############################################################ //
// !                    Class Member                          ! //
// ############################################################ //

PedestrianVehicleRetrograde::PedestrianVehicleRetrograde() : task_param_manager_(nullptr) {
  if (!task_param_manager_)
    task_param_manager_ = task_param_manager::getInstance();
}

PedestrianVehicleRetrograde::~PedestrianVehicleRetrograde() = default;

void PedestrianVehicleRetrograde::force_release_result(const task_id_t &task_id) {
  for (auto iter = obj_to_alarm_boxes_.begin(); iter != obj_to_alarm_boxes_.end();) {
    const auto &key = iter->first;
    if (key.task_id == task_id) {
      auto &values = iter->second;
      for (auto &value : values) {

        if (value.roi_img.data_) {
          LOG_TRACE("will free roi snap in gpu {}.", value.roi_img_is_in_gpu);
          if (value.roi_img_is_in_gpu) {
            CHECK(cudaFree(value.roi_img.data_));
          } else {
            delete[] value.roi_img.data_;
          }
          value.roi_img.data_ = nullptr;
        }

        if (value.ori_img.data_) {
          LOG_TRACE("will free ori snap in gpu {}.", value.ori_img_is_in_gpu);
          if (value.ori_img_is_in_gpu) {
            CHECK(cudaFree(value.ori_img.data_));
          } else {
            delete[] value.ori_img.data_;
          }
          value.ori_img.data_ = nullptr;
        }
      }
      values.clear();
      iter = obj_to_alarm_boxes_.erase(iter);
    } else {
      ++iter;
    }
  }
}

std::shared_ptr<results_data_t> PedestrianVehicleRetrograde::get_results_by_id(const obj_key_t &id, bool do_erase) {
  auto it = obj_to_alarm_boxes_.find(id);
  if (it == obj_to_alarm_boxes_.end()) {
    return nullptr;
  }
    
  std::shared_ptr<results_data_t> res = std::make_shared<results_data_t>(it->second);
  if (do_erase)
    obj_to_alarm_boxes_.erase(id);

  if (obj_to_traces_.find(id) != obj_to_traces_.end()) {
    obj_to_traces_.erase(id); //221009 !不删除会出现轨迹已消失但继续报警的情况，导致内存泄露
  }

  return res;
}

multi_obj_key_t PedestrianVehicleRetrograde::retrograde_aux() {
  /**
   * @brief 使用box的历史轨迹点和当前轨迹点结合超参中的线段进行逻辑判断出是否逆行.
   *
   */
  // LOG_TRACE("---> retrograde_aux called.");

  multi_obj_key_t ret;
  const unsigned minimum_boxes_num = MAX_TRACE_NUM - 1;
  for (auto &obj_to_trace : obj_to_traces_) {

    auto &obj_key = obj_to_trace.first;
    auto &task_id = obj_key.task_id;
    auto &algor_type = obj_key.algor_type;
    auto &&algor_type_seq = task_id_to_algorithm_type_seq(task_id, task_param_manager_);
    // LOG_DEBUG("55555555555 {} {} {}", obj_key.task_id, obj_key.obj_id, obj_key.algor_type);
    // LOG_TRACE("---> Ckpt-0");

    const task_param_manager::algo_param_type_t_ *param_ptr_wrap =
        get_task_param_ptr(task_id, algor_type, task_param_manager_);
    if (!param_ptr_wrap)
      continue;

    using param_type_t = algor_config_param_retrograde_basic;
    const param_type_t *param_ptr = (param_type_t *)(param_ptr_wrap->algor_param);

    // LOG_TRACE("---> Ckpt-1");

    auto &boxes = obj_to_trace.second;
    const auto box_size = boxes.size();

    if (box_size < minimum_boxes_num)
      continue;

    // LOG_TRACE("---> Ckpt-2");

    int point_x = boxes[box_size - minimum_boxes_num].cx();
    const int point_y = boxes[box_size - minimum_boxes_num].cy();

    double k = 0.0, b = 0.0;
    double dist_c = 0.0, dist_p = 0.0; // current point and prev(history) point.

    //! 使用超参中的点 连成的线水平还是垂直.
    const bool is_horizontal_line = std::abs(int(param_ptr->px1 - param_ptr->px2)) >= 1;
    if (is_horizontal_line) {
      k = double(param_ptr->py1 - param_ptr->py2) / (param_ptr->px1 - param_ptr->px2);
      b = double(param_ptr->px1 * param_ptr->py2 - param_ptr->px2 * param_ptr->py1) / (param_ptr->px1 - param_ptr->px2);

      dist_c = fabs(k * boxes[box_size - 1].cx() - boxes[box_size - 1].cy() + b) / sqrt(k * k + 1.0);
      dist_p = fabs(k * point_x - point_y + b) / sqrt(k * k + 1.0);
    } else {
      k = 0.0;
      b = double(param_ptr->px1);

      dist_c = fabs(boxes[box_size - 1].cx() - b);
      dist_p = fabs(point_x - b);
    }

    // 从历史轨迹点中找到当前点距离超过阈值的最小点.
    bool flag = true;
    if (abs(dist_c - dist_p) < MINIMUM_DISTANCE) {
      flag = false;
      int prev_idx = box_size - MAX_TRACE_NUM;
      while (prev_idx >= 0) {
        point_x = boxes[prev_idx].cx();
        dist_p = is_horizontal_line ? fabs(k * point_x - point_y + b) / sqrt(k * k + 1.0) : fabs(point_x - b);
        if (abs(dist_c - dist_p) >= MINIMUM_DISTANCE) {
          flag = true;
          break;
        }
        --prev_idx;
      }
    }

    // LOG_TRACE("---> Ckpt-10");

    if (!flag)
      continue;

    // 当前轨迹点和历史轨迹点是否越线
    bool flag_c = is_horizontal_line ? (k * boxes[box_size - 1].cx() + b >= boxes[box_size - 1].cy())
                                     : (boxes[box_size - 1].cx() >= b);
    bool flag_p = is_horizontal_line ? (k * point_x + b >= point_y) : (point_x >= b);

    /**
     * PS. 拌线: 使用超参中的两点模拟的线段.
     * 1. POSITIVE && is_horizontal_line:  方向点在拌线上方，正常方向为从下到上(↑)，越界方向为从上到下(↓)
     * 2. NEGATIVE && is_horizontal_line:  方向点在拌线下方，正常方向为从上到下(↓)，越界方向为从下到上(↑)
     * 3. POSITIVE && !is_horizontal_line: 方向点在拌线右边，正常方向为从左到右(→)，越界方向为从右到左(←)
     * 4. NEGATIVE && !is_horizontal_line: 方向点在拌线左边，正常方向为从右到左(←)，越界方向为从左到右(→)
     */
    bool is_alarmed = false;
    if (direction_t::POSITIVE == static_cast<direction_t>(param_ptr->direction)) {
      if (flag_c * flag_p == 1) {
        if (dist_c <= dist_p) {
          is_alarmed = true;
        }
      } else if (flag_c == 0 && flag_p == 1) {
        is_alarmed = true;
      } else if (flag_c == 0 && flag_p == 0) {
        if (dist_p <= dist_c) {
          is_alarmed = true;
        }
      }
    } else {
      if (flag_c == 0 && flag_p == 0) {
        if (dist_c <= dist_p) {
          is_alarmed = true;
        }
      } else if (flag_c == 1 && flag_p == 0) {
        is_alarmed = true;
      } else if (flag_c == 1 && flag_p == 1) {
        if (dist_p <= dist_c) {
          is_alarmed = true;
        }
      }
    }

    if (is_alarmed) {
      ret.emplace_back(obj_key);
    }
  }
  return ret;
}

bool PedestrianVehicleRetrograde::update_mstreams(const std::set<task_id_t> &tasks_id, const sy_img *det_input_images,
                                                  const std::vector<onelevel_det_result> &det_results) {
  //! check.
  if (tasks_id.empty() || tasks_id.size() != det_results.size())
    return false;

  //! loop.
  unsigned stream_idx = 0;
  std::map<task_id_t, unsigned> task_id_to_stream_idx;
  for (auto task_id_iter = tasks_id.begin(); task_id_iter != tasks_id.end(); ++task_id_iter, ++stream_idx) {
    auto task_id = *task_id_iter;
    task_id_to_stream_idx[task_id] = stream_idx;
    auto &&algor_type_seq = task_id_to_algorithm_type_seq(task_id, task_param_manager_);
    if (algor_type_seq.empty())
      continue;

    auto &det_result = det_results[stream_idx];
    for (unsigned box_idx = 0; box_idx < det_result.obj_count; ++box_idx) {
      auto &box = det_result.obj[box_idx];
      // LOG_DEBUG("333333333333 {} {}", task_id, box.id);
      box_t unique_box;
      {
        unique_box.id = box.id;
        unique_box.cls = box.index;

        unique_box.top = box.top;
        unique_box.left = box.left;
        unique_box.right = box.right;
        unique_box.bottom = box.bottom;
        unique_box.score = box.confidence;
      }

      //! loop algor
      for (auto algor_type : algor_type_seq) {
        obj_key_t obj_key{box.id, task_id, algor_type};
        if (!is_valid_box(unique_box, algor_type, get_task_param_ptr(task_id, algor_type, task_param_manager_)))
          continue;

        //! add or update.
        // LOG_DEBUG("444444444444 {} {}", task_id, box.id);
        if (obj_to_traces_.find(obj_key) == obj_to_traces_.end()) {
          obj_to_traces_[obj_key].emplace_back(std::move(unique_box));
        } else {
          auto &traces = obj_to_traces_[obj_key];
          traces.emplace_back(std::move(unique_box));
          if (traces.size() > MAX_TRACE_NUM)
            traces.pop_front(); 
        }
      }
    }
  }

  multi_obj_key_t &&alarm_objs = retrograde_aux(); // 右值引用

  //!
  {
    for (const auto &obj_key : alarm_objs) {
      auto &traces = obj_to_traces_[obj_key];

      if (traces.empty())
        continue;

      // 221009 byzsh记录10条即可--------------------------------------------------------------------------------------
      if (obj_to_alarm_boxes_.find(obj_key) != obj_to_alarm_boxes_.end() && obj_to_alarm_boxes_[obj_key].size() >= 10)
        continue;
      //--------------------------------------------------------------------------------------------------------------
      
      auto &trace = traces.at(0);
      auto &&it = task_id_to_stream_idx.find(obj_key.task_id);
      if (it == task_id_to_stream_idx.end())
        continue;

      const unsigned stream_idx = it->second;
      auto &src_img = det_input_images[stream_idx];
 
      result_data_t result_data;
      { result_data.box = trace; }

#ifdef _USE_SHALLOW_COPY
      {
        result_data.ori_img = src_img;
        // result_data.roi_img = flattened_imgs[n];
      }
#else
      {
        {
          sy_img img;
          {
            img.c_ = src_img.c_;
            img.h_ = src_img.h_;
            img.w_ = src_img.w_;
          }

          const unsigned size = img.c_ * img.h_ * img.w_;
          img.data_ = new unsigned char[size];
          // img.data_ = new std::remove_pointer<decltype(img.data_)>[size];
          CHECK(cudaMemcpy(img.data_, src_img.data_, size * sizeof(unsigned char), cudaMemcpyDeviceToHost));
          result_data.ori_img = std::move(img);
          result_data.ori_img_is_in_gpu = false;
#if 0
                            const unsigned nbytes = img.c_ * img.h_ * img.w_ * sizeof(unsigned char);
                            CHECK(cudaMalloc(&img.data_, nbytes));
                            CHECK(cudaMemcpy(img.data_, src_img.data_, nbytes, cudaMemcpyDeviceToDevice));
                            result_data.ori_img = std::move(img);
                            result_data.ori_img_is_in_gpu = true;
#endif
        }

        {
          const int top = clip(trace.top, 0, src_img.h_);
          const int left = clip(trace.left, 0, src_img.w_);
          const int right = clip(trace.right, 0, src_img.w_);
          const int bottom = clip(trace.bottom, 0, src_img.h_);

          sy_img img;
          {
            img.c_ = src_img.c_;
            img.h_ = bottom - top;
            img.w_ = right - left;
          }
          const unsigned nbytes = img.c_ * img.h_ * img.w_ * sizeof(unsigned char);
          CHECK(cudaMalloc(&img.data_, nbytes));
          CHECK(cudacommon::CropImgGpu(src_img.data_, src_img.w_, src_img.h_, img.data_, left, top, img.w_, img.h_));
          result_data.roi_img = std::move(img); // deep copy.
          result_data.roi_img_is_in_gpu = true;
        }
      }
#endif
      // LOG_DEBUG("2222222222222 {} {} {}", obj_key.task_id, obj_key.obj_id, obj_key.algor_type);
      obj_to_alarm_boxes_[obj_key].emplace_back(std::move(result_data));  
    }
  }

  return true;
}

bool PedestrianVehicleRetrograde::update_mstreams2(const std::vector<task_id_t> &tasks_id, const sy_img *det_input_images,
                                                  const std::vector<onelevel_det_result> &det_results) {
  //! check.
  if (tasks_id.empty() || tasks_id.size() != det_results.size())
    return false;

  //! loop.
  unsigned stream_idx = 0;
  std::map<task_id_t, unsigned> task_id_to_stream_idx;
  for (auto task_id_iter = tasks_id.begin(); task_id_iter != tasks_id.end(); ++task_id_iter, ++stream_idx) {
    auto task_id = *task_id_iter;
    task_id_to_stream_idx[task_id] = stream_idx;
    auto &&algor_type_seq = task_id_to_algorithm_type_seq(task_id, task_param_manager_);
    if (algor_type_seq.empty())
      continue;

    auto &det_result = det_results[stream_idx];
    for (unsigned box_idx = 0; box_idx < det_result.obj_count; ++box_idx) {
      auto &box = det_result.obj[box_idx];
      // LOG_DEBUG("333333333333 {} {}", task_id, box.id);
      box_t unique_box;
      {
        unique_box.id = box.id;
        unique_box.cls = box.index;

        unique_box.top = box.top;
        unique_box.left = box.left;
        unique_box.right = box.right;
        unique_box.bottom = box.bottom;
        unique_box.score = box.confidence;
      }

      //! loop algor
      for (auto algor_type : algor_type_seq) {
        obj_key_t obj_key{box.id, task_id, algor_type};
        if (!is_valid_box(unique_box, algor_type, get_task_param_ptr(task_id, algor_type, task_param_manager_)))
          continue;

        //! add or update.
        // LOG_DEBUG("444444444444 {} {}", task_id, box.id);
        if (obj_to_traces_.find(obj_key) == obj_to_traces_.end()) {
          obj_to_traces_[obj_key].emplace_back(std::move(unique_box));
        } else {
          auto &traces = obj_to_traces_[obj_key];
          traces.emplace_back(std::move(unique_box));
          if (traces.size() > MAX_TRACE_NUM)
            traces.pop_front(); 
        }
      }
    }
  }

  multi_obj_key_t &&alarm_objs = retrograde_aux(); // 右值引用

  //!
  {
    for (const auto &obj_key : alarm_objs) {
      auto &traces = obj_to_traces_[obj_key];

      if (traces.empty())
        continue;

      // 221009 byzsh记录10条即可--------------------------------------------------------------------------------------
      if (obj_to_alarm_boxes_.find(obj_key) != obj_to_alarm_boxes_.end() && obj_to_alarm_boxes_[obj_key].size() >= 10)
        continue;
      //--------------------------------------------------------------------------------------------------------------
      
      auto &trace = traces.at(0);
      auto &&it = task_id_to_stream_idx.find(obj_key.task_id);
      if (it == task_id_to_stream_idx.end())
        continue;

      const unsigned stream_idx = it->second;
      auto &src_img = det_input_images[stream_idx];
 
      result_data_t result_data;
      { result_data.box = trace; }

#ifdef _USE_SHALLOW_COPY
      {
        result_data.ori_img = src_img;
        // result_data.roi_img = flattened_imgs[n];
      }
#else
      {
        {
          sy_img img;
          {
            img.c_ = src_img.c_;
            img.h_ = src_img.h_;
            img.w_ = src_img.w_;
          }

          const unsigned size = img.c_ * img.h_ * img.w_;
          img.data_ = new unsigned char[size];
          // img.data_ = new std::remove_pointer<decltype(img.data_)>[size];
          CHECK(cudaMemcpy(img.data_, src_img.data_, size * sizeof(unsigned char), cudaMemcpyDeviceToHost));
          result_data.ori_img = std::move(img);
          result_data.ori_img_is_in_gpu = false;
#if 0
                            const unsigned nbytes = img.c_ * img.h_ * img.w_ * sizeof(unsigned char);
                            CHECK(cudaMalloc(&img.data_, nbytes));
                            CHECK(cudaMemcpy(img.data_, src_img.data_, nbytes, cudaMemcpyDeviceToDevice));
                            result_data.ori_img = std::move(img);
                            result_data.ori_img_is_in_gpu = true;
#endif
        }

        {
          const int top = clip(trace.top, 0, src_img.h_);
          const int left = clip(trace.left, 0, src_img.w_);
          const int right = clip(trace.right, 0, src_img.w_);
          const int bottom = clip(trace.bottom, 0, src_img.h_);

          sy_img img;
          {
            img.c_ = src_img.c_;
            img.h_ = bottom - top;
            img.w_ = right - left;
          }
          const unsigned nbytes = img.c_ * img.h_ * img.w_ * sizeof(unsigned char);
          CHECK(cudaMalloc(&img.data_, nbytes));
          CHECK(cudacommon::CropImgGpu(src_img.data_, src_img.w_, src_img.h_, img.data_, left, top, img.w_, img.h_));
          result_data.roi_img = std::move(img); // deep copy.
          result_data.roi_img_is_in_gpu = true;
        }
      }
#endif
      // LOG_DEBUG("2222222222222 {} {} {}", obj_key.task_id, obj_key.obj_id, obj_key.algor_type);
      obj_to_alarm_boxes_[obj_key].emplace_back(std::move(result_data));  
    }
  }

  return true;
}

} // namespace pedestrian_vehicle_retrograde
} // namespace ai_engine_module