/////////////////////////////////////////////////////////
// processGrayImage
//
/////////////////////////////////////////////////////////
void pix_fiducialtrack :: processGrayImage(imageStruct &image)
{
if(image.xsize!=m_width || image.ysize!=m_height)
deinit_segmenter();
m_width =image.xsize;
m_height=image.ysize;
if(!initialized){
initialize_segmenter( &segmenter, m_width, m_height, treeidmap.max_adjacencies );
initialized=true;
}
step_segmenter( &segmenter, image.data);
int count = find_fiducialsX( fiducials, MAX_FIDUCIAL_COUNT,
&fidtrackerx ,
&segmenter,
m_width, m_height);
int i;
for(i=0;i< count;i++) {
if(fiducials[i].id!=INVALID_FIDUCIAL_ID){
SETFLOAT((m_outlist+0), (fiducials[i].id)); // id (as in treeidmap)
SETFLOAT((m_outlist+1), (fiducials[i].x/m_width)); // x (normalized)
SETFLOAT((m_outlist+2), (fiducials[i].y/m_height)); // y (normalized)
SETFLOAT((m_outlist+3), (fiducials[i].angle)); // phi (radiant)
outlet_list(m_infoOut, gensym("list"), 4, m_outlist);
}
}
}
void FidtrackFinder::process(unsigned char *src, unsigned char *dest, SDL_Surface *display) {
/*
#ifdef WIN32
long start_time = GetTickCount();
#else
struct timeval tv;
struct timezone tz;
gettimeofday(&tv,&tz);
long start_time = (tv.tv_sec*1000000)+(tv.tv_usec);
#endif
*/
// segmentation
step_segmenter( &segmenter, dest );
// fiducial recognition
int fid_count = find_fiducialsX( fiducials, MAX_FIDUCIAL_COUNT, &fidtrackerx, &segmenter, width, height);
float total_leaf_size = 0.0f;
float total_fiducial_size = 0.0f;
int valid_fiducial_count = 0;
// process found symbols
for(int i=0;i< fid_count;i++) {
if ( fiducials[i].id >=0 ) {
valid_fiducial_count ++;
total_leaf_size += fiducials[i].leaf_size;
total_fiducial_size += fiducials[i].root_size;
}
FiducialObject *existing_fiducial = NULL;
// update objects we had in the last frame
// also check if we have an ID/position conflict
// or correct an INVALID_FIDUCIAL_ID if we had an ID in the last frame
for (std::list<FiducialObject>::iterator fiducial = fiducialList.begin(); fiducial!=fiducialList.end(); fiducial++) {
float distance = fiducial->distance(fiducials[i].x,fiducials[i].y);
if (fiducials[i].id==fiducial->fiducial_id) {
// find and match a fiducial we had last frame already ...
if(!existing_fiducial) {
existing_fiducial = &(*fiducial);
for (int j=0;j<fid_count;j++) {
if ( (i!=j) && (fiducials[j].id==fiducial->fiducial_id) && (fiducial->distance(fiducials[j].x,fiducials[j].y)<distance)) {
//check if there is another fiducial with the same id closer
existing_fiducial = NULL;
break;
}
}
if (existing_fiducial!=NULL) {
for (std::list<FiducialObject>::iterator test = fiducialList.begin(); test!=fiducialList.end(); test++) {
FiducialObject *test_fiducial = &(*test);
if ( (test_fiducial!=existing_fiducial) && (test_fiducial->fiducial_id==existing_fiducial->fiducial_id) && (test_fiducial->distance(fiducials[i].x,fiducials[i].y)<distance)) {
//check if there is another fiducial with the same id closer
existing_fiducial = NULL;
break;
}
}}
} /*else if (distance<existing_fiducial->distance(fiducials[i].x,fiducials[i].y)) {
existing_fiducial = &(*fiducial);
// is this still necessary?
} */
} else if ((distance<average_fiducial_size/1.2) && (abs(fiducials[i].node_count-fiducial->node_count)<=FUZZY_NODE_RANGE)) {
// do we have a different ID at the same place?
// this should correct wrong or invalid fiducial IDs
// assuming that between two frames
// there can't be a rapid exchange of two symbols
// at the same place
for (int j=0;j<fid_count;j++) {
if ( (i!=j) && (fiducial->distance(fiducials[j].x,fiducials[j].y)<distance)) goto fiducialList_loop_end;
}
if (fiducials[i].id==INVALID_FIDUCIAL_ID) {
//printf("corrected invalid ID to %d (%ld)\n", fiducial->fiducial_id,fiducial->session_id);
//two pixel threshold since missing/added leaf nodes result in a slightly different position
float dx = abs(fiducial->getX() - fiducials[i].x);
float dy = abs(fiducial->getY() - fiducials[i].y);
if ((dx<2.0f) && (dy<2.0f)) {
fiducials[i].x = (short int)fiducial->getX();
fiducials[i].y = (short int)fiducial->getY();
}
fiducials[i].angle=fiducial->getAngle();
fiducials[i].id=fiducial->fiducial_id;
fiducial->state = FIDUCIAL_INVALID;
drawObject(fiducials[i].id,(int)(fiducials[i].x),(int)(fiducials[i].y),display,0);
} else /*if (fiducials[i].id!=fiducial->fiducial_id)*/ {
if (!fiducial->checkIdConflict(session_id,fiducials[i].id)) {
//printf("corrected wrong ID from %d to %d (%ld)\n", fiducials[i].id,fiducial->fiducial_id,fiducial->session_id);
fiducials[i].id=fiducial->fiducial_id;
} else {
session_id++;
}
}
existing_fiducial = &(*fiducial);
break;
}
fiducialList_loop_end:;
}
if (existing_fiducial!=NULL) {
// just update the fiducial from last frame ...
existing_fiducial->update(fiducials[i].x,fiducials[i].y,fiducials[i].angle,fiducials[i].root_size,fiducials[i].leaf_size);
if(existing_fiducial->state!=FIDUCIAL_INVALID) drawObject(existing_fiducial->fiducial_id,(int)(existing_fiducial->getX()),(int)(existing_fiducial->getY()),display,1);
} else if (fiducials[i].id!=INVALID_FIDUCIAL_ID) {
// add the newly found object
session_id++;
FiducialObject addFiducial(session_id, fiducials[i].id, width, height,fiducials[i].root_colour,fiducials[i].node_count);
addFiducial.update(fiducials[i].x,fiducials[i].y,fiducials[i].angle,fiducials[i].root_size,fiducials[i].leaf_size);
drawObject(fiducials[i].id,(int)(fiducials[i].x),(int)(fiducials[i].y),display,1);
fiducialList.push_back(addFiducial);
#ifndef DISABLE_MIDISERVER
if (midi_server!=NULL) midi_server->sendAddMessage(fiducials[i].id);
#endif
if (msg_listener) {
std::stringstream add_message;
add_message << "add obj " << session_id << " " << fiducials[i].id;
msg_listener->setMessage(add_message.str());
}
}
//else drawObject(fiducials[i].id,(int)(fiducials[i].x),(int)(fiducials[i].y),display,0);
}
if (valid_fiducial_count>0) {
average_leaf_size = (average_leaf_size + total_leaf_size/(double)valid_fiducial_count)/2;
average_fiducial_size = (average_fiducial_size + total_fiducial_size/(double)valid_fiducial_count)/2;
}
//std::cout << "leaf size: " << average_leaf_size << std::endl;
//std::cout << "fiducial size: " << average_fiducial_size << std::endl;
//std::cout << "finger size: " << average_finger_size << std::endl;
float min_object_size = average_fiducial_size - average_fiducial_size/4.0f;
float max_object_size = average_fiducial_size + average_fiducial_size/4.0f;
float min_finger_size = average_finger_size - average_finger_size/1.75f;
float max_finger_size = average_finger_size + average_finger_size/1.75f;
// plain object tracking
int min_region_size = min_finger_size;
int max_region_size = max_object_size;
if ((average_finger_size==0) || (min_region_size>min_object_size)) min_region_size = min_object_size;
int reg_count = find_regionsX( regions, MAX_FIDUCIAL_COUNT, &fidtrackerx, &segmenter, width, height, min_region_size, max_region_size);
//std::cout << reg_count << std::endl;
//if (average_fiducial_size>width/4) goto send_messages;
for(int j=0;j< reg_count;j++) {
//printf("region: %d %d\n", regions[j].x, regions[j].y);
//printf("region: %d %d\n", regions[j].width, regions[j].height);
/*
//reactable specific temporary hack
//check if object is within the inner circle
float dx = width/2 - regions[j].x;
float dy = height/2 - regions[j].y;
float distance = sqrt(dx*dx+dy*dy);
if (distance>(height/2-height/10)) continue;
*/
int diff = abs(regions[j].width - regions[j].height);
int max_diff = regions[j].width;
if (regions[j].height > regions[j].width) max_diff = regions[j].height/2.0f;
// plain objects
if ((regions[j].width>min_object_size) && (regions[j].width<max_object_size) &&
(regions[j].height>min_object_size) && (regions[j].height<max_object_size) && diff < max_diff) {
//printf("region: %d %d\n", regions[j].width, regions[j].height);
FiducialObject *existing_fiducial = NULL;
for (std::list<FiducialObject>::iterator fiducial = fiducialList.begin(); fiducial!=fiducialList.end(); fiducial++) {
float distance = fiducial->distance(regions[j].x,regions[j].y);
if ((distance<average_fiducial_size/1.5f) && (fiducial->root_colour==regions[j].colour)) {
existing_fiducial = &(*fiducial);
// check if the fiducial was already found anyway
for (int i=0;i<fid_count;i++) {
float dx = fiducials[i].x - regions[j].x;
float dy = fiducials[i].y - regions[j].y;
distance = sqrt(dx*dx+dy*dy);
if ( distance<average_fiducial_size/1.5f) {
existing_fiducial->setBlobOffset(dx,dy);
existing_fiducial = NULL;
break;
}
}
if (existing_fiducial != NULL) break;
}
}
if (existing_fiducial!=NULL) {
FloatPoint offset = existing_fiducial->getBlobOffset();
float xpos = regions[j].x + offset.x;
float ypos = regions[j].y + offset.y;
float angle = existing_fiducial->getAngle();
//two pixel threshold since root node blobs do not provide a precise position
float dx = abs(existing_fiducial->getX() - xpos);
float dy = abs(existing_fiducial->getY() - ypos);
if ((dx<2.0f) && (dy<2.0f)) {
xpos = existing_fiducial->getX();
ypos = existing_fiducial->getY();
}
existing_fiducial->update(xpos,ypos,angle,(regions[j].width+regions[j].height)/2,0);
//printf("region: %d %d\n", regions[j].width, regions[j].height);
//printf("recovered plain fiducial %d (%ld)\n", existing_fiducial->fiducial_id,existing_fiducial->session_id);
existing_fiducial->state = FIDUCIAL_REGION;
drawObject(existing_fiducial->fiducial_id,xpos,ypos,display,2);
} //else goto plain_analysis;
// plain fingers
} else if (detect_finger && (regions[j].colour==255) && (regions[j].width>min_finger_size) && (regions[j].width<max_finger_size) &&
(regions[j].height>min_finger_size) && (regions[j].height<max_finger_size) && diff < max_diff) {
//check first if the finger is valid
//printf("candidate: %d %d\n", regions[j].width, regions[j].height);
int finger_match = check_finger(®ions[j],dest,(unsigned char*)(display->pixels));
if(finger_match<0) continue;//goto plain_analysis;
//printf("finger: %d %d\n", regions[j].width, regions[j].height);
FingerObject *existing_finger = NULL;
float closest = width;
// ignore fingers within existing fiducials
for (std::list<FiducialObject>::iterator fiducial = fiducialList.begin(); fiducial!=fiducialList.end(); fiducial++) {
if (fiducial->distance(regions[j].x, regions[j].y)<fiducial->root_size/2) goto region_loop_end;
}
// attach to existing fingers in the list
for (std::list<FingerObject>::iterator finger = fingerList.begin(); finger!=fingerList.end(); finger++) {
float distance = finger->distance(regions[j].x,regions[j].y);
if ((distance<average_finger_size*2) && (distance<closest)){
existing_finger = &(*finger);
closest = distance;
//is there another finger closer than that?
if (distance>average_finger_size/2) {
for(int k=0;k< reg_count;k++) {
if(j==k) continue;
if ((regions[k].width>min_finger_size) && (regions[k].width<max_finger_size) &&
(regions[k].height>min_finger_size) && (regions[k].height<max_finger_size) && diff < max_diff) {
int dx = abs(regions[k].x - regions[j].x);
int dy = abs(regions[k].y - regions[j].y);
float dist = sqrt((float)(dx*dx+dy*dy));
if (dist<=distance) { existing_finger=NULL; closest=dist; break; }
}
}}
if (existing_finger!=NULL) {
for (std::list<FingerObject>::iterator test = fingerList.begin(); test!=fingerList.end(); test++) {
FingerObject *test_finger = &(*test);
if ( (test_finger!=existing_finger) && (test_finger->distance(regions[j].x,regions[j].y)<distance)) {
//check if there is another fiducial with the same id closer
existing_finger = NULL;
break;
}
}}
}
}
// update or add the finger
if (existing_finger!=NULL) {
existing_finger->update(regions[j].x,regions[j].y,regions[j].area);
} else if (finger_match==1) {
// add the newly found cursor
FingerObject addFinger(width, height);
addFinger.update(regions[j].x,regions[j].y,regions[j].area);
fingerList.push_back(addFinger);
} else continue; //goto plain_analysis;
drawObject(FINGER_ID,(int)(regions[j].x),(int)(regions[j].y),display,1);
region_loop_end:;
} /*else if ( (regions[j].width>average_leaf_size*2) && (regions[j].height>average_leaf_size*2)) {
plain_analysis:;
//do the plain object analysis larger than twice the leaf size.
bool analyse = true;
// check if object is within any found fiducial
for (std::list<FiducialObject>::iterator fiducial = fiducialList.begin(); fiducial!=fiducialList.end(); fiducial++) {
float distance = fiducial->distance(regions[j].x,regions[j].y);
if (distance<average_fiducial_size/2) { analyse = false; break; }
}
if (analyse) obj_analyzer->process(®ions[j],src, dest, display);
}*/
}
if (tuio_server!=NULL) {
sendTuioMessages();
if (detect_finger) sendCursorMessages();
} else {
#ifndef DISABLE_MIDISERVER
if (midi_server!=NULL) sendMidiMessages();
#endif
}
if (show_grid) drawGrid(src,dest,display);
if (show_settings) drawGUI(display);
//printStatistics(start_time);
}
void moFiducialTrackerModule::applyFilter(IplImage *src) {
fiducials_data_t *fids = static_cast<fiducials_data_t*>(this->internal);
moDataGenericContainer *fiducial;
FiducialX *fdx;
int fid_count, valid_fiducials = 0;
bool do_image = this->output->getObserverCount() > 0 ? true : false;
CvSize size = cvGetSize(src);
CvFont font, font2;
cvInitFont(&font, CV_FONT_HERSHEY_DUPLEX, 1.0, 1.0, 0, 2);
cvInitFont(&font2, CV_FONT_HERSHEY_PLAIN, 1.0, 1.0, 0, 1);
assert( src != NULL );
assert( fids != NULL );
assert( src->imageData != NULL );
if ( src->nChannels != 1 ) {
this->setError("FiducialTracker input image must be a single channel binary image.");
this->stop();
return;
}
// prepare image if we have listener on output
if ( do_image )
cvSet(this->output_buffer, CV_RGB(0, 0, 0));
// libfidtrack
step_segmenter(&fids->segmenter, (const unsigned char*)src->imageData);
fid_count = find_fiducialsX(fids->fiducials, MAX_FIDUCIALS,
&fids->fidtrackerx, &fids->segmenter, src->width, src->height);
// prepare to refill fiducials
this->clearFiducials();
for ( int i = 0; i < fid_count; i++ ) {
fdx = &fids->fiducials[i];
// invalid id (INVALID_FIDUCIAL_ID)
if ( fdx->id < 0 )
continue;
// got a valid fiducial ! process...
valid_fiducials++;
LOGM(MO_DEBUG, "fid:" << i << " id=" << fdx->id << " pos=" \
<< fdx->x << "," << fdx->y << " angle=" << fdx->angle);
fiducial = new moDataGenericContainer();
fiducial->properties["implements"] = new moProperty("fiducial,pos,tracked");
fiducial->properties["fiducial_id"] = new moProperty(fdx->id);
fiducial->properties["blob_id"] = new moProperty(fdx->id);
fiducial->properties["x"] = new moProperty(fdx->x / size.width);
fiducial->properties["y"] = new moProperty(fdx->y / size.height);
fiducial->properties["angle"] = new moProperty(fdx->angle);
fiducial->properties["leaf_size"] = new moProperty(fdx->leaf_size);
fiducial->properties["root_size"] = new moProperty(fdx->root_size);
this->fiducials.push_back(fiducial);
// draw on output image
if ( do_image ) {
std::ostringstream oss;
oss << fdx->id;
cvPutText(this->output_buffer, oss.str().c_str(),
cvPoint(fdx->x, fdx->y - 20), &font, cvScalar(20, 255, 20));
oss.str("");
oss << "angle:" << int(fdx->angle * 180 / 3.14159265);
cvPutText(this->output_buffer, oss.str().c_str(),
cvPoint(fdx->x - 30, fdx->y), &font2, cvScalar(20, 255, 20));
oss.str("");
oss << "l/r:" << fdx->leaf_size << "/" << fdx->root_size;
cvPutText(this->output_buffer, oss.str().c_str(),
cvPoint(fdx->x - 50, fdx->y + 20), &font2, cvScalar(20, 255, 20));
}
}
LOGM(MO_DEBUG, "-> Found " << valid_fiducials << " fiducials");
this->output_data->push(&this->fiducials);
}