void detections_callback(cv_tracker::image_obj_ranged image_objects_msg)
{
if(!detect_ready_)
{
unsigned int num = image_objects_msg.obj.size();
std::vector<cv_tracker::image_rect_ranged> objects = image_objects_msg.obj;
object_type = image_objects_msg.type;
image_objects_header = image_objects_msg.header;
//points are X,Y,W,H and repeat for each instance
_dpm_detections.clear();
_ranges.clear();
_min_heights.clear();
_max_heights.clear();
for (unsigned int i=0; i<num;i++)
{
cv::Rect tmp;
tmp.x = objects.at(i).rect.x;
tmp.y = objects.at(i).rect.y;
tmp.width = objects.at(i).rect.width;
tmp.height = objects.at(i).rect.height;
_dpm_detections.push_back(cv::LatentSvmDetector::ObjectDetection(tmp, 0));
_ranges.push_back(objects.at(i).range);
_min_heights.push_back(objects.at(i).min_height);
_max_heights.push_back(objects.at(i).max_height);
}
//_ready = true;
detect_ready_ = true;
}
//cout << "received pos" << endl;
publish_if_possible();
}
void detections_callback(cv_tracker_msgs::image_obj_ranged image_objects_msg)
{
//if(ready_)
// return;
if (!detect_ready_)//must NOT overwrite, data is probably being used by tracking.
{
unsigned int num = image_objects_msg.obj.size();
std::vector<cv_tracker_msgs::image_rect_ranged> objects = image_objects_msg.obj;
tracked_type_ = image_objects_msg.type;
//points are X,Y,W,H and repeat for each instance
obj_detections_.clear();
ranges_.clear();
for (unsigned int i=0; i<num;i++)
{
cv::Rect tmp;
tmp.x = objects.at(i).rect.x;
tmp.y = objects.at(i).rect.y;
tmp.width = objects.at(i).rect.width;
tmp.height = objects.at(i).rect.height;
ObjectDetection tmp_obj;
tmp_obj.rect=tmp; tmp_obj.score=0;
obj_detections_.push_back(tmp_obj);
ranges_.push_back(objects.at(i).range);
min_heights_.push_back(objects.at(i).min_height);
max_heights_.push_back(objects.at(i).max_height);
}
detect_ready_ = true;
}
publish_if_possible();
//ready_ = true;
}
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 << "START tracking...";
doTracking(detections, frameNumber, kstates, active, image, tracked_detections, colors);
tm.stop();
//std::cout << "END Tracking time = " << tm.getTimeSec() << " sec" << std::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;
kf_objects_msg.header = image_objects_header;
kf_objects_msg_ = kf_objects_msg;;
track_ready_ = true;
publish_if_possible();
//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;
ObjectDetection empty_detection;
empty_detection.rect=cv::Rect(0,0,0,0);
empty_detection.score=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;
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();
ranges_.clear();
cv_tracker_msgs::image_obj_tracked tmp_objects_msg;
tmp_objects_msg.type = tracked_type_;
tmp_objects_msg.total_num = num;
copy(rect_ranged_array.begin(), rect_ranged_array.end(), back_inserter(tmp_objects_msg.rect_ranged)); // copy vector
copy(real_data.begin(), real_data.end(), back_inserter(tmp_objects_msg.real_data)); // copy vector
copy(obj_id.begin(), obj_id.end(), back_inserter(tmp_objects_msg.obj_id)); // copy vector
copy(lifespan.begin(), lifespan.end(), back_inserter(tmp_objects_msg.lifespan)); // copy vector
tmp_objects_msg.header = image_source.header;
ros_objects_msg_ = tmp_objects_msg;
//publisher_tracked_objects_.publish(ros_objects_msg);
//cv::imshow("KLT",image_track);
//cv::waitKey(1);
track_ready_ = true;
//ready_ = false;
publish_if_possible();
}
