|
|
#include "vp_ba_wrong_direction_node.h"
|
|
namespace vp_nodes
|
{
|
|
vp_ba_wrong_direction_node::vp_ba_wrong_direction_node(std::string node_name,
|
std::map<int, std::vector<vp_objects::vp_point>> left_lines,
|
std::map<int, std::vector<vp_objects::vp_point>> right_lines,
|
int half_width,
|
bool need_record_image,
|
bool need_record_video) : vp_node(node_name), all_left_lines(left_lines), all_right_lines(right_lines), half_width(half_width),need_record_image(need_record_image), need_record_video(need_record_video)
|
{
|
VP_INFO(vp_utils::string_format("[%s] %s", node_name.c_str(), to_string().c_str()));
|
this->initialized();
|
}
|
|
vp_ba_wrong_direction_node::~vp_ba_wrong_direction_node()
|
{
|
deinitialized();
|
}
|
|
std::string vp_ba_wrong_direction_node::to_string()
|
{
|
/*
|
* return vertexs of all jam regions
|
* [channel0: x1,y1 x2,y2 ...][channel1: x1,y1 x2,y2 ...]...
|
*/
|
std::stringstream ss;
|
for (auto &r : all_left_lines)
|
{
|
ss << "[" << r.first << ":";
|
for (auto &p : r.second)
|
{
|
ss << " " << p.x << "," << p.y;
|
}
|
ss << "]";
|
}
|
return ss.str();
|
}
|
|
bool vp_ba_wrong_direction_node::point_in_poly(vp_objects::vp_point p, std::vector<vp_objects::vp_point> region)
|
{
|
int i, j, c = 0;
|
int nvert = region.size();
|
|
for (i = 0, j = nvert - 1; i < nvert; j = i++)
|
{
|
if (((region[i].y > p.y) != (region[j].y > p.y)) &&
|
(p.x < (region[j].x - region[i].x) * (p.y - region[i].y) / (region[j].y - region[i].y) + region[i].x))
|
c = !c;
|
}
|
return c;
|
}
|
|
bool vp_ba_wrong_direction_node::check_wrong_direction(std::vector<vp_objects::vp_point> test_line, std::vector<vp_objects::vp_point> right_line, double angle_tolerance = 30.0)
|
{
|
if (test_line.size() != 2 || right_line.size() != 2)
|
{
|
// 线段必须由两个点组成
|
return false;
|
}
|
|
// 计算 test_line 的方向向量
|
double dx_test = test_line[1].x - test_line[0].x;
|
double dy_test = test_line[1].y - test_line[0].y;
|
|
// 计算 right_line 的方向向量
|
double dx_right = right_line[1].x - right_line[0].x;
|
double dy_right = right_line[1].y - right_line[0].y;
|
|
// 计算两个向量的模
|
double magnitude_test = std::sqrt(dx_test * dx_test + dy_test * dy_test);
|
double magnitude_right = std::sqrt(dx_right * dx_right + dy_right * dy_right);
|
|
if (magnitude_test == 0 || magnitude_right == 0)
|
{
|
// 如果某条线段的长度为零，则无法比较方向
|
return false;
|
}
|
|
// 归一化方向向量
|
dx_test /= magnitude_test;
|
dy_test /= magnitude_test;
|
|
dx_right /= magnitude_right;
|
dy_right /= magnitude_right;
|
|
// 计算两个向量的夹角余弦值
|
double dot_product = dx_test * dx_right + dy_test * dy_right;
|
|
// 将余弦值限制在 -1 到 1 的范围内（防止计算误差）
|
dot_product = std::max(-1.0, std::min(1.0, dot_product));
|
|
// 计算夹角（弧度制）
|
double angle_rad = std::acos(dot_product);
|
|
// 将夹角转换为角度制
|
double angle_deg = angle_rad * 180.0 / 3.1415926;
|
|
// 判断夹角是否在允许的范围内
|
return angle_deg > angle_tolerance;
|
}
|
|
std::shared_ptr<vp_objects::vp_meta> vp_ba_wrong_direction_node::handle_frame_meta(std::shared_ptr<vp_objects::vp_frame_meta> meta)
|
{
|
// if need applied on current channel or not
|
if (all_left_lines.count(meta->channel_index) == 0 || all_right_lines.count(meta->channel_index) == 0)
|
{
|
return meta;
|
}
|
|
// for current channel
|
// auto &jam_region = all_jam_regions[meta->channel_index];
|
auto &left_line = all_left_lines[meta->channel_index];
|
auto &right_line = all_right_lines[meta->channel_index];
|
|
auto &wrong_direction_checking_status = all_wrong_direction_checking_status[meta->channel_index];
|
auto &wrong_direction_result = all_wrong_direction_results[meta->channel_index];
|
auto &last_notify = all_last_notifys[meta->channel_index];
|
|
// for vp_frame_target only
|
// for vp_frame_target only
|
std::vector<int> hit_traget_ids;
|
for (auto &target : meta->targets)
|
{
|
if (target->primary_label != target->class_label_of_person)
|
{
|
auto len = target->tracks.size();
|
|
// target has been tracked AND tracked enough frames
|
if (len < check_interval_frames || target->track_id < 0)
|
{
|
continue;
|
}
|
// if target inside of stop region or not
|
// if (!point_in_poly(loc, jam_region))
|
//{
|
// continue;
|
//}
|
if (len > 2 && target->track_id >= 0)
|
{
|
// check the last 2 points in tracks
|
auto p1 = target->tracks[len - 1].track_point();
|
auto p2 = target->tracks[len - 2].track_point();
|
|
// 使用 auto 自动推导类型
|
auto test_line = {p2, p1};
|
|
auto is_wrong_direction = (p1.x < half_width) ? check_wrong_direction(test_line, left_line) : check_wrong_direction(test_line, right_line);
|
|
if (is_wrong_direction)
|
{
|
wrong_direction_checking_status[target->track_id]++;
|
hit_traget_ids.push_back(target->track_id);
|
}
|
}
|
}
|
}
|
std::vector<int> involve_targets;
|
for (auto i = wrong_direction_checking_status.begin(); i != wrong_direction_checking_status.end();)
|
{
|
if (std::find(hit_traget_ids.begin(), hit_traget_ids.end(), i->first) == hit_traget_ids.end())
|
{
|
// remove since it not satisfy stop condition
|
i = wrong_direction_checking_status.erase(i);
|
continue;
|
}
|
if (i->second >= check_min_hit_frames)
|
{
|
involve_targets.push_back(i->first);
|
}
|
i++;
|
}
|
if (involve_targets.size() > 0 && (meta->frame_index - last_notify) > (check_notify_interval * meta->fps))
|
{
|
wrong_direction_result = true;
|
last_notify = meta->frame_index;
|
// send record image and record video signal, recording actions would occur if record nodes exist in pipeline
|
std::string image_file_name_without_ext = ""; // empty means no recording image
|
std::string video_file_name_without_ext = ""; // empty means no recording video
|
|
// send image record control meta
|
if (need_record_image)
|
{
|
image_file_name_without_ext = vp_utils::time_format(NOW, "wrong_direction_image__<year><mon><day><hour><min><sec><mili>");
|
auto image_record_control_meta = std::make_shared<vp_objects::vp_image_record_control_meta>(meta->channel_index, image_file_name_without_ext, true);
|
pendding_meta(image_record_control_meta);
|
}
|
// send video record control meta
|
if (need_record_video)
|
{
|
video_file_name_without_ext = vp_utils::time_format(NOW, "wrong_direction_video__<year><mon><day><hour><min><sec><mili>");
|
auto video_record_control_meta = std::make_shared<vp_objects::vp_video_record_control_meta>(meta->channel_index, video_file_name_without_ext);
|
pendding_meta(video_record_control_meta);
|
}
|
|
// std::vector<vp_objects::vp_point> involve_region{line.start, line.end};
|
auto ba_result = std::make_shared<vp_objects::vp_ba_result>(vp_objects::vp_ba_type::WRONGDIRECTION,
|
meta->channel_index,
|
meta->frame_index,
|
involve_targets,
|
// involve_region,
|
left_line,
|
"wrong_direction", // meaningful label
|
image_file_name_without_ext,
|
video_file_name_without_ext);
|
// fill back to frame meta
|
meta->ba_results.push_back(ba_result);
|
// info log
|
VP_INFO(vp_utils::string_format("[%s] [channel %d] has found target wrong direction.", node_name.c_str(), meta->channel_index));
|
if (need_record_image || need_record_video)
|
{
|
VP_INFO(vp_utils::string_format("[%s] [channel %d] image & video record file names are: [%s & %s]", node_name.c_str(), meta->channel_index, image_file_name_without_ext.c_str(), video_file_name_without_ext.c_str()));
|
}
|
}
|
if (involve_targets.size() == 0 && wrong_direction_result)
|
{
|
wrong_direction_result = false;
|
last_notify = meta->frame_index;
|
}
|
return meta;
|
}
|
}
|
