298 lines
12 KiB
C++
298 lines
12 KiB
C++
|
#include "ANSOCSortTrack.h"
|
||
|
|
#include "OCSortObject.h"
|
||
|
|
#include "boost/property_tree/ptree.hpp"
|
||
|
|
#include "boost/property_tree/json_parser.hpp"
|
||
|
|
#include "boost/foreach.hpp"
|
||
|
|
#include "boost/optional.hpp"
|
||
|
|
#include <map>
|
||
|
|
namespace ANSCENTER {
|
||
|
|
|
||
|
|
Eigen::MatrixXf GetOCSortInputs(const char* jsonString) {
|
||
|
|
Eigen::MatrixXf ret(0, 9); // 0 row and 9 columns
|
||
|
|
try {
|
||
|
|
boost::property_tree::ptree pt;
|
||
|
|
std::stringstream ss;
|
||
|
|
ss << jsonString;
|
||
|
|
boost::property_tree::read_json(ss, pt);
|
||
|
|
boost::property_tree::ptree detections = pt.get_child("results");
|
||
|
|
if (detections.size() > 0) {
|
||
|
|
for (const auto& detection : detections) {
|
||
|
|
float xMin = detection.second.get<float>("x");
|
||
|
|
float yMin = detection.second.get<float>("y");
|
||
|
|
float width = detection.second.get<float>("width");
|
||
|
|
float height = detection.second.get<float>("height");
|
||
|
|
float conf = detection.second.get<float>("prob");
|
||
|
|
int classId = detection.second.get<int>("class_id");
|
||
|
|
std::string object_id = detection.second.get<std::string>("object_id");
|
||
|
|
std::stringstream ss(object_id);
|
||
|
|
std::vector<std::string> result;
|
||
|
|
int i = 0;
|
||
|
|
while (ss.good())
|
||
|
|
{
|
||
|
|
std::string substr;
|
||
|
|
getline(ss, substr, ';');
|
||
|
|
result.push_back(substr);
|
||
|
|
i++;
|
||
|
|
if (i > 5)break;
|
||
|
|
}
|
||
|
|
// We now need to parse object_id into DGId,CameraId,and ModelId
|
||
|
|
double xMax = xMin + width;
|
||
|
|
double yMax = yMin + height;
|
||
|
|
int DGId = 0;
|
||
|
|
int CameraId = 0;
|
||
|
|
int ModelId = 0;
|
||
|
|
if (result.size() >= 4) {
|
||
|
|
DGId = stoi(result[1]);
|
||
|
|
CameraId = stoi(result[2]);
|
||
|
|
ModelId = stoi(result[3]);
|
||
|
|
}
|
||
|
|
Eigen::MatrixXf newRow(1, ret.cols());
|
||
|
|
newRow << xMin, yMin, xMax, yMax, conf, classId, DGId, CameraId, ModelId;
|
||
|
|
ret.conservativeResize(ret.rows() + 1, Eigen::NoChange);
|
||
|
|
ret.row(ret.rows() - 1) = newRow;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
catch (const std::exception& e) {
|
||
|
|
return ret;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
ANSOCSortTrack::ANSOCSortTrack()
|
||
|
|
{
|
||
|
|
_licenseValid = false;
|
||
|
|
CheckLicense();
|
||
|
|
|
||
|
|
float det_thresh = 0;
|
||
|
|
int max_age = 50;
|
||
|
|
int min_hits = 1;
|
||
|
|
float iou_threshold = 0.22136877277096445;
|
||
|
|
int delta_t = 1;
|
||
|
|
std::string asso_func = "iou";//giou//"iou"
|
||
|
|
float inertia = 0.3941737016672115;
|
||
|
|
bool use_byte = true;
|
||
|
|
|
||
|
|
|
||
|
|
tracker.update_parameters(det_thresh, max_age, min_hits, iou_threshold, delta_t, asso_func, inertia, use_byte);
|
||
|
|
}
|
||
|
|
|
||
|
|
ANSOCSortTrack::~ANSOCSortTrack() {
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
bool ANSOCSortTrack::Destroy() {
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
bool ANSOCSortTrack::UpdateParameters(const std::string& trackerParameters) {
|
||
|
|
//// Use JSON Boost to parse paramter from trackerParameters
|
||
|
|
try {
|
||
|
|
float det_thresh = 0;
|
||
|
|
int max_age = 50;
|
||
|
|
int min_hits = 1;
|
||
|
|
float iou_threshold = 0.22136877277096445;
|
||
|
|
int delta_t = 1;
|
||
|
|
int asso_func_in = 0;
|
||
|
|
std::string asso_func = "giou";
|
||
|
|
float inertia = 0.3941737016672115;
|
||
|
|
bool use_byte = true;
|
||
|
|
int use_byte_in = 0;
|
||
|
|
if (!trackerParameters.empty()) {
|
||
|
|
std::stringstream ss;
|
||
|
|
ss << trackerParameters;
|
||
|
|
boost::property_tree::ptree pt;
|
||
|
|
boost::property_tree::read_json(ss, pt);
|
||
|
|
auto rootNode = pt.get_child("parameters");
|
||
|
|
|
||
|
|
auto childNode = rootNode.get_child("det_thresh");
|
||
|
|
det_thresh = childNode.get_value<float>();
|
||
|
|
|
||
|
|
childNode = rootNode.get_child("max_age");
|
||
|
|
max_age = childNode.get_value<int>();
|
||
|
|
|
||
|
|
childNode = rootNode.get_child("min_hits");
|
||
|
|
min_hits = childNode.get_value<int>();
|
||
|
|
|
||
|
|
childNode = rootNode.get_child("iou_threshold");
|
||
|
|
iou_threshold = childNode.get_value<float>();
|
||
|
|
|
||
|
|
childNode = rootNode.get_child("delta_t");
|
||
|
|
delta_t = childNode.get_value<int>();
|
||
|
|
|
||
|
|
childNode = rootNode.get_child("asso_func");
|
||
|
|
asso_func_in = childNode.get_value<int>();
|
||
|
|
|
||
|
|
if (asso_func_in == 1)asso_func = "giou";
|
||
|
|
else asso_func = "iou";
|
||
|
|
|
||
|
|
childNode = rootNode.get_child("inertia");
|
||
|
|
inertia = childNode.get_value<float>();
|
||
|
|
|
||
|
|
childNode = rootNode.get_child("use_byte");
|
||
|
|
use_byte_in = childNode.get_value<int>();
|
||
|
|
|
||
|
|
if (use_byte_in == 1) use_byte = true;
|
||
|
|
else use_byte = false;
|
||
|
|
}
|
||
|
|
tracker.update_parameters(det_thresh, max_age, min_hits, iou_threshold, delta_t, asso_func, inertia, use_byte);
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
catch (const std::exception& e) {
|
||
|
|
this->_logger.LogFatal("ANSOCSortTrack::UpdateParameters", e.what(), __FILE__, __LINE__);
|
||
|
|
return false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
std::string ANSOCSortTrack::Update(int modelId, const std::string& detectionData) {
|
||
|
|
if (!_licenseValid) {
|
||
|
|
this->_logger.LogFatal("ANSOCSortTrack::Update", "Invalid license", __FILE__, __LINE__);
|
||
|
|
return "";
|
||
|
|
}
|
||
|
|
try {
|
||
|
|
boost::property_tree::ptree root;
|
||
|
|
boost::property_tree::ptree trackingObjects;
|
||
|
|
boost::property_tree::ptree pt;
|
||
|
|
std::vector<std::string> obj_ids;
|
||
|
|
Eigen::MatrixXf objects = GetOCSortInputs(detectionData.c_str());
|
||
|
|
//2. Do tracking
|
||
|
|
const auto outputs = tracker.update(objects);
|
||
|
|
if (outputs.size() > 0) {
|
||
|
|
for (int i = 0; i < outputs.size(); i++) {
|
||
|
|
double x_min = outputs[i][0];
|
||
|
|
double y_min = outputs[i][1];
|
||
|
|
double x_max = outputs[i][2];
|
||
|
|
double y_max = outputs[i][3];
|
||
|
|
int trackId = static_cast<int>(outputs[i][4]);
|
||
|
|
int classId = static_cast<int>(outputs[i][5]);
|
||
|
|
double prob = static_cast<double>(outputs[i][6]);
|
||
|
|
int DGId = static_cast<int>(outputs[i][7]);
|
||
|
|
int CameraId = static_cast<int>(outputs[i][8]);
|
||
|
|
int MId = static_cast<int>(outputs[i][9]);
|
||
|
|
std::string name = std::to_string(MId) + ":" + std::to_string(classId);
|
||
|
|
std::string object_id = name + ";" + std::to_string(DGId) + ";" + std::to_string(CameraId) + ";" + std::to_string(MId);
|
||
|
|
bool is_valid = true;
|
||
|
|
|
||
|
|
double width = x_max - x_min;
|
||
|
|
double heigh = y_max - y_min;
|
||
|
|
double left = x_min;
|
||
|
|
double top = y_min;
|
||
|
|
double right = x_max;
|
||
|
|
double bottom = y_max;
|
||
|
|
|
||
|
|
boost::property_tree::ptree trackingNode;
|
||
|
|
trackingNode.put("model_id", modelId);
|
||
|
|
trackingNode.put("track_id", trackId);
|
||
|
|
trackingNode.put("class_id", classId);
|
||
|
|
trackingNode.put("prob", prob);
|
||
|
|
trackingNode.put("x", x_min);
|
||
|
|
trackingNode.put("y", y_min);
|
||
|
|
trackingNode.put("width", width);
|
||
|
|
trackingNode.put("height", heigh);
|
||
|
|
trackingNode.put("left", left);
|
||
|
|
trackingNode.put("top", top);
|
||
|
|
trackingNode.put("right", right);
|
||
|
|
trackingNode.put("bottom", bottom);
|
||
|
|
trackingNode.put("valid", is_valid);
|
||
|
|
trackingNode.put("object_id", object_id);
|
||
|
|
trackingObjects.push_back(std::make_pair("", trackingNode));
|
||
|
|
}
|
||
|
|
}
|
||
|
|
//3. Convert result
|
||
|
|
root.add_child("results", trackingObjects);
|
||
|
|
std::ostringstream stream;
|
||
|
|
boost::property_tree::write_json(stream, root,false);
|
||
|
|
std::string st = stream.str();
|
||
|
|
return st;
|
||
|
|
|
||
|
|
}
|
||
|
|
catch (const std::exception& e) {
|
||
|
|
this->_logger.LogFatal("ANSOCSortTrack::Update", e.what(), __FILE__, __LINE__);
|
||
|
|
return "";
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
std::vector<TrackerObject> ANSOCSortTrack::UpdateTracker(int modelId, const std::vector<TrackerObject>& detectionObjects) {
|
||
|
|
if (!_licenseValid) {
|
||
|
|
this->_logger.LogFatal("ANSOCSortTrack::UpdateTracker", "Invalid license", __FILE__, __LINE__);
|
||
|
|
return std::vector<TrackerObject>();
|
||
|
|
}
|
||
|
|
try {
|
||
|
|
std::vector<TrackerObject> trackingResults;
|
||
|
|
Eigen::MatrixXf ret(0, 9); // 0 row and 9 columns
|
||
|
|
if (detectionObjects.size() > 0) {
|
||
|
|
for (const auto& detection : detectionObjects) {
|
||
|
|
float xMin = detection.x;
|
||
|
|
float yMin = detection.y;
|
||
|
|
float width = detection.width;
|
||
|
|
float height = detection.height;
|
||
|
|
float conf = detection.prob;
|
||
|
|
int classId = detection.class_id;
|
||
|
|
std::string object_id = detection.object_id;
|
||
|
|
|
||
|
|
// We now need to parse object_id into DGId,CameraId,and ModelId
|
||
|
|
double xMax = xMin + width;
|
||
|
|
double yMax = yMin + height;
|
||
|
|
int DGId = 0;
|
||
|
|
int CameraId = 0;
|
||
|
|
int ModelId = 0;
|
||
|
|
Eigen::MatrixXf newRow(1, ret.cols());
|
||
|
|
newRow << xMin, yMin, xMax, yMax, conf, classId, DGId, CameraId, ModelId;
|
||
|
|
ret.conservativeResize(ret.rows() + 1, Eigen::NoChange);
|
||
|
|
ret.row(ret.rows() - 1) = newRow;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
//2. Do tracking
|
||
|
|
const auto outputs = tracker.update(ret);
|
||
|
|
if (outputs.size() > 0) {
|
||
|
|
for (int i = 0; i < outputs.size(); i++) {
|
||
|
|
TrackerObject trackObj;
|
||
|
|
|
||
|
|
double x_min = outputs[i][0];
|
||
|
|
double y_min = outputs[i][1];
|
||
|
|
double x_max = outputs[i][2];
|
||
|
|
double y_max = outputs[i][3];
|
||
|
|
int trackId = static_cast<int>(outputs[i][4]);
|
||
|
|
int classId = static_cast<int>(outputs[i][5]);
|
||
|
|
double prob = static_cast<double>(outputs[i][6]);
|
||
|
|
int DGId = static_cast<int>(outputs[i][7]);
|
||
|
|
int CameraId = static_cast<int>(outputs[i][8]);
|
||
|
|
int MId = static_cast<int>(outputs[i][9]);
|
||
|
|
std::string name = std::to_string(MId) + ":" + std::to_string(classId);
|
||
|
|
std::string object_id = name + ";" + std::to_string(DGId) + ";" + std::to_string(CameraId) + ";" + std::to_string(MId);
|
||
|
|
bool is_valid = true;
|
||
|
|
|
||
|
|
double width = x_max - x_min;
|
||
|
|
double heigh = y_max - y_min;
|
||
|
|
double left = x_min;
|
||
|
|
double top = y_min;
|
||
|
|
double right = x_max;
|
||
|
|
double bottom = y_max;
|
||
|
|
|
||
|
|
trackObj.track_id = trackId;
|
||
|
|
trackObj.class_id = classId;
|
||
|
|
trackObj.prob = prob;
|
||
|
|
trackObj.x = x_min;
|
||
|
|
trackObj.y = y_min;
|
||
|
|
trackObj.width = width;
|
||
|
|
trackObj.height = heigh;
|
||
|
|
trackObj.left = left;
|
||
|
|
trackObj.top = top;
|
||
|
|
trackObj.right = right;
|
||
|
|
trackObj.bottom = bottom;
|
||
|
|
trackObj.object_id = object_id;
|
||
|
|
trackingResults.push_back(trackObj);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return trackingResults;
|
||
|
|
}
|
||
|
|
catch (const std::exception& e) {
|
||
|
|
this->_logger.LogFatal("ANSOCSortTrack::UpdateTracker", e.what(), __FILE__, __LINE__);
|
||
|
|
return std::vector<TrackerObject>();
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
}
|