Hu Chunming / vpt_ascend_arm

/*
 * 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.h"
#include <cmath>
#include "../decoder/interface/DeviceMemory.hpp"
#include "../common/logger.hpp"
#include "../ai_platform/mvpt_process_assist.h"

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,
};

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_desc) {
          VPCUtil::vpc_pic_desc_release(value.roi_img_desc);
        }

        if (value.origin_img_desc) {
          VPCUtil::vpc_pic_desc_release(value.origin_img_desc);
        }
      }
      values.clear();
      iter = obj_to_alarm_boxes_.erase(iter);
    } else {
      ++iter;
    }
  }
  //-221024 byzsh删除存储的轨迹信息--------------------------------------------------
  for (auto iter = obj_to_traces_.begin(); iter != obj_to_traces_.end();) {
    const auto &key = iter->first;
    if (key.task_id == task_id) {
      auto &values = iter->second;
      values.clear();
      iter = obj_to_traces_.erase(iter);
    } else {
      ++iter;
    }
  }
  //-------------------------------------------------------------------------


}

vector<result_data_t> PedestrianVehicleRetrograde::get_results_by_id(const obj_key_t &id, bool do_erase) {
  LOG_DEBUG("obj_to_alarm_boxes_  size:{}", obj_to_alarm_boxes_.size());

  vector<result_data_t> res;

  auto it = obj_to_alarm_boxes_.find(id);
  if (it == obj_to_alarm_boxes_.end()) {
    return res;
  }
    
  res = 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;
}

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

  vector<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::vector<task_id_t> &tasks_id, vector<DeviceMemory*> vec_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;

        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(); 
        }
      }
    }
  }

  vector<obj_key_t> &&alarm_objs = retrograde_aux(); // 右值引用

  VPCUtil* pVpcUtil = VPCUtil::getInstance();

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

    if (traces.empty())
      continue;

    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 = vec_det_input_images[stream_idx];

    result_data_t result_data;
    result_data.box = trace;

    // 原图
    vpc_img_info src_img_info = VPCUtil::vpc_devMem2vpcImg(src_img);
    result_data.origin_img_desc = src_img_info.pic_desc;

    // 抠图
    int width = src_img->getWidth();
    int height = src_img->getHeight();
    video_object_info obj;
    strcpy(obj.task_id, obj_key.task_id.c_str());
    obj.object_id = obj_key.obj_id;
    obj.left = clip(trace.left, 0, width);
    obj.top = clip(trace.top, 0, height);
    obj.right = clip(trace.right, 0, width);
    obj.bottom = clip(trace.bottom, 0, height);

    vpc_img_info img_info = pVpcUtil->crop(src_img, obj);
    if(img_info.pic_desc != nullptr){
      result_data.roi_img_desc = img_info.pic_desc;
      // 保存结果
      obj_to_alarm_boxes_[obj_key].emplace_back(std::move(result_data));
    }
  }

  if(obj_to_alarm_boxes_.size() > 0) {
    LOG_DEBUG("obj_to_alarm_boxes_   size: {}", obj_to_alarm_boxes_.size());
  }

  return true;
}

} // namespace pedestrian_vehicle_retrograde
} // namespace ai_engine_module