void Tracker2D::writeToVideo(cv::VideoWriter & writer)
{
cv::Mat dispImage;
drawTrackers(dispImage);
writer.write(dispImage);
}
void Tracker2D::displayCurrentStatus()
{
cv::Mat dispImg;
drawTrackers(dispImg);
cv::imshow("tracking status", dispImg);
cv::waitKey();
}
void Tracker2D::displayCurrentStatus(std::string & window_name)
{
cv::Mat dispImg;
drawTrackers(dispImg);
cv::imshow(window_name, dispImg);
//cv::waitKey();
}
void Tracker2D::saveResult(int seq_num)
{
cv::Mat dispImg;
drawTrackers(dispImg);
char name[100];
sprintf(name, "seq.%06d.png", seq_num);
cv::imwrite(name, dispImg);
}
void rgb_pcl::getTracker(std::vector<PointCloudPtr> object_clouds, Mat displayImage){
//Assign tracker to the clouds
for(unsigned int count1 = 0; count1 < trackerList.size(); count1++){
cv::Point3d position(0, 0, 0);
double maxScore = 0;
int id_cloud = -1;
cv::Point3d pos_tracker;
int size_tracker;
double hue_tracker;
for(unsigned int count2 = 0; count2 < object_clouds.size(); count2 ++){
int size = object_clouds[count2]->points.size();
cv::Point3d position(0, 0, 0);
double hue = 0;
getCloudFeatures(position, hue, object_clouds[count2]);
double score = trackerList[count1].isRecognized(position, 0, size);
if(score > maxScore){
maxScore = score;
id_cloud = count2;
pos_tracker = position;
size_tracker = size;
hue_tracker = hue;
}
}
if(id_cloud != -1){
trackerList[count1].updateTracker(pos_tracker, hue_tracker, size_tracker, object_clouds[id_cloud]);
object_clouds.erase(object_clouds.begin()+id_cloud);
}
}
// cout<<trackerList.size()<<endl;
//Create new tracker for the remaining clouds
for(auto cloudCluster: object_clouds){
int size = object_clouds[0]->points.size();
cv::Point3d position(0, 0, 0);
double hue = 0;
getCloudFeatures(position, hue, cloudCluster);
track_3d newTracker(position, hue, size);
trackerList.push_back(newTracker);
}
//Delete lost tracker and display tracked clouds
for(unsigned int count = 0; count < trackerList.size(); count ++){
trackerList[count].step();
if(!trackerList[count].isAlive()){
trackerList.erase(trackerList.begin()+count);
count --;
}else if(trackerList[count].isFound() && !trackerList[count].isGone()){
#if DISPLAY
drawTrackers(displayImage, trackerList[count], to_string(count));
#endif
}
}
}
