Skip to content

Commit

Permalink
update OD reID sample
Browse files Browse the repository at this point in the history
  • Loading branch information
@P-yver
P-yver committed Apr 2, 2021
1 parent 9356efb commit a8cf8d0
Show file tree
Hide file tree
Showing 3 changed files with 49 additions and 100 deletions.
11 changes: 5 additions & 6 deletions object detection/birds eye viewer/cpp/include/BatchSystemHandler.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,6 @@ class BatchSystemHandler {
BatchSystemHandler(int data_retention_time);
~BatchSystemHandler();


///
/// \brief clear the remaining data in queue (free memory).
/// Make sure it's called before zed is closed, otherwise you will have memory leaks.
Expand All @@ -45,7 +44,7 @@ class BatchSystemHandler {
/// \param point cloud as sl::Mat (on GPU memory) to be stored
/// \param batch_ : batch_ from ZED SDK batching system
///
void push(sl::Pose local_pose_, sl::Pose world_pose_, sl::Mat image_, sl::Mat pc_, std::vector<sl::ObjectsBatch> batch_);
void push(sl::Pose local_pose_, sl::Pose world_pose_, sl::Mat image_, sl::Mat pc_, std::vector<sl::ObjectsBatch> &batch_);


///
Expand All @@ -61,7 +60,7 @@ class BatchSystemHandler {
/// \brief push: push data in the FIFO system. Overloaded fct for objects data only
/// \param batch_ : batch_ from ZED SDK batching system
///
void push(std::vector<sl::ObjectsBatch> batch_);
void push(std::vector<sl::ObjectsBatch> &batch_);

///
/// \brief pop : pop the data from the FIFO system. Overloaded fct for objects data only
Expand All @@ -73,9 +72,9 @@ class BatchSystemHandler {
/// Ingest fcts
void ingestWorldPoseInMap(sl::Pose pose);
void ingestLocalPoseInMap(sl::Pose pose);
void ingestImageInMap(sl::Timestamp ts, sl::Mat image);
void ingestDepthInMap(sl::Timestamp ts, sl::Mat depth);
void ingestInObjectsQueue(std::vector<sl::ObjectsBatch> trajs);
void ingestImageInMap(sl::Timestamp ts, sl::Mat &image);
void ingestDepthInMap(sl::Timestamp ts, sl::Mat &depth);
void ingestInObjectsQueue(std::vector<sl::ObjectsBatch> &batch_);

/// Retrieve fct
sl::Pose findClosestWorldPoseFromTS(unsigned long long timestamp);
Expand Down
107 changes: 31 additions & 76 deletions object detection/birds eye viewer/cpp/src/BatchSystemHandler.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,50 +1,38 @@
#include <BatchSystemHandler.hpp>
#include "BatchSystemHandler.hpp"

BatchSystemHandler::BatchSystemHandler(int data_retention_time)
{
BatchSystemHandler::BatchSystemHandler(int data_retention_time) {
batch_data_retention = data_retention_time;
}




BatchSystemHandler::~BatchSystemHandler()
{
clear();
}


void BatchSystemHandler::clear()
{
void BatchSystemHandler::clear() {
objects_tracked_queue.clear();
camWorldPoseMap_ms.clear();
camLocalPoseMap_ms.clear();


while(!imageMap_ms.empty() )
{
while(!imageMap_ms.empty()) {
std::map<unsigned long long,sl::Mat>::iterator it = imageMap_ms.begin();
it->second.free();
imageMap_ms.erase(it);
}

while(!depthMap_ms.empty() )
{
while(!depthMap_ms.empty()) {
std::map<unsigned long long,sl::Mat>::iterator it = depthMap_ms.begin();
it->second.free(sl::MEM::GPU);
depthMap_ms.erase(it);
}
}



///
/// \brief push: push data in the FIFO system
/// \param pose_ : current pose of the camera
/// \param batch_ : batch_ from ZED SDK batching system
///
void BatchSystemHandler::push(sl::Pose local_pose_,sl::Pose world_pose_,sl::Mat image_, sl::Mat pc_, std::vector<sl::ObjectsBatch> batch_)
{
void BatchSystemHandler::push(sl::Pose local_pose_, sl::Pose world_pose_, sl::Mat image_, sl::Mat pc_, std::vector<sl::ObjectsBatch> &batch_) {
ingestWorldPoseInMap(world_pose_);
ingestLocalPoseInMap(local_pose_);
#if WITH_IMAGE_RETENTION
Expand All @@ -54,21 +42,17 @@ void BatchSystemHandler::push(sl::Pose local_pose_,sl::Pose world_pose_,sl::Mat
ingestInObjectsQueue(batch_);
}


///
/// \brief pop : pop the data from the FIFO system
/// \param pose_ : pose at the sl::Objects timestamp
/// \param objects_ : sl::Objects in the past.
///
void BatchSystemHandler::pop(sl::Pose& local_pose_,sl::Pose& world_pose_,sl::Mat& image_, sl::Mat& depth_,sl::Objects& objects_)
{
void BatchSystemHandler::pop(sl::Pose& local_pose_,sl::Pose& world_pose_,sl::Mat& image_, sl::Mat& depth_,sl::Objects& objects_) {
memset(&objects_,0,sizeof(sl::Objects));
memset(&local_pose_,0,sizeof(sl::Pose));
memset(&world_pose_,0,sizeof(sl::Pose));


if (objects_tracked_queue.size()>0)
{
if (objects_tracked_queue.size()>0) {
sl::Objects tracked_merged_obj = objects_tracked_queue.front();
if (init_queue_ts.data_ns==0ULL)
init_queue_ts = tracked_merged_obj.timestamp;
Expand All @@ -94,44 +78,36 @@ void BatchSystemHandler::pop(sl::Pose& local_pose_,sl::Pose& world_pose_,sl::Mat
objects_ = tracked_merged_obj;
objects_tracked_queue.pop_front();
}


}


///
/// \brief push: push data in the FIFO system. Overloaded fct for objects data only
/// \param batch_ : batch_ from ZED SDK batching system
///
void BatchSystemHandler::push(std::vector<sl::ObjectsBatch> batch_)
{
void BatchSystemHandler::push(std::vector<sl::ObjectsBatch> &batch_) {
ingestInObjectsQueue(batch_);
}

///
/// \brief pop : pop the data from the FIFO system. Overloaded fct for objects data only
/// \param objects_ : sl::Objects in the past.
///
void BatchSystemHandler::pop(sl::Objects& objects_)
{
void BatchSystemHandler::pop(sl::Objects& objects_) {
memset(&objects_,0,sizeof(sl::Objects));
if (objects_tracked_queue.size()>0)
{
if (objects_tracked_queue.size()) {
sl::Objects tracked_merged_obj = objects_tracked_queue.front();
objects_ = tracked_merged_obj;
objects_tracked_queue.pop_front();
}
}


///
/// \brief ingestPoseInMap
/// \param ts: timestamp of the pose
/// \param pose : sl::Pose of the camera
/// \param batch_duration_sc: duration in seconds in order to remove past elements.
///
void BatchSystemHandler::ingestWorldPoseInMap(sl::Pose pose)
{
void BatchSystemHandler::ingestWorldPoseInMap(sl::Pose pose) {
std::map<unsigned long long,sl::Pose>::iterator it = camWorldPoseMap_ms.begin();
sl::Timestamp ts = pose.timestamp;
if (init_app_ts.data_ns==0ULL)
Expand All @@ -147,8 +123,7 @@ void BatchSystemHandler::ingestWorldPoseInMap(sl::Pose pose)
camWorldPoseMap_ms[ts.getMilliseconds()]=pose;
}

void BatchSystemHandler::ingestLocalPoseInMap(sl::Pose pose)
{
void BatchSystemHandler::ingestLocalPoseInMap(sl::Pose pose) {
std::map<unsigned long long,sl::Pose>::iterator it = camLocalPoseMap_ms.begin();
sl::Timestamp ts = pose.timestamp;
if (init_app_ts.data_ns==0ULL)
Expand All @@ -164,8 +139,7 @@ void BatchSystemHandler::ingestLocalPoseInMap(sl::Pose pose)
camLocalPoseMap_ms[ts.getMilliseconds()]=pose;
}

void BatchSystemHandler::ingestImageInMap(sl::Timestamp ts, sl::Mat image)
{
void BatchSystemHandler::ingestImageInMap(sl::Timestamp ts, sl::Mat &image) {
imageMap_ms[ts.getMilliseconds()].clone(image);
for(auto it = imageMap_ms.begin(); it != imageMap_ms.end(); ) {
if(it->first<ts.getMilliseconds() - (unsigned long long)batch_data_retention*1000*2)
Expand All @@ -178,12 +152,10 @@ void BatchSystemHandler::ingestImageInMap(sl::Timestamp ts, sl::Mat image)
}
}

void BatchSystemHandler::ingestDepthInMap(sl::Timestamp ts, sl::Mat depth)
{
void BatchSystemHandler::ingestDepthInMap(sl::Timestamp ts, sl::Mat &depth) {
depthMap_ms[ts.getMilliseconds()].clone(depth);
for(auto it = depthMap_ms.begin(); it != depthMap_ms.end(); ) {
if(it->first<ts.getMilliseconds() - (unsigned long long)batch_data_retention*1000*2)
{
if(it->first<ts.getMilliseconds() - (unsigned long long)batch_data_retention*1000*2) {
it->second.free(sl::MEM::GPU);
it = depthMap_ms.erase(it);
}
Expand All @@ -192,14 +164,12 @@ void BatchSystemHandler::ingestDepthInMap(sl::Timestamp ts, sl::Mat depth)
}
}


///
/// \brief findClosestPoseFromTS : find the sl::Pose that matched the given timestamp
/// \param timestamp in milliseconds. ( at least in the same unit than camPoseMap_ms)
/// \return sl::Pose found.
///
sl::Pose BatchSystemHandler::findClosestWorldPoseFromTS(unsigned long long timestamp)
{
sl::Pose BatchSystemHandler::findClosestWorldPoseFromTS(unsigned long long timestamp) {
sl::Pose pose = sl::Pose();
unsigned long long ts_found = 0;
if (camWorldPoseMap_ms.find(timestamp)!=camWorldPoseMap_ms.end()) {
Expand All @@ -209,8 +179,7 @@ sl::Pose BatchSystemHandler::findClosestWorldPoseFromTS(unsigned long long times
return pose;
}

sl::Pose BatchSystemHandler::findClosestLocalPoseFromTS(unsigned long long timestamp)
{
sl::Pose BatchSystemHandler::findClosestLocalPoseFromTS(unsigned long long timestamp) {
sl::Pose pose = sl::Pose();
unsigned long long ts_found = 0;
if (camLocalPoseMap_ms.find(timestamp)!=camLocalPoseMap_ms.end()) {
Expand All @@ -220,33 +189,27 @@ sl::Pose BatchSystemHandler::findClosestLocalPoseFromTS(unsigned long long times
return pose;
}


///
/// \brief ingestInObjectsQueue : convert a list of batched objects from SDK retreiveObjectsBatch() to a sorted list of sl::Objects
/// \n Use this function to fill a std::deque<sl::Objects> that can be considered and used as a stream of objects with a delay.
/// \param trajs from retreiveObjectsBatch()
/// \param batch_ from retreiveObjectsBatch()
///
void BatchSystemHandler::ingestInObjectsQueue(std::vector<sl::ObjectsBatch> trajs)
{
void BatchSystemHandler::ingestInObjectsQueue(std::vector<sl::ObjectsBatch> &batch_) {
// If list is empty, do nothing.
if (trajs.empty())
if (batch_.empty())
return;

// add objects in map with timestamp as a key.
// This ensure
std::map<uint64_t,sl::Objects> list_of_newobjects;
for (int i=0;i<trajs.size();i++)
{
sl::ObjectsBatch current_traj = trajs.at(i);
for(auto &current_traj: batch_) {

// Impossible (!!) but still better to check...
if (current_traj.timestamps.size()!=current_traj.positions.size())
continue;


//For each sample, construct a objetdata and put it in the corresponding sl::Objects
for (int j=0;j<current_traj.timestamps.size();j++)
{
for (int j=0;j<current_traj.timestamps.size();j++) {
sl::Timestamp ts = current_traj.timestamps.at(j);
sl::ObjectData newObjectData;
newObjectData.id = current_traj.id;
Expand All @@ -256,11 +219,11 @@ void BatchSystemHandler::ingestInObjectsQueue(std::vector<sl::ObjectsBatch> traj
newObjectData.sublabel = current_traj.sublabel;

newObjectData.bounding_box_2d.clear();
for (int p = 0;p<current_traj.bounding_boxes_2d.at(j).size();p++)
for (int p = 0;p<current_traj.bounding_boxes_2d.at(j).size();p++)
newObjectData.bounding_box_2d.push_back(current_traj.bounding_boxes_2d.at(j).at(p));
newObjectData.bounding_box.clear();
for (int k=0;k<current_traj.bounding_boxes.at(j).size();k++)
newObjectData.bounding_box.push_back(current_traj.bounding_boxes.at(j).at(k));
for (int k=0;k<current_traj.bounding_boxes.at(j).size();k++)
newObjectData.bounding_box.push_back(current_traj.bounding_boxes.at(j).at(k));


if (list_of_newobjects.find(ts.getMilliseconds())!=list_of_newobjects.end())
Expand All @@ -272,24 +235,19 @@ void BatchSystemHandler::ingestInObjectsQueue(std::vector<sl::ObjectsBatch> traj
current_obj.is_new = true;
current_obj.is_tracked = true;
current_obj.object_list.push_back(newObjectData);
list_of_newobjects[ts.getMilliseconds()] = current_obj;
list_of_newobjects[ts.getMilliseconds()] = current_obj;
}
}
}


// Ingest in Queue of objects that will be empty by the main loop
// Since std::map is sorted by key, we are sure that timestamp are continous.
for (auto &elem : list_of_newobjects)
objects_tracked_queue.push_back(elem.second);

return;
}


sl::Mat BatchSystemHandler::findClosestImageFromTS(unsigned long long timestamp)
{
sl::Mat image = sl::Mat();
sl::Mat BatchSystemHandler::findClosestImageFromTS(unsigned long long timestamp) {
sl::Mat image;
unsigned long long ts_found = 0;
if (imageMap_ms.find(timestamp)!=imageMap_ms.end()) {
ts_found = timestamp;
Expand All @@ -298,15 +256,12 @@ sl::Mat BatchSystemHandler::findClosestImageFromTS(unsigned long long timestamp)
return image;
}


sl::Mat BatchSystemHandler::findClosestDepthFromTS(unsigned long long timestamp)
{
sl::Mat depth = sl::Mat();
sl::Mat BatchSystemHandler::findClosestDepthFromTS(unsigned long long timestamp) {
sl::Mat depth;
unsigned long long ts_found = 0;
if (depthMap_ms.find(timestamp)!=depthMap_ms.end()) {
ts_found = timestamp;
depth = depthMap_ms[timestamp];
}
return depth;
}

Loading

0 comments on commit a8cf8d0

Please sign in to comment.