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 obj_print_info(FILE *dst, void *obj)
{
if (!obj)
fprintf(dst,"<NULL object>\n");
else
fprintf(dst,"<%s at %p, id: %lu, refcount: %i, alive: %i>\n",
((object_t *)obj)->type->name, obj, obj_id(obj),
((object_t *)obj)->refcount/2, obj_alive(obj));
}
CORBA::ArrayDef_ptr
TAO_Repository_i::create_array_i (CORBA::ULong length,
CORBA::IDLType_ptr element_type)
{
u_int count = 0;
this->config_->get_integer_value (this->arrays_key_,
"count",
count);
char *name = TAO_IFR_Service_Utils::int_to_string (count++);
this->config_->set_integer_value (this->arrays_key_,
"count",
count);
// Make new database entry.
ACE_Configuration_Section_Key new_key;
this->config_->open_section (this->arrays_key_,
name,
1,
new_key);
// Set the length attribute.
this->config_->set_integer_value (new_key,
"length",
length);
// Set the def_kind attribute.
this->config_->set_integer_value (new_key,
"def_kind",
CORBA::dk_Array);
// Set the "name" for destroy to use.
this->config_->set_string_value (new_key,
"name",
name);
char *element_path =
TAO_IFR_Service_Utils::reference_to_path (element_type);
// To get key to element type.
this->config_->set_string_value (new_key,
"element_path",
element_path);
// Create the object reference.
ACE_TString obj_id ("arrays\\");
obj_id += name;
CORBA::Object_var obj =
TAO_IFR_Service_Utils::create_objref (CORBA::dk_Array,
obj_id.c_str (),
this->repo_);
return CORBA::ArrayDef::_narrow (obj.in ());
}
CORBA::PrimitiveDef_ptr
TAO_Repository_i::get_primitive (CORBA::PrimitiveKind kind)
{
ACE_TString obj_id ("pkinds\\");
obj_id += this->pkind_to_string (kind);
CORBA::Object_var obj =
TAO_IFR_Service_Utils::create_objref (CORBA::dk_Primitive,
obj_id.c_str (),
this->repo_);
return CORBA::PrimitiveDef::_narrow (obj.in ());
}
CORBA::WstringDef_ptr
TAO_Repository_i::create_wstring_i (CORBA::ULong bound)
{
u_int count = 0;
this->config_->get_integer_value (this->wstrings_key_,
"count",
count);
char *name = TAO_IFR_Service_Utils::int_to_string (count++);
this->config_->set_integer_value (this->wstrings_key_,
"count",
count);
// Make new database entry.
ACE_Configuration_Section_Key new_key;
this->config_->open_section (this->wstrings_key_,
name,
1,
new_key);
this->config_->set_integer_value (new_key,
"bound",
bound);
this->config_->set_integer_value (new_key,
"def_kind",
CORBA::dk_Wstring);
this->config_->set_string_value (new_key,
"name",
name);
// Create the object reference.
ACE_TString obj_id ("wstrings\\");
obj_id += name;
CORBA::Object_var obj =
TAO_IFR_Service_Utils::create_objref (CORBA::dk_Wstring,
obj_id.c_str (),
this->repo_);
return CORBA::WstringDef::_narrow (obj.in ());
}
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;
}
void obj_unlock_debug(void *obj, const char *file, int line)
{
if (obj)
{
fprintf(stderr,"%s:%i obj_unlock(%p <%s id: %lu>)\n",file,line,obj,obj_type(obj)->name,obj_id(obj));
obj_deref(obj);
}
}
myint SaveTheGame(const char *fn, const char *tag, myint dx, myint dy)
{
objtype *ob;
myint fd, i, x, y;
int32_t cs;
fd = OpenWrite(fn);
if (fd != -1) {
WriteBytes(fd, (byte *)GAMEHDR, 8);
WriteBytes(fd, (byte *)SAVTYPE, 4);
WriteInt32(fd, 0x00000000); /* write version */
WriteBytes(fd, (byte *)GAMETYPE, 4);
WriteInt32(fd, time(NULL));
WriteInt32(fd, 0x00000000);
WriteInt32(fd, 0x00000000); /* write checksum (placeholder) */
WriteBytes(fd, (byte *)tag, 32); /* write savegame name */
DiskFlopAnim(dx, dy);
WriteInt32(fd, gamestate_difficulty);
WriteInt32(fd, gamestate.mapon);
WriteInt32(fd, gamestate.oldscore);
WriteInt32(fd, gamestate.score);
WriteInt32(fd, gamestate.nextextra);
WriteInt32(fd, gamestate.lives);
WriteInt32(fd, gamestate.health);
WriteInt32(fd, gamestate.ammo);
WriteInt32(fd, gamestate.keys);
WriteInt32(fd, gamestate.bestweapon);
WriteInt32(fd, gamestate.weapon);
WriteInt32(fd, gamestate.chosenweapon);
WriteInt32(fd, gamestate.faceframe);
WriteInt32(fd, gamestate.attackframe);
WriteInt32(fd, gamestate.attackcount);
WriteInt32(fd, gamestate.weaponframe);
WriteInt32(fd, gamestate_episode);
WriteInt32(fd, gamestate.secretcount);
WriteInt32(fd, gamestate.treasurecount);
WriteInt32(fd, gamestate.killcount);
WriteInt32(fd, gamestate.secrettotal);
WriteInt32(fd, gamestate.treasuretotal);
WriteInt32(fd, gamestate.killtotal);
WriteInt32(fd, gamestate.TimeCount);
WriteInt32(fd, gamestate.killx);
WriteInt32(fd, gamestate.killy);
WriteInt8(fd, gamestate.victoryflag);
DiskFlopAnim(dx, dy);
#ifdef SPEAR
for (i = 0; i < 20; i++) {
#else
for (i = 0; i < 8; i++) {
#endif
WriteInt32(fd, LevelRatios[i].kill);
WriteInt32(fd, LevelRatios[i].secret);
WriteInt32(fd, LevelRatios[i].treasure);
WriteInt32(fd, LevelRatios[i].time);
}
DiskFlopAnim(dx, dy);
WriteBytes(fd, (byte *)tilemap, 64*64); /* MAPSIZE * MAPSIZE */
DiskFlopAnim(dx, dy);
for (x = 0; x < 64; x++)
for (y = 0; y < 64; y++)
WriteInt32(fd, actorat[x][y]);
DiskFlopAnim(dx, dy);
WriteBytes(fd, (byte *)&areabyplayer, 8);
for (ob = player; ob; ob = obj_next(ob)) {
DiskFlopAnim(dx, dy);
WriteInt32(fd, obj_id(ob));
WriteInt32(fd, ob->active);
WriteInt32(fd, ob->ticcount);
WriteInt32(fd, ob->obclass);
WriteInt32(fd, ob->state);
WriteInt8(fd, ob->flags);
WriteInt32(fd, ob->distance);
WriteInt32(fd, ob->dir);
WriteInt32(fd, ob->x);
WriteInt32(fd, ob->y);
WriteInt32(fd, ob->tilex);
WriteInt32(fd, ob->tiley);
WriteInt8(fd, ob->areanumber);
WriteInt32(fd, ob->viewx);
WriteInt32(fd, ob->viewheight);
WriteInt32(fd, ob->transx);
WriteInt32(fd, ob->transy);
WriteInt32(fd, ob->angle);
WriteInt32(fd, ob->hitpoints);
WriteInt32(fd, ob->speed);
WriteInt32(fd, ob->temp1);
WriteInt32(fd, ob->temp2);
//WriteInt32(fd, ob->temp3);
}
WriteInt32(fd, 0xFFFFFFFF); /* end of actor list */
DiskFlopAnim(dx, dy);
WriteInt32(fd, laststatobj - statobjlist); /* ptr offset */
for (i = 0; i < 400; i++) { /* MAXSTATS */
WriteInt8(fd, statobjlist[i].tilex);
WriteInt8(fd, statobjlist[i].tiley);
WriteInt32(fd, statobjlist[i].shapenum);
WriteInt8(fd, statobjlist[i].is_bonus);
WriteInt8(fd, statobjlist[i].itemnumber);
}
DiskFlopAnim(dx, dy);
for (i = 0; i < 64; i++) { /* MAXDOORS */
WriteInt32(fd, doorposition[i]);
}
DiskFlopAnim(dx, dy);
for (i = 0; i < 64; i++) { /* MAXDOORS */
WriteInt8(fd, doorobjlist[i].tilex);
WriteInt8(fd, doorobjlist[i].tiley);
WriteInt8(fd, doorobjlist[i].vertical);
WriteInt8(fd, doorobjlist[i].lock);
WriteInt8(fd, doorobjlist[i].action);
WriteInt32(fd, doorobjlist[i].ticcount);
}
DiskFlopAnim(dx, dy);
WriteInt32(fd, pwallstate);
WriteInt32(fd, pwallx);
WriteInt32(fd, pwally);
WriteInt32(fd, pwalldir);
WriteInt32(fd, pwallpos);
DiskFlopAnim(dx, dy);
CloseWrite(fd);
fd = OpenRead(fn);
ReadSeek(fd, 64, SEEK_SET);
cs = CalcFileChecksum(fd, ReadLength(fd) - 64);
CloseRead(fd);
fd = OpenWriteAppend(fn);
WriteSeek(fd, 28, SEEK_SET);
WriteInt32(fd, cs);
CloseWrite(fd);
} else {
