//=============================================================================
int main(int argc, const char** argv)
{
//parse command line arguments
char ftFile[256],conFile[256],triFile[256];
bool fcheck = false; double scale = 1; int fpd = -1; bool show = true;
if(parse_cmd(argc,argv,ftFile,conFile,triFile,fcheck,scale,fpd)<0)return 0;
//set other tracking parameters
std::vector<int> wSize1(1); wSize1[0] = 7;
std::vector<int> wSize2(3); wSize2[0] = 11; wSize2[1] = 9; wSize2[2] = 7;
int nIter = 5; double clamp=3,fTol=0.01;
FACETRACKER::Tracker model(ftFile);
cv::Mat tri=FACETRACKER::IO::LoadTri(triFile);
cv::Mat con=FACETRACKER::IO::LoadCon(conFile);
//initialize camera and display window
cv::Mat frame,gray,im; double fps=0; char sss[256]; std::string text;
CvCapture* camera = cvCreateCameraCapture(CV_CAP_ANY); if(!camera)return -1;
int64 t1,t0 = cvGetTickCount(); int fnum=0;
cvNamedWindow("Face Tracker",1);
std::cout << "Hot keys: " << std::endl
<< "\t ESC - quit" << std::endl
<< "\t d - Redetect" << std::endl;
//loop until quit (i.e user presses ESC)
bool failed = true;
while(1){
//grab image, resize and flip
IplImage* I = cvQueryFrame(camera); if(!I)continue; frame = I;
if(scale == 1)im = frame;
else cv::resize(frame,im,cv::Size(scale*frame.cols,scale*frame.rows));
cv::flip(im,im,1); cv::cvtColor(im,gray,CV_BGR2GRAY);
//track this image
std::vector<int> wSize; if(failed)wSize = wSize2; else wSize = wSize1;
if(model.Track(gray,wSize,fpd,nIter,clamp,fTol,fcheck) == 0){
int idx = model._clm.GetViewIdx(); failed = false;
Draw(im,model._shape,con,tri,model._clm._visi[idx]);
}else{
if(show){cv::Mat R(im,cvRect(0,0,150,50)); R = cv::Scalar(0,0,255);}
model.FrameReset(); failed = true;
}
//draw framerate on display image
if(fnum >= 9){
t1 = cvGetTickCount();
fps = 10.0/((double(t1-t0)/cvGetTickFrequency())/1e+6);
t0 = t1; fnum = 0;
}else fnum += 1;
if(show){
sprintf(sss,"%d frames/sec",(int)round(fps)); text = sss;
cv::putText(im,text,cv::Point(10,20),
CV_FONT_HERSHEY_SIMPLEX,0.5,CV_RGB(255,255,255));
}
//show image and check for user input
imshow("Face Tracker",im);
int c = cvWaitKey(10);
if(c == 27)break; else if(char(c) == 'd')model.FrameReset();
}return 0;
}
int main(int argc, const char** argv)
{
printf( "\n===== Face tracking Tutorial for DARwIn =====\n\n");
change_current_dir();
minIni* ini = new minIni(INI_FILE_PATH);
Image* rgb_ball = new Image(Camera::WIDTH, Camera::HEIGHT, Image::RGB_PIXEL_SIZE);
LinuxCamera::GetInstance()->Initialize(0);
LinuxCamera::GetInstance()->LoadINISettings(ini);
mjpg_streamer* streamer = new mjpg_streamer(Camera::WIDTH, Camera::HEIGHT);
BallTracker tracker = BallTracker();
//////////////////// Framework Initialize ////////////////////////////
LinuxCM730 linux_cm730(U2D_DEV_NAME);
CM730 cm730(&linux_cm730);
if(MotionManager::GetInstance()->Initialize(&cm730) == false)
{
printf("Fail to initialize Motion Manager!\n");
// return 0;
}
MotionManager::GetInstance()->LoadINISettings(ini);
MotionManager::GetInstance()->AddModule((MotionModule*)Head::GetInstance());
LinuxMotionTimer *motion_timer = new LinuxMotionTimer(MotionManager::GetInstance());
motion_timer->Start();
MotionStatus::m_CurrentJoints.SetEnableBodyWithoutHead(false);
MotionManager::GetInstance()->SetEnable(true);
/////////////////////////////////////////////////////////////////////
//Head::GetInstance()->LoadINISettings(ini);
Head::GetInstance()->m_Joint.SetEnableHeadOnly(true, true);
Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_PAN, 8);
Head::GetInstance()->m_Joint.SetPGain(JointData::ID_HEAD_TILT, 8);
printf("Press the ENTER key to begin!\n");
getchar();
//parse command line arguments
char ftFile[256],conFile[256],triFile[256];
bool fcheck = false; double scale = 1; int fpd = -1; bool show = true;
if(parse_cmd(argc,argv,ftFile,conFile,triFile,fcheck,scale,fpd)<0)return 0;
//set other tracking parameters
std::vector<int> wSize1(1); wSize1[0] = 7;
std::vector<int> wSize2(3); wSize2[0] = 11; wSize2[1] = 9; wSize2[2] = 7;
int nIter = 5; double clamp=3,fTol=0.01;
FACETRACKER::Tracker model(ftFile);
cv::Mat tri=FACETRACKER::IO::LoadTri(triFile);
cv::Mat con=FACETRACKER::IO::LoadCon(conFile);
//initialize camera and display window
cv::Mat frame,gray,im; double fps=0; char sss[256]; std::string text;
int64 t1,t0 = cvGetTickCount(); int fnum=0;
bool failed = true;
while(1)
{
Point2D pos;
LinuxCamera::GetInstance()->CaptureFrame();
//Create the image header which will contain the converted BYTE image
int channel = 3; //1 for black and white pictures, 3 for colored pictures
int step = LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_WidthStep;//depends from the channel
//grab image, resize and flip
IplImage* I = cvCreateImageHeader(cv::Size(LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_Width,LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_Height), IPL_DEPTH_8U, channel);
//Set the BYTE pointer as attribut of the IplImage --> "converts" BYTE *pData in IplImage *frame
cvSetData(I, LinuxCamera::GetInstance()->fbuffer->m_RGBFrame->m_ImageData, step);
if(!I)continue; frame = I;
if(scale == 1)im = frame;
else cv::resize(frame,im,cv::Size(scale*frame.cols,scale*frame.rows));
cv::flip(im,im,1);
cv::cvtColor(im,gray,CV_RGB2GRAY);
//track this image
std::vector<int> wSize; if(failed)wSize = wSize2; else wSize = wSize1;
if(model.Track(gray,wSize,fpd,nIter,clamp,fTol,fcheck) == 0){
int idx = model._clm.GetViewIdx(); failed = false;
Draw(im,model._shape,con,tri,model._clm._visi[idx]);
pos.X = model._rect.x+ model._rect.width/2;
pos.Y = model._rect.y+ model._rect.height/2;
}else{
if(show){cv::Mat R(im,cvRect(0,0,150,50)); R = cv::Scalar(0,0,255);}
model.FrameReset(); failed = true;
pos.X = -1;
pos.Y = -1;
}
//draw framerate on display image
if(fnum >= 9){
t1 = cvGetTickCount();
fps = 10.0/((double(t1-t0)/cvGetTickFrequency())/1e+6);
t0 = t1; fnum = 0;
}else fnum += 1;
if(show){
sprintf(sss,"%d frames/sec",(int)round(fps)); text = sss;
cv::putText(im,text,cv::Point(10,20),
CV_FONT_HERSHEY_SIMPLEX,0.5,CV_RGB(255,255,255));
}
//show image and check for user input
imshow("Face Tracker",im);
int c = cv::waitKey(10);
if(c == 27)break; else if(char(c) == 'd')model.Save("/home/koreki/Documents/perso/darwin/Linux/project/tutorial/face_tracker/koreki.tracker");//model.FrameReset();
//model.FrameReset();
// pos = facefinder->GetPosition(LinuxCamera::GetInstance()->fbuffer->m_RGBFrame);
tracker.Process(pos);
fprintf(stderr, "posx: %f, posy: %f \r", pos.X, pos.Y);
rgb_ball = LinuxCamera::GetInstance()->fbuffer->m_RGBFrame;
rgb_ball->m_ImageData = im.data;
streamer->send_image(rgb_ball);
}
return 0;
}