int main(int argc,char **argv)
{
int numSensors=0, numButtons=0, numValuators=0;
int trackerKey=0, controllerKey=0;
int i; // sensornum;
//CAVE_CONFIG_ST config;
//CAVE_SENSOR_ST sensor;
//CAVE_CONTROLLER_ST controlvals;
if (argc < 5)
{
fprintf(stderr,"Usage: %s [-sensors <numsensors>] [-buttons <numbuttons>]"
" [-valuators <numvaluators>] [-trackerkey <keynum>] [-controllerkey <keynum>]\n", argv[0]);
exit(1);
}
// Get command line
for (i=1; i < argc; i++)
{
if (!strcmp(argv[i],"-sensors"))
numSensors = atoi(argv[++i]);
else if (!strcmp(argv[i],"-buttons"))
numButtons = atoi(argv[++i]);
else if (!strcmp(argv[i],"-valuators"))
numValuators = atoi(argv[++i]);
else if (!strcmp(argv[i],"-trackerkey"))
trackerKey = atoi(argv[++i]);
else if (!strcmp(argv[i],"-controllerkey"))
controllerKey = atoi(argv[++i]);
else
printf("Unknown option \"%s\"\n",argv[i]);
}
allocateTrackerMem(trackerKey);
allocateControllerMem(controllerKey);
initTracker();
initController();
tracker->header.numSensors = numSensors;
controller->header.numButtons = numButtons;
//controller->controller.num_buttons = numButtons;
controller->header.numValuators = numValuators;
//controller->controller.num_valuators = numValuators;
while (1)
{
setNewTrackerData(numSensors);
getTimeStaMp(tracker->header.timestamp);
if (get_new_controller_data(numButtons,numValuators))
{
getTimeStaMp(controller->header.timestamp);
}
usleep(100);
}
return 1;
}
// ######################################################################
void VisualTracker::setTargets(const Image<byte>& grey, const Point2D<int> loc)
{
if (!loc.isValid())
{
itsTracking = false;
return;
}
if (itsInitTracker)
initTracker(grey.getDims());
#ifdef HAVE_OPENCV
itsCurrentNumPoints = itsMaxNumPoints;
IplImage* currentImg = img2ipl(grey);
itsCurrentPoints[0].x = loc.i;
itsCurrentPoints[0].y = loc.j;
//cvFindCornerSubPix(currentImg, itsCurrentPoints, itsCurrentNumPoints,
// cvSize(itsInitTrackWindowSize.getVal(),itsInitTrackWindowSize.getVal()),
// cvSize(-1,-1),
// cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,
// 20,0.03));
cvReleaseImageHeader(¤tImg);
itsPreviousGreyImg = grey;
itsTrackFlags = 0;
IplImage *swap_temp;
CV_SWAP( itsPreviousPyramid, itsCurrentPyramid, swap_temp );
CvPoint2D32f* swap_points;
CV_SWAP( itsPreviousPoints, itsCurrentPoints, swap_points );
if (itsUseKalman)
{
itsKalman->state_post->data.fl[0] = loc.i;
itsKalman->state_post->data.fl[1] = loc.j;
}
#endif
itsTracking = true;
}
// ######################################################################
void VisualTracker::setTargets(const Image<byte>& grey, const Image<byte>& target)
{
#ifdef HAVE_OPENCV
if (itsInitTracker)
initTracker(grey.getDims());
IplImage* currentImg = img2ipl(grey);
cvCalcHist( ¤tImg, itsObjectHist );
float max_val = 0.f;
cvGetMinMaxHistValue(itsObjectHist, 0, &max_val, 0, 0 );
cvConvertScale( itsObjectHist->bins, itsObjectHist->bins, max_val ? 255. / max_val : 0., 0 );
itsTargetTempl = target;
#endif
}
int SingleSLAM::initTracker(int f) {
vector<FeaturePoint*> existPts;
return initTracker(f, existPts);
}
void TrackShirt::ImageCallback(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
frame = cv_ptr->image;
char key = (char)cvWaitKey(10);
if (key ==27 ) {
ros::requestShutdown();
} else if ( key =='z' ) {
IMSHOW = true;
//namedWindow(OPENCV_WINDOW);
} else if (key == 'x') {
IMSHOW = false;
cvDestroyAllWindows() ;
//namedWindow(OPENCV_WINDOW);
}
if (trackObject == -1) {
//Initial stage, before selecting object. Do nothing. Camera view shown as is.
} else if (trackObject == 0) {
rectangle(frame, Point(selection.x,selection.y),Point(selection.x+selection.width,selection.y+selection.height),Scalar(0,0,255),1);
} else if (PerFoRoMode == 3) {
Mat imgHSV, imgThresh, binFrame;
int contSize;
cvtColor(frame, imgHSV, CV_BGR2HSV);
//Get binary image using HSV threshold
inRange(imgHSV, mLowerBound, mUpperBound, imgThresh);
//Morphological operations to get smoother blobs with reduced noise
dilate( imgThresh, imgThresh, elemDilate );
erode( imgThresh, imgThresh, elemErode );
dilate( imgThresh, imgThresh, elemDilate );
erode( imgThresh, imgThresh, elemErode );
morphologyEx(imgThresh, imgThresh, MORPH_OPEN, structure_elem);
imgThresh.copyTo(binFrame);
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
/// Find contours
findContours( binFrame, contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );
contSize = contours.size();
//cout<<"contours size "<<contSize<<endl;
//If no contours
if (contSize==0) {
navX = 0;
navY = 0;
if (IMSHOW) {
imshow(OPENCV_WINDOW, frame);
//imshow("Binary Image with Detected Object", imgThresh);
}
return;
}
/// Approximate contours to polygons + get bounding rects
vector<vector<Point> > contours_poly( contSize );
vector<Rect> boundRect( contSize );
/// Get the moments
vector<Moments> mu(contSize );
cv::Mat contArea = Mat::zeros(contSize,1,CV_32FC1);
for( int i = 0; i < contSize; i++ )
{
approxPolyDP( Mat(contours[i]), contours_poly[i], 3, true );
boundRect[i] = boundingRect( Mat(contours_poly[i]) );
mu[i] = moments( contours[i], false );
contArea.at<float>(i) = contourArea(contours[i]);
}
/// Get the mass centers:
vector<Point2f> mc( contSize );
for( int i = 0; i < contSize; i++ )
{
mc[i] = Point2f( mu[i].m10/mu[i].m00 , mu[i].m01/mu[i].m00 );
}
///Nearest centroid to previous position
cv::Mat dist = Mat::zeros(contSize,1,CV_32FC1);
cv::Mat normDist = Mat::zeros(contSize,1,CV_32FC1);
for( int i = 0; i < contSize; i++ )
{
dist.at<float>(i) = abs(mc[i].x - selectCentroid.x) + abs(mc[i].y - selectCentroid.y);
normDist.at<float>(i) = maxDistance - dist.at<float>(i);
}
cv::Mat normSelect= Mat::zeros(contSize,1,CV_32FC1);
normSelect = contArea + normDist; //
cv::Mat sortedSelect = Mat::zeros(contSize,1,CV_32FC1);
cv::sortIdx(normSelect, sortedSelect, CV_SORT_EVERY_COLUMN+CV_SORT_DESCENDING);
Point selectPt = mc[sortedSelect.at<int>(0)];
//If first tracked frame, initialze Kalman
if (trackObject == 1) {
initTracker();
trackObject = 2;
}
//Kalman estimate based on previous state and measurement
kalmanEstimatePt = kalmanTracker(selectPt);
///Distance of object position estimate from previous position
distPrevCurrent = abs(kalmanEstimatePt.x - selectCentroid.x) + abs(kalmanEstimatePt.y - selectCentroid.y);
distPrevCurrent = distPrevCurrent / maxDistance;
if (missCount > 5) {
distThresh*=1.5;
} else {
distThresh = minDistThresh;
}
/// /////////////////////////////////////////////////////////////
///Threshold the detected centroid's distance from prev///////////////
if (distPrevCurrent < distThresh && contArea.at<float>(sortedSelect.at<int>(0)) >= 10) {
//Final object position estimate using kalman
selectCentroid = kalmanEstimatePt;
if (IMSHOW) {
rectangle( frame, boundRect[sortedSelect.at<int>(0)], Scalar(255,255,255), 2, 8, 0 );
}
shirt_msg.x = selectCentroid.x;
shirt_msg.y = selectCentroid.y;
shirt_msg.area = boundRect[sortedSelect.at<int>(0)].width * boundRect[sortedSelect.at<int>(0)].height;
track_shirt_pub_.publish(shirt_msg);
//cout<<"X="<<navX<<"Y="<<navY<<endl;
missCount = 0;
drawArrow(frame, cv::Point(frame.cols/2, frame.rows/2), selectCentroid, Scalar(255,0,0));
} else {
missCount++;
navX = 0.0;
navY = 0.0;
}
}
// Update GUI Window
//if (IMSHOW) {
// imshow(OPENCV_WINDOW, frame);
///imshow("Binary Image with Detected Object", imgThresh);
//}
//cv::waitKey(3);
// Output modified video stream
image_shirt_pub_.publish(cv_ptr->toImageMsg());
}
int main(int argc, char *argv[])
#endif
{
SDL_Event event;
char *loadFile = 0;
pp_int32 defaultBPP = -1;
PPDisplayDevice::Orientations orientation = PPDisplayDevice::ORIENTATION_NORMAL;
bool swapRedBlue = false, noSplash = false;
bool recVelocity = false;
// Parse command line
while ( argc > 1 )
{
--argc;
#ifdef __APPLE__
// OSX: Swallow "-psn_xxx" argument passed by Finder on OSX <10.9
if ( strncmp(argv[argc], "-psn", 4) == 0 )
{
continue;
}
else
#endif
if ( strcmp(argv[argc-1], "-bpp") == 0 )
{
defaultBPP = atoi(argv[argc]);
--argc;
}
else if ( strcmp(argv[argc], "-nosplash") == 0 )
{
noSplash = true;
}
else if ( strcmp(argv[argc], "-swap") == 0 )
{
swapRedBlue = true;
}
else if ( strcmp(argv[argc-1], "-orientation") == 0 )
{
if (strcmp(argv[argc], "NORMAL") == 0)
{
orientation = PPDisplayDevice::ORIENTATION_NORMAL;
}
else if (strcmp(argv[argc], "ROTATE90CCW") == 0)
{
orientation = PPDisplayDevice::ORIENTATION_ROTATE90CCW;
}
else if (strcmp(argv[argc], "ROTATE90CW") == 0)
{
orientation = PPDisplayDevice::ORIENTATION_ROTATE90CW;
}
else
goto unrecognizedCommandLineSwitch;
--argc;
}
else if ( strcmp(argv[argc], "-nonstdkb") == 0)
{
stdKb = false;
}
else if ( strcmp(argv[argc], "-recvelocity") == 0)
{
recVelocity = true;
}
else
{
unrecognizedCommandLineSwitch:
if (argv[argc][0] == '-')
{
fprintf(stderr,
"Usage: %s [-bpp N] [-swap] [-orientation NORMAL|ROTATE90CCW|ROTATE90CW] [-nosplash] [-nonstdkb] [-recvelocity]\n", argv[0]);
exit(1);
}
else
{
loadFile = argv[argc];
}
}
}
globalMutex = new PPMutex();
// Store current working path (init routine is likely to change it)
PPPath_POSIX path;
PPSystemString oldCwd = path.getCurrent();
globalMutex->lock();
initTracker(defaultBPP, orientation, swapRedBlue, noSplash);
globalMutex->unlock();
#ifdef HAVE_LIBASOUND
if (myMidiReceiver && recVelocity)
{
myMidiReceiver->setRecordVelocity(true);
}
#endif
if (loadFile)
{
PPSystemString newCwd = path.getCurrent();
path.change(oldCwd);
SendFile(loadFile);
path.change(newCwd);
pp_uint16 chr[3] = {VK_RETURN, 0, 0};
PPEvent event(eKeyDown, &chr, sizeof(chr));
RaiseEventSerialized(&event);
}
// Main event loop
done = 0;
while (!done && SDL_WaitEvent(&event))
{
switch (event.type)
{
case SDL_QUIT:
exitSDLEventLoop(false);
break;
case SDL_MOUSEMOTION:
{
// Ignore old mouse motion events in the event queue
SDL_Event new_event;
if (SDL_PeepEvents(&new_event, 1, SDL_GETEVENT, SDL_MOUSEMOTION, SDL_MOUSEMOTION) > 0)
{
while (SDL_PeepEvents(&new_event, 1, SDL_GETEVENT, SDL_MOUSEMOTION, SDL_MOUSEMOTION) > 0);
processSDLEvents(new_event);
}
else
{
processSDLEvents(event);
}
break;
}
// Open modules drag 'n dropped onto MilkyTracker (currently only works on Dock icon, OSX)
case SDL_DROPFILE:
SendFile(event.drop.file);
SDL_free(event.drop.file);
break;
// Refresh GUI if window resized
case SDL_WINDOWEVENT:
switch (event.window.event) {
case SDL_WINDOWEVENT_RESIZED:
myTrackerScreen->update();
}
break;
case SDL_USEREVENT:
processSDLUserEvents((const SDL_UserEvent&)event);
break;
default:
processSDLEvents(event);
break;
}
}
ticking = false;
SDL_RemoveTimer(timer);
globalMutex->lock();
#ifdef HAVE_LIBASOUND
delete myMidiReceiver;
#endif
delete myTracker;
myTracker = NULL;
delete myTrackerScreen;
myTrackerScreen = NULL;
delete myDisplayDevice;
globalMutex->unlock();
SDL_Quit();
delete globalMutex;
return 0;
}
int main(int argc, char *argv[])
#endif
{
Uint32 videoflags;
SDL_Event event;
char *loadFile = 0;
pp_int32 defaultBPP = -1;
PPDisplayDevice::Orientations orientation = PPDisplayDevice::ORIENTATION_NORMAL;
bool swapRedBlue = false, fullScreen = false, noSplash = false;
bool recVelocity = false;
// Parse command line
while ( argc > 1 )
{
--argc;
if ( strcmp(argv[argc-1], "-bpp") == 0 )
{
defaultBPP = atoi(argv[argc]);
--argc;
}
else if ( strcmp(argv[argc], "-nosplash") == 0 )
{
noSplash = true;
}
else if ( strcmp(argv[argc], "-swap") == 0 )
{
swapRedBlue = true;
}
else if ( strcmp(argv[argc], "-fullscreen") == 0)
{
fullScreen = true;
}
else if ( strcmp(argv[argc-1], "-orientation") == 0 )
{
if (strcmp(argv[argc], "NORMAL") == 0)
{
orientation = PPDisplayDevice::ORIENTATION_NORMAL;
}
else if (strcmp(argv[argc], "ROTATE90CCW") == 0)
{
orientation = PPDisplayDevice::ORIENTATION_ROTATE90CCW;
}
else if (strcmp(argv[argc], "ROTATE90CW") == 0)
{
orientation = PPDisplayDevice::ORIENTATION_ROTATE90CW;
}
else
goto unrecognizedCommandLineSwitch;
--argc;
}
else if ( strcmp(argv[argc], "-nonstdkb") == 0)
{
stdKb = false;
}
else if ( strcmp(argv[argc], "-recvelocity") == 0)
{
recVelocity = true;
}
else
{
unrecognizedCommandLineSwitch:
if (argv[argc][0] == '-')
{
fprintf(stderr,
"Usage: %s [-bpp N] [-swap] [-orientation NORMAL|ROTATE90CCW|ROTATE90CW] [-fullscreen] [-nosplash] [-nonstdkb] [-recvelocity]\n", argv[0]);
exit(1);
}
else
{
loadFile = argv[argc];
}
}
}
// Workaround for seg-fault in SDL_Init on Eee PC (thanks nostromo)
// (see http://forum.eeeuser.com/viewtopic.php?pid=136945)
#if HAVE_DECL_SDL_PUTENV
SDL_putenv("SDL_VIDEO_X11_WMCLASS=Milkytracker");
#endif
timerMutex = new PPMutex();
globalMutex = new PPMutex();
// Store current working path (init routine is likely to change it)
PPPath_POSIX path;
PPSystemString oldCwd = path.getCurrent();
globalMutex->lock();
initTracker(defaultBPP, orientation, swapRedBlue, fullScreen, noSplash);
globalMutex->unlock();
#ifdef HAVE_LIBASOUND
if (myMidiReceiver && recVelocity)
{
myMidiReceiver->setRecordVelocity(true);
}
#endif
if (loadFile)
{
PPSystemString newCwd = path.getCurrent();
path.change(oldCwd);
SendFile(loadFile);
path.change(newCwd);
pp_uint16 chr[3] = {VK_RETURN, 0, 0};
PPEvent event(eKeyDown, &chr, sizeof(chr));
RaiseEventSerialized(&event);
}
/* Main event loop */
done = 0;
while (!done && SDL_WaitEvent(&event))
{
switch (event.type)
{
case SDL_QUIT:
exitSDLEventLoop(false);
break;
case SDL_MOUSEMOTION:
{
// ignore old mouse motion events in the event queue
SDL_Event new_event;
if (SDL_PeepEvents(&new_event, 1, SDL_GETEVENT, SDL_EVENTMASK(SDL_MOUSEMOTION)) > 0)
{
while (SDL_PeepEvents(&new_event, 1, SDL_GETEVENT, SDL_EVENTMASK(SDL_MOUSEMOTION)) > 0);
processSDLEvents(new_event);
}
else
{
processSDLEvents(event);
}
break;
}
case SDL_USEREVENT:
processSDLUserEvents((const SDL_UserEvent&)event);
break;
default:
processSDLEvents(event);
break;
}
}
#ifdef __GP2X__
SDL_JoystickClose(0);
#endif
timerMutex->lock();
ticking = false;
timerMutex->unlock();
SDL_SetTimer(0, NULL);
timerMutex->lock();
globalMutex->lock();
#ifdef HAVE_LIBASOUND
delete myMidiReceiver;
#endif
delete myTracker;
myTracker = NULL;
delete myTrackerScreen;
myTrackerScreen = NULL;
delete myDisplayDevice;
globalMutex->unlock();
timerMutex->unlock();
SDL_Quit();
delete globalMutex;
delete timerMutex;
/* Quoting from README.Qtopia (Application Porting Notes):
One thing I have noticed is that applications sometimes don't exit
correctly. Their icon remains in the taskbar and they tend to
relaunch themselves automatically. I believe this problem doesn't
occur if you exit your application using the exit() method. However,
if you end main() with 'return 0;' or so, this seems to happen.
*/
#ifdef __QTOPIA__
exit(0);
#else
return 0;
#endif
}
