void trackAndDrawObjects(cv::Mat& image, int frameNumber, std::vector<cv::LatentSvmDetector::ObjectDetection> detections,
std::vector<kstate>& kstates, std::vector<bool>& active,
std::vector<cv::Scalar> colors, const sensor_msgs::Image& image_source)
{
std::vector<kstate> tracked_detections;
cv::TickMeter tm;
tm.start();
//std::cout << endl << "START tracking...";
doTracking(detections, frameNumber, kstates, active, image, tracked_detections, colors);
tm.stop();
//std::cout << "END Tracking time = " << tm.getTimeSec() << " sec" << endl;
//ROS
int num = tracked_detections.size();
std::vector<cv_tracker::image_rect_ranged> rect_ranged_array;
std::vector<int> real_data(num,0);
std::vector<int> obj_id(num, 0);
std::vector<int> lifespan(num, 0);
//ENDROS
for (size_t i = 0; i < tracked_detections.size(); i++)
{
kstate od = tracked_detections[i];
cv_tracker::image_rect_ranged rect_ranged;
//od.rect contains x,y, width, height
rectangle(image, od.pos, od.color, 3);
putText(image, SSTR(od.id), cv::Point(od.pos.x + 4, od.pos.y + 13), cv::FONT_HERSHEY_SIMPLEX, 0.55, od.color, 2);
//ROS
obj_id[i] = od.id; // ?
rect_ranged.rect.x = od.pos.x;
rect_ranged.rect.y = od.pos.y;
rect_ranged.rect.width = od.pos.width;
rect_ranged.rect.height = od.pos.height;
rect_ranged.range = od.range;
rect_ranged.min_height = od.min_height;
rect_ranged.max_height = od.max_height;
rect_ranged_array.push_back(rect_ranged);
real_data[i] = od.real_data;
lifespan[i] = od.lifespan;
//ENDROS
}
//more ros
cv_tracker::image_obj_tracked kf_objects_msg;
kf_objects_msg.type = object_type;
kf_objects_msg.total_num = num;
copy(rect_ranged_array.begin(), rect_ranged_array.end(), back_inserter(kf_objects_msg.rect_ranged)); // copy vector
copy(real_data.begin(), real_data.end(), back_inserter(kf_objects_msg.real_data)); // copy vector
copy(obj_id.begin(), obj_id.end(), back_inserter(kf_objects_msg.obj_id)); // copy vector
copy(lifespan.begin(), lifespan.end(), back_inserter(kf_objects_msg.lifespan)); // copy vector
kf_objects_msg.header = image_source.header;
image_objects.publish(kf_objects_msg);
//cout << "."<< endl;
}
void image_callback(const sensor_msgs::Image& image_source)
{
cv_bridge::CvImagePtr cv_image = cv_bridge::toCvCopy(image_source, sensor_msgs::image_encodings::BGR8);
cv::Mat image_track = cv_image->image;
cv::LatentSvmDetector::ObjectDetection empty_detection(cv::Rect(0,0,0,0),0,0);
unsigned int i;
std::vector<bool> tracker_matched(obj_trackers_.size(), false);
std::vector<bool> object_matched(obj_detections_.size(), false);
//check object detections vs current trackers
for (i =0; i< obj_detections_.size(); i++)
{
for (unsigned int j = 0; j < obj_trackers_.size(); j++)
{
if (tracker_matched[j] || object_matched[i])
continue;
cv::LatentSvmDetector::ObjectDetection tmp_detection = obj_detections_[i];
int area = tmp_detection.rect.width * tmp_detection.rect.height;
cv::Rect intersection = tmp_detection.rect & obj_trackers_[j]->GetTrackedObject().rect;
if ( (intersection.width * intersection.height) > area*0.3 )
{
obj_trackers_[j]->Track(image_track, obj_detections_[i], true);
tracker_matched[j] = true;
object_matched[i] = true;
//std::cout << "matched " << i << " with " << j << std::endl;
}
}
}
//run the trackers not matched
for(i = 0; i < obj_trackers_.size(); i++)
{
if(!tracker_matched[i])
{
obj_trackers_[i]->Track(image_track, empty_detection, false);
}
}
//create trackers for those objects not being tracked yet
for(unsigned int i=0; i<obj_detections_.size(); i++)
{
if (!object_matched[i])//if object wasn't matched by overlapping area, create a new tracker
{
if (num_trackers_ >10)
num_trackers_=0;
LkTracker* new_tracker = new LkTracker(++num_trackers_, min_heights_[i], max_heights_[i], ranges_[i]);
new_tracker->Track(image_track, obj_detections_[i], true);
//std::cout << "added new tracker" << std::endl;
obj_trackers_.push_back(new_tracker);
}
}
ApplyNonMaximumSuppresion(obj_trackers_, 0.3);
//remove those trackers with its lifespan <=0
std::vector<LkTracker*>::iterator it;
for(it = obj_trackers_.begin(); it != obj_trackers_.end();)
{
if ( (*it)->GetRemainingLifespan()<=0 )
{
it = obj_trackers_.erase(it);
//std::cout << "deleted a tracker " << std::endl;
}
else
it++;
}
//copy results to ros msg
unsigned int num = obj_trackers_.size();
std::vector<cv_tracker_msgs::image_rect_ranged> rect_ranged_array;//tracked rectangles
std::vector<int> real_data(num,0);//boolean array to show if data in rect_ranged comes from tracking or detection
std::vector<unsigned int> obj_id(num, 0);//id number for each rect_range
std::vector<unsigned int> lifespan(num, 0);//remaining lifespan of each rectranged
for(i=0; i < num; i++)
{
cv_tracker_msgs::image_rect_ranged rect_ranged;
LkTracker tracker_tmp = *obj_trackers_[i];
rect_ranged.rect.x = tracker_tmp.GetTrackedObject().rect.x;
rect_ranged.rect.y = tracker_tmp.GetTrackedObject().rect.y;
rect_ranged.rect.width = tracker_tmp.GetTrackedObject().rect.width;
rect_ranged.rect.height = tracker_tmp.GetTrackedObject().rect.height;
rect_ranged.rect.score = tracker_tmp.GetTrackedObject().score;
rect_ranged.max_height = tracker_tmp.max_height_;
rect_ranged.min_height = tracker_tmp.min_height_;
rect_ranged.range = tracker_tmp.range_;
rect_ranged_array.push_back(rect_ranged);
lifespan[i] = tracker_tmp.GetRemainingLifespan();
obj_id[i] = tracker_tmp.object_id;
if(lifespan[i]==tracker_tmp.DEFAULT_LIFESPAN_)
real_data[i] = 1;
cv::rectangle(image_track, tracker_tmp.GetTrackedObject().rect, cv::Scalar(0,255,0), 2);
}
//std::cout << "TRACKERS: " << obj_trackers_.size() << std::endl;
obj_detections_.clear();
cv_tracker_msgs::image_obj_tracked ros_objects_msg;
ros_objects_msg.type = tracked_type_;
ros_objects_msg.total_num = num;
copy(rect_ranged_array.begin(), rect_ranged_array.end(), back_inserter(ros_objects_msg.rect_ranged)); // copy vector
copy(real_data.begin(), real_data.end(), back_inserter(ros_objects_msg.real_data)); // copy vector
copy(obj_id.begin(), obj_id.end(), back_inserter(ros_objects_msg.obj_id)); // copy vector
copy(lifespan.begin(), lifespan.end(), back_inserter(ros_objects_msg.lifespan)); // copy vector
ros_objects_msg.header = image_source.header;
publisher_tracked_objects_.publish(ros_objects_msg);
//cv::imshow("KLT",image_track);
//cv::waitKey(1);
ready_ = false;
}
