void MainWindow::OpticalFlow()
{
cvReleaseCapture(&pCapture);
// pCapture=cvCaptureFromCAM(0);
//use webcam
CvCapture* cam = cvCaptureFromCAM( 0 ) ;
while(1)
{
//get a color image
IplImage* frame = cvQueryFrame(cam) ;
CvSize img_sz = cvGetSize(frame);
const int win_size = 10 ;
//convert the image to grey image
IplImage* frame_prev = cvQueryFrame(cam) ;
IplImage* img_prev = cvCreateImage(img_sz,IPL_DEPTH_8U,1) ;
cvCvtColor( frame_prev,img_prev ,CV_BGR2GRAY);
//convert the image to grey image
IplImage* frame_cur = cvQueryFrame(cam) ;
IplImage* img_curr = cvCreateImage(img_sz,IPL_DEPTH_8U,1) ;
cvCvtColor( frame_cur,img_curr ,CV_BGR2GRAY);
//create a imge to display result
IplImage* img_res = cvCreateImage(img_sz,IPL_DEPTH_8U,1) ;
for ( int y = 0 ; y < img_sz.height ; ++y )
{
uchar* ptr = (uchar*)( img_res->imageData + y * img_res->widthStep ) ;
for ( int x = 0 ; x <img_res->width; ++x )
{
ptr[x] = 255 ;
}
}
//get good features
IplImage* img_eig = cvCreateImage(img_sz,IPL_DEPTH_32F,1) ;
IplImage* img_temp = cvCreateImage(img_sz,IPL_DEPTH_32F,1) ;
int corner_count = MAX_CORNERS ;
CvPoint2D32f* features_prev = new CvPoint2D32f[MAX_CORNERS] ;
cvGoodFeaturesToTrack(
img_prev,
img_eig,
img_temp,
features_prev,
&corner_count,
0.01,
5.0,
0,
3,
0,
0.4
);
cvFindCornerSubPix(
img_prev,
features_prev,
corner_count,
cvSize(win_size,win_size),
cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER,20,0.03)
);
// L-K
char feature_found[ MAX_CORNERS ] ;
float feature_errors[ MAX_CORNERS ] ;
CvSize pyr_sz = cvSize( frame->width + 8 ,frame->height / 3 ) ;
IplImage* pyr_prev = cvCreateImage(pyr_sz,IPL_DEPTH_32F,1) ;
IplImage* pyr_cur = cvCreateImage(pyr_sz,IPL_DEPTH_32F,1) ;
CvPoint2D32f* features_cur = new CvPoint2D32f[ MAX_CORNERS ] ;
cvCalcOpticalFlowPyrLK(
img_prev,
img_curr,
pyr_prev,
pyr_cur,
features_prev,
features_cur,
corner_count,
cvSize(win_size,win_size),
5,
feature_found,
feature_errors,
cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER,20,0.3),
0
);
for ( int i = 0 ; i < corner_count ; i++)
{
if ( 0 == feature_found[i] || feature_errors[i] > 550 )
{
// printf("error is %f \n" , feature_errors[i] ) ;
continue ;
}
// printf("find it !\n") ;
CvPoint pt_prev = cvPoint( features_prev[i].x , features_prev[i].y ) ;
CvPoint pt_cur = cvPoint( features_cur[i].x , features_cur[i].y ) ;
cvLine( img_res,pt_prev,pt_cur,CV_RGB( 255,0,0),2 );
}
if(27==cvWaitKey(33))
break;
MainWindow::Display(frame_cur,img_curr,img_res);
cvReleaseImage(&img_curr);
cvReleaseImage(&img_eig);
cvReleaseImage(&img_prev);
cvReleaseImage(&img_res);
cvReleaseImage(&img_temp);
}
}
void KfieldGui::on_mouse(int event, int x, int y, int flags, void* param) {
if (state == FINISH || state == READY_TO_SEND_TWO || state == READY_TO_SEND_SINGLE)
return;
pthread_mutex_lock(&lock);
cvCopy(field, screenfield);
//Select mouse Event
if (event == CV_EVENT_LBUTTONDOWN) {
if (state == TWOPOINTSELECTION_2) {
cvLine(cleanfield, cvPoint(point1.x, point1.y), cvPoint(x, y), color["gray"], 1, 0, 0);
cvLine(field, cvPoint(point1.x, point1.y), cvPoint(x, y), color["gray"], 1, 0, 0);
state = TWOPOINTSELECTION_3;
}
last.x = x;
last.y = y;
drawing = 1;
} else if (event == CV_EVENT_LBUTTONUP) {
cvLine(cleanfield, cvPoint(last.x, last.y), cvPoint(x, y), color["gray"], 1, CV_AA, 0);
cvCircle(cleanfield, cvPoint(last.x, last.y), 100 / scale/*sqrt((last.x - x) * (last.x - x) + (last.y - y) * (last.y - y))*/, CV_RGB(100, 100, 50), 1, CV_AA, 0);
std::cout << "X " << last.x << " Y " << last.y << " angle " << atan2f(-(y - last.y), x - last.x) * 180.0 / 3.141592653589793238 << std::endl;
if (state == ONEPOINTSELECTION || state == TWOPOINTSELECTION_1) {
point1 = last;
// point1.y = last.y;
KfieldGui::pose1.set_x(gui2world_x(last.x));//last.x * scale - (2 * margintoline + field_width) / 2.0);
KfieldGui::pose1.set_y(gui2world_y(last.y)); //-(last.y * scale - (2 * margintoline + field_height) / 2.0));
KfieldGui::pose1.set_phi(atan2f(-(y - last.y), x - last.x));
KfieldGui::pose1.set_confidence(sqrt((last.x - x) * (last.x - x) + (last.y - y) * (last.y - y)));
std::cout << "X " << pose1.x() << " Y " << pose1.y() << " angle " << atan2f(-(y - last.y), x - last.x) * 180.0 / 3.141592653589793238 << std::endl;
tempparticl.x = pose1.x();
tempparticl.y = pose1.y();
tempparticl.phi = pose1.phi();
tempparticl.Weight = pose1.confidence();
if (state == ONEPOINTSELECTION) {
state = READY_TO_SEND_SINGLE;
drawing = 0;
} else {
state = TWOPOINTSELECTION_2;
cout << "Select Second Point " << endl;
}
} else if (state == TWOPOINTSELECTION_3) {
KfieldGui::pose2.set_x(gui2world_x(last.x));
KfieldGui::pose2.set_y(gui2world_y(last.y));
KfieldGui::pose2.set_phi(atan2f(-(y - last.y), x - last.x));
KfieldGui::pose2.set_confidence(sqrt((last.x - x) * (last.x - x) + (last.y - y) * (last.y - y)));
state = FINISH;//READY_TO_SEND_TWO;
// cout << " Ready to send two" << endl;
record_odometry_data();
drawing = 0;
}
} else if (event == CV_EVENT_MOUSEMOVE) {
if (drawing)
drawCursor(x, y);
}
pthread_mutex_unlock(&lock);
}
// parameters:
// img - input video frame
// dst - resultant motion picture
// args - optional parameters
void update_mhi(IplImage* img, IplImage* dst, int diff_threshold)
{
// 获取当前时间
double timestamp = (double)clock()/CLOCKS_PER_SEC; // get current time in seconds
// 获取当前帧大小
CvSize size = cvSize(img->width,img->height); // get current frame size
int i, idx1 = last, idx2;
IplImage* silh;
CvSeq* seq;
CvRect comp_rect;
double count;
double angle;
CvPoint center;
double magnitude;
CvScalar color;
// 给图像分配空间或者在尺寸改变的时候重新分配
// allocate images at the beginning or
// reallocate them if the frame size is changed
if (!mhi || mhi->width != size.width || mhi->height != size.height)
{
if (buf == 0)
{
buf = (IplImage**)malloc(N*sizeof(buf[0]));
memset( buf, 0, N*sizeof(buf[0]));
}
for( i = 0; i < N; i++ ) {
cvReleaseImage( &buf[i] );
buf[i] = cvCreateImage( size, IPL_DEPTH_8U, 1 );
cvZero( buf[i] );
}
cvReleaseImage( &mhi );
cvReleaseImage( &orient );
cvReleaseImage( &segmask );
cvReleaseImage( &mask );
mhi = cvCreateImage( size, IPL_DEPTH_32F, 1 );
// 在开始时清空MHI
cvZero( mhi ); // clear MHI at the beginning
// 按img的尺寸创建图像
orient = cvCreateImage( size, IPL_DEPTH_32F, 1 );
segmask = cvCreateImage( size, IPL_DEPTH_32F, 1 );
mask = cvCreateImage( size, IPL_DEPTH_8U, 1 );
}
// 转换为灰度
cvCvtColor( img, buf[last], CV_BGR2GRAY ); // convert frame to grayscale
idx2 = (last + 1) % N; // index of (last - (N-1))th frame
last = idx2;
silh = buf[idx2];
// 获取两帧间的差异，当前帧跟背景图相减，放到silh里面
cvAbsDiff(buf[idx1], buf[idx2], silh); // get difference between frames
// 二值化
cvThreshold(silh, silh, diff_threshold, 1, CV_THRESH_BINARY); // and threshold it
// 去掉影像(silhouette) 以更新运动历史图像
cvUpdateMotionHistory(silh, mhi, timestamp, MHI_DURATION); // update MHI
// 转换MHI到蓝色8位图
// convert MHI to blue 8u image
cvCvtScale( mhi, mask, 255./MHI_DURATION,
(MHI_DURATION - timestamp)*255./MHI_DURATION );
cvZero( dst );
cvCvtPlaneToPix( mask, 0, 0, 0, dst );
// 计算运动历史图像的梯度方向
// 计算运动梯度趋向和合法的趋向掩码
// calculate motion gradient orientation and valid orientation mask
cvCalcMotionGradient( mhi, mask, orient, MAX_TIME_DELTA, MIN_TIME_DELTA, 3 );
if( !storage )
storage = cvCreateMemStorage(0);
else
cvClearMemStorage(storage);
// 将整个运动分割为独立的运动部分
// 分割运动:获取运动组件序列
// 分割掩码是运动组件图标识出来的，不再过多的使用
// segment motion: get sequence of motion components
// segmask is marked motion components map. It is not used further
seq = cvSegmentMotion(mhi, segmask, storage, timestamp, MAX_TIME_DELTA);
// 按运动组件的数目来循环
// 通过运动组件迭代
// 根据整幅图像(全局运动)进行相应的一次或多次迭代
// iterate through the motion components,
// One more iteration (i == -1) corresponds to the whole image (global motion)
for (i = -1; i < seq->total; i++)
{
if (i < 0)
{
// 全局运动事件
// case of the whole image
// 获取当前帧的范围
comp_rect = cvRect( 0, 0, size.width, size.height );
// 设置颜色为白色
color = CV_RGB(255,255,255);
// 设置放大倍数为100
magnitude = 100;
}
else
{
// 第i个运动组件
// i-th motion component
// 获取当前运动组件的范围
comp_rect = ((CvConnectedComp*)cvGetSeqElem( seq, i ))->rect;
// 丢弃很小的组件
if( comp_rect.width + comp_rect.height < 100 ) // reject very small components
continue;
// 设置颜色为红色
color = CV_RGB(255,0,0);
// 设置放大倍数为30
magnitude = 30;
}
// 选择组件感兴趣的区域
// select component ROI
cvSetImageROI( silh, comp_rect );
cvSetImageROI( mhi, comp_rect );
cvSetImageROI( orient, comp_rect );
cvSetImageROI( mask, comp_rect );
// 计算某些选择区域的全局运动方向
// 每个运动部件的运动方向就可以被这个函数利用提取的特定部件的掩模(mask)计算出来(使用cvCmp)
// 计算趋势
// calculate orientation
angle = cvCalcGlobalOrientation( orient, mask, mhi, timestamp, MHI_DURATION);
// 根据左上角的原点来调整图像的角度
angle = 360.0 - angle; // adjust for images with top-left origin
// 计算数组的绝对范数， 绝对差分范数或者相对差分范数
// 计算轮廓感兴趣区域中点的个数
count = cvNorm( silh, 0, CV_L1, 0 ); // calculate number of points within silhouette ROI
cvResetImageROI( mhi );
cvResetImageROI( orient );
cvResetImageROI( mask );
cvResetImageROI( silh );
// 检测小运动事件
// check for the case of little motion
if (count < comp_rect.width*comp_rect.height * 0.05)
{
continue;
}
// 画一个带箭头的时钟来指示方向
// draw a clock with arrow indicating the direction
center = cvPoint( (comp_rect.x + comp_rect.width/2),
(comp_rect.y + comp_rect.height/2) );
cvCircle( dst, center, cvRound(magnitude*1.2), color, 3, CV_AA, 0 );
cvLine( dst, center, cvPoint( cvRound( center.x + magnitude*cos(angle*CV_PI/180)),
cvRound( center.y - magnitude*sin(angle*CV_PI/180))), color, 3, CV_AA, 0 );
}
}
int testfaceLib_pThread ( const char* str_video, int trackerType, int multiviewType, int recognizerType, const char* str_facesetxml, int threads,
bool blink, bool smile, bool gender, bool age, bool recog, bool quiet, bool saveface, const char* sfolder, bool bEnableAutoCluster )
{
FILE* fp_imaginfo = fopen( "imaginfo.txt", "w" );
bool bAutoFocus = false;
IplImage *imgAutoFocus = NULL;
int sampleRate =1;
if(str_facesetxml == NULL)
str_facesetxml = "faceset_model.xml";
int prob_estimate[7];
char sState[256];
EnumViewAngle viewAngle = (EnumViewAngle)multiviewType;
//dynamic clustering for smooth ID registration
//bEnableAutoCluster = true;
CxlibFaceAnalyzer faceAnalyzer(viewAngle, (EnumTrackerType)trackerType, blink, smile, gender, age, recog, sampleRate, str_facesetxml, recognizerType, bEnableAutoCluster);
/////////////////////////////////////////////////////////////////////////////////////
// init GUI window
const char* str_title = "Face Tester";
if( ! quiet )
cvNamedWindow( str_title, CV_WINDOW_AUTOSIZE );
char sCaptionInfo[256] = "";
CvFont *pFont = new CvFont;
cvInitFont(pFont, CV_FONT_HERSHEY_PLAIN, 0.85, 0.85, 0, 1);
// load GUI smile icon images
IplImage *pImgSmileBGR;
IplImage *pImgSmileMask;
if(age == 0)
{ // smile icon
pImgSmileBGR = cvLoadImage( "smile.bmp" );
pImgSmileMask = cvLoadImage( "smilemask.bmp", 0 );
}
else
{ // gender/age/smile icons
pImgSmileBGR = cvLoadImage( "faceicon.bmp" );
pImgSmileMask = cvLoadImage( "faceiconMask.bmp", 0 );
}
IplImage *pImgSmileBGRA = cvCreateImage( cvSize(pImgSmileBGR->width, pImgSmileBGR->height), IPL_DEPTH_8U, 4 );
cvCvtColor(pImgSmileBGR, pImgSmileBGRA, CV_BGR2BGRA );
// open video source
size_t len = strlen( str_video );
bool is_piclist = (0 == stricmp( str_video + len - 4, ".txt" ));
CxImageSeqReader* vidcap = NULL;
if( is_piclist )
vidcap = new CxPicListReader( str_video );
else
vidcap = new CxVideoReader( str_video );
if( cvGetErrStatus() < 0 )
{
cvSetErrStatus( CV_StsOk );
return -1;
}
// when using camera, set to 640x480, 30fps
if( isdigit(str_video[0]) != 0 && str_video[1] == '\0' )
{
vidcap->width( 640 );
vidcap->height( 480 );
vidcap->fps( 30 );
}
// print beginning info
printf( "tracker cascade: '%s'\n", trackerType== TRA_HAAR ? "haar" : (recognizerType== TRA_SURF ? "surf" : "pf tracker SURF"));
printf( "face recognizer: '%s'\n", recognizerType == RECOGNIZER_BOOST_GB240 ? "boost gabor240" : "cascade gloh" );
printf( "video: '%s', %dx%d, %2.1f fps\n", str_video,
vidcap->width(), vidcap->height(), vidcap->fps() );
// set mouse event process
CxMouseParam mouse_faceparam;
mouse_faceparam.updated = false;
mouse_faceparam.play = true;
mouse_faceparam.ret_online_collecting = 0;
static const int MAX_FACES = 16;
if(! quiet)
{
mouse_faceparam.play = true;
mouse_faceparam.updated = false;
mouse_faceparam.face_num = faceAnalyzer.getMaxFaceNum();
mouse_faceparam.rects = faceAnalyzer.getFaceRects();
mouse_faceparam.image = NULL;
mouse_faceparam.cut_big_face= faceAnalyzer.getBigCutFace();
mouse_faceparam.typeRecognizer = 0;
mouse_faceparam.faceRecognizer = &faceAnalyzer;
mouse_faceparam.ret_online_collecting = 0;
cvSetMouseCallback( str_title, my_mouse_callback, (void*)&mouse_faceparam );
faceAnalyzer.setMouseParam(&mouse_faceparam);
}
// init count ticks
int64 ticks, start_ticks, total_ticks;
int64 tracker_total_ticks;
double tracker_fps, total_fps;
start_ticks = total_ticks = 0;
tracker_total_ticks = 0;
// loop for each frame of a video/camera
int frames = 0;
IplImage *pImg = NULL;
while( ! vidcap->eof() )
{
// capture a video frame
if( mouse_faceparam.play == true)
pImg = vidcap->query();
else
continue;
if ( pImg == NULL )
break;
// make a copy, flip if upside-down
CvImage image( cvGetSize(pImg), pImg->depth, pImg->nChannels );
if( pImg->origin == IPL_ORIGIN_BL ) //flip live camera's frame
cvFlip( pImg, image );
else
cvCopy( pImg, image );
// convert to gray_image for face analysis
CvImage gray_image( image.size(), image.depth(), 1 );
if( image.channels() == 3 )
cvCvtColor( image, gray_image, CV_BGR2GRAY );
else
cvCopy( image, gray_image );
///////////////////////////////////////////////////////////////////
// do face tracking and face recognition
start_ticks = ticks = cvGetTickCount();
if( is_piclist )
faceAnalyzer.detect(gray_image, prob_estimate, sState);
else
faceAnalyzer.track(gray_image, prob_estimate, sState, image); // track face in each frame but recognize by pthread
//faceAnalyzer.detect(gray_image, prob_estimate, sState);// track and recognizer face in each frame
int face_num = faceAnalyzer.getFaceNum();
ticks = cvGetTickCount() - ticks;
tracker_fps = 1000.0 / ( 1e-3 * ticks / cvGetTickFrequency() );
tracker_total_ticks += ticks;
//set param for mouse event processing
if(!quiet)
{
mouse_faceparam.face_num = face_num;
mouse_faceparam.image = image;
}
if( fp_imaginfo != NULL )
fprintf( fp_imaginfo, "%s %d", vidcap->filename(), face_num );
// blink/smile/gender/age/face recognize section
for( int i=0; i<face_num; i++ )
{
// get face rect and id from face tracker
CvRectItem rectItem = faceAnalyzer.getFaceRect(i);
CvRect rect = rectItem.rc;
int face_trackid = rectItem.fid;
float probSmile = faceAnalyzer.getFaceSmileProb(i);
int bBlink = faceAnalyzer.getFaceBlink(i);
int bSmile = faceAnalyzer.getFaceSmile(i);
int bGender = faceAnalyzer.getFaceGender(i);
int nAgeID = faceAnalyzer.getFaceAge(i);
int nFaceID = faceAnalyzer.getFaceID(i);
float fFaceProb= faceAnalyzer.getFaceProb(i);
char *sFaceCaption = NULL;
char sFaceNameBuff[256];
char *sFaceName = faceAnalyzer.getFaceName(i);
if(sFaceName[0] != '\0')
{
sprintf(sFaceNameBuff, "%s %.2f", sFaceName, fFaceProb);
sFaceCaption = sFaceName;
sFaceCaption = sFaceNameBuff;
}
if( ! quiet )
{
CvPoint2D32f *landmark6 = NULL;
sprintf(sCaptionInfo, "FPS:%04d, %s", (int)tracker_fps, sState);
int trackid = -1; //face_trackid , don't display trackid if -1
cxlibDrawFaceBlob( image, pFont, trackid, rect, landmark6, probSmile,
bBlink, bSmile, bGender, nAgeID, sFaceCaption, NULL,
pImgSmileBGR, pImgSmileBGRA, pImgSmileMask);
}
if( fp_imaginfo != NULL )
fprintf( fp_imaginfo, " %d %d %d %d", rect.x, rect.y, rect.width, rect.height );
}
if( fp_imaginfo != NULL )
fprintf( fp_imaginfo, "\n" );
///////////////////////////////////////////////////////////////////
total_ticks += (cvGetTickCount() - start_ticks);
// frame face_num
frames++;
//auto focus faces
if(quiet == false && bAutoFocus)
{
if(imgAutoFocus)
cvCopy(image, imgAutoFocus);
else
imgAutoFocus = cvCloneImage(image);
CvRectItem *rects = faceAnalyzer.getFaceRects();
cxlibAutoFocusFaceImage(imgAutoFocus, image, rects, face_num);
}
// next frame if quiet
if( quiet )
continue;
else
{
// draw status info for custom interaction
if(mouse_faceparam.ret_online_collecting == 1)
{
sprintf(sCaptionInfo, "Collecting faces for track_id = %d", mouse_faceparam.ret_facetrack_id);
//draw face collecting region
cvLine(image, cvPoint(image.width()/4, 0), cvPoint(image.width()/4, image.height()-1), CV_RGB(255,255,0), 2);
cvLine(image, cvPoint(image.width()*3/4, 0), cvPoint(image.width()*3/4, image.height()-1), CV_RGB(255,255,0), 2);
}
else
sprintf(sCaptionInfo, "FPS:%04d, %s", (int)tracker_fps, sState);
cxlibDrawCaption( image, pFont, sCaptionInfo);
}
//show Image
if (image.width() <= 800)
cvShowImage( str_title, image );
else
{ // display scaled smaller aimge
CvImage scale_image (cvSize(800, image.height()*800/image.width()), image.depth(), 3 );
cvResize (image, scale_image);
cvShowImage( str_title, scale_image );
}
// user interaction
int key = cvWaitKey(1);
//int key = cvWaitKey(0);
if( key == ' ' ) // press space bar to pause the video play
cvWaitKey( 0 );
else if( key == 27 ) // press 'esc' to exit
break;
else if( key == 'a' )
{ // add new face name
if(face_num > 0)
{
CvRect rect = faceAnalyzer.getFaceRect(0).rc;
int x = rect.x+rect.width/2;
int y = rect.y+rect.height/2;
addFaceSet( x, y, &mouse_faceparam);
}
}
else if( key == 'c' )
{ //enable flag to collect face exemplars for the selected face name
mouse_faceparam.ret_online_collecting = 1; //enable online face exemplar collecting
}
else if( key == 'z' )
bAutoFocus = !bAutoFocus;
else if(key >= 0)
{
if(mouse_faceparam.ret_online_collecting == 1)
{ // stop collecting face exemplars
mouse_faceparam.ret_online_collecting = 0; //disable online face exemplar collecting
mouse_faceparam.ret_facetrack_id = -1;
}
if( key == 's')
{
// save faceset xml model
faceAnalyzer.saveFaceModelXML("faceset_model.xml");
sprintf(sCaptionInfo, "%s", "saved the face model");
cxlibDrawCaption( pImg, pFont, sCaptionInfo);
cvShowImage( str_title, pImg );
cvWaitKey( 400 );
}
}
}
// print info about fps
float temp = 1e-6f / cvGetTickFrequency();
tracker_fps = 1.0f / ( tracker_total_ticks * temp / frames );
total_fps = 1.0f / (total_ticks * temp / frames);
printf( "Total frames:%d Speed:%.1f fps\n", frames, total_fps);
printf( "FPS: %.1f ", tracker_fps);
//save updated faceset model
if(mouse_faceparam.updated == true)
{
sprintf(sCaptionInfo, "%s", "press key 's' to save updated face model or other keys to cancel");
cxlibDrawCaption( pImg, pFont, sCaptionInfo);
cvShowImage( str_title, pImg );
int key = cvWaitKey();
if( key == 's')
faceAnalyzer.saveFaceModelXML("faceset_model.xml");
}
//save merged face model for dynamic clustering of smoothID
vFaceSet vMergedFaceSet;
int minWeight =10;
faceAnalyzer.getMergedFaceSet(vMergedFaceSet, minWeight);
faceAnalyzer.saveFaceModelXML("faceset_modelMerged.xml", &vMergedFaceSet);
//release global GUI data
if( !quiet )
cvDestroyWindow( str_title );
cvReleaseImage(&pImgSmileBGR);
cvReleaseImage(&pImgSmileBGRA);
cvReleaseImage(&pImgSmileMask);
delete pFont;
delete vidcap;
if( fp_imaginfo != NULL )
fclose( fp_imaginfo );
return 0;
}
int mainMatch(void)
{
// Initialise capture device
CvCapture* capture = cvCaptureFromCAM( CV_CAP_ANY );
if(!capture) error("No Capture");
// Declare Ipoints and other stuff
IpPairVec matches;
IpVec ipts, ref_ipts;
// This is the reference object we wish to find in video frame
// Replace the line below with IplImage *img = cvLoadImage("imgs/object.jpg");
// where object.jpg is the planar object to be located in the video
IplImage *img = cvLoadImage("imgs/object.jpg");
if (img == NULL) error("Need to load reference image in order to run matching procedure");
CvPoint src_corners[4] = {{0,0}, {img->width,0}, {img->width, img->height}, {0, img->height}};
CvPoint dst_corners[4];
// Extract reference object Ipoints
surfDetDes(img, ref_ipts, false, 3, 4, 3, 0.004f);
drawIpoints(img, ref_ipts);
showImage(img);
// Create a window
cvNamedWindow("OpenSURF", CV_WINDOW_AUTOSIZE );
// Main capture loop
while( true )
{
// Grab frame from the capture source
img = cvQueryFrame(capture);
// Detect and describe interest points in the frame
surfDetDes(img, ipts, false, 3, 4, 3, 0.004f);
// Fill match vector
getMatches(ipts,ref_ipts,matches);
// This call finds where the object corners should be in the frame
if (translateCorners(matches, src_corners, dst_corners))
{
// Draw box around object
for(int i = 0; i < 4; i++ )
{
CvPoint r1 = dst_corners[i%4];
CvPoint r2 = dst_corners[(i+1)%4];
cvLine( img, cvPoint(r1.x, r1.y),
cvPoint(r2.x, r2.y), cvScalar(255,255,255), 3 );
}
for (unsigned int i = 0; i < matches.size(); ++i)
drawIpoint(img, matches[i].first);
}
// Draw the FPS figure
drawFPS(img);
// Display the result
cvShowImage("OpenSURF", img);
// If ESC key pressed exit loop
if( (cvWaitKey(10) & 255) == 27 ) break;
}
// Release the capture device
cvReleaseCapture( &capture );
cvDestroyWindow( "OpenSURF" );
return 0;
}
int main()
{
// Initialize capturing live feed from the camera
CvCapture* capture = 0;
capture = cvCaptureFromCAM(0);
// Couldn't get a device? Throw an error and quit
if(!capture)
{
printf("Could not initialize capturing...\n");
return -1;
}
// The two windows we'll be using
cvNamedWindow("video");
cvNamedWindow("thresh");
// This image holds the "scribble" data...
// the tracked positions of the ball
IplImage* imgScribble = NULL;
// An infinite loop
while(true)
{
// Will hold a frame captured from the camera
IplImage* frame = 0;
frame = cvQueryFrame(capture);
// If we couldn't grab a frame... quit
if(!frame)
break;
// If this is the first frame, we need to initialize it
if(imgScribble == NULL)
{
imgScribble = cvCreateImage(cvGetSize(frame), 8, 3);
}
// Holds the yellow thresholded image (yellow = white, rest = black)
IplImage* imgYellowThresh = GetThresholdedImage(frame);
// Calculate the moments to estimate the position of the ball
CvMoments *moments = (CvMoments*)malloc(sizeof(CvMoments));
cvMoments(imgYellowThresh, moments, 1);
// The actual moment values
double moment10 = cvGetSpatialMoment(moments, 1, 0);
double moment01 = cvGetSpatialMoment(moments, 0, 1);
double area = cvGetCentralMoment(moments, 0, 0);
// Holding the last and current ball positions
static int posX = 0;
static int posY = 0;
int lastX = posX;
int lastY = posY;
posX = moment10/area;
posY = moment01/area;
// Print it out for debugging purposes
printf("position (%d,%d)\n", posX, posY);
// We want to draw a line only if its a valid position
if(lastX>0 && lastY>0 && posX>0 && posY>0)
{
// Draw a yellow line from the previous point to the current point
cvLine(imgScribble, cvPoint(posX, posY), cvPoint(lastX, lastY), cvScalar(0,255,255), 5);
}
// Add the scribbling image and the frame... and we get a combination of the two
cvAdd(frame, imgScribble, frame);
cvShowImage("thresh", imgYellowThresh);
cvShowImage("video", frame);
// Wait for a keypress
int c = cvWaitKey(10);
if(c!=-1)
{
// If pressed, break out of the loop
break;
}
// Release the thresholded image... we need no memory leaks.. please
cvReleaseImage(&imgYellowThresh);
delete moments;
}
// We're done using the camera. Other applications can now use it
cvReleaseCapture(&capture);
return 0;
}
CvRect SimpleHandDetector::get_feature(IplImage *img, FeatureData *feature)
{
IplImage* pFrame = img;
char* f = feature->ch;
#if DEBUG
cvZero(feature_img);
#endif
cvCvtColor(pFrame, YCrCb, CV_BGR2YCrCb);
cvInRangeS(YCrCb, lower, upper, skin);
//cvErode(skin,skin, 0, 1); //形态学滤波，除去噪声
//cvDilate(skin,skin, 0, 3);
cvSmooth(skin,skin);
//cvCopy(skin,hand);
cvClearMemStorage(storage);
CvSeq * contour = 0;
cvFindContours(skin, storage, &contour, sizeof (CvContour), CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cvPoint(0, 0));
if(contour == NULL)
return cvRect(-1,-1,-1,-1);
float max = fabs(cvContourArea(contour, CV_WHOLE_SEQ));
for (CvSeq* p = contour; p != NULL; p = p->h_next)
{
float now = fabs(cvContourArea(p, CV_WHOLE_SEQ));
if (now > max)
{
max = now;
contour = p;
}
}
CvRect rect = cvBoundingRect(contour, 0);
if (rect.width > STEP && rect.height > STEP)
{
//cvRectangle(hand, cvPoint(rect.x, rect.y), cvPoint(rect.x + rect.width, rect.y + rect.height), cvScalar(255, 255, 255), 3);
cvZero(hand);
cvDrawContours(hand, contour, CV_RGB(255, 255, 255), CV_RGB(255, 255, 255), 0, CV_FILLED,8, cvPoint(0, 0));
int w_s = rect.width / STEP;
int h_s = rect.height / STEP;
int w_off = rect.x;
int h_off = rect.y;
#if DEBUG
for(int s = 0;s <= STEP;s++)
{
cvLine(hand,cvPoint(rect.x,h_off),cvPoint(rect.x + rect.width,h_off),cvScalar(255),1);
h_off += h_s;
cvLine(hand,cvPoint(w_off,rect.y),cvPoint(w_off,rect.y + rect.height),cvScalar(255),1);
w_off += w_s;
}
#endif
w_s = rect.width / STEP;
h_s = rect.height / STEP;
int half = w_s * h_s;
for(int p = 0;p < STEP;p++)
{
for(int q = 0;q < STEP;q++)
{
int count = 0;
w_off = rect.x + q * w_s;
h_off = rect.y + p * h_s;
for(int y = 0;y < h_s;y++)
{
for(int x = 0;x < w_s;x++)
{
if(IMG8U(hand,w_off + x,h_off + y) == 255)
count++;
}
}
if((double)count / half > 0.5)
{
f[p * STEP + q] = '1';
#if DEBUG
cvSetImageROI(feature,cvRect(q * 100,p * 100,100,100));
cvSet(feature,cvScalar(255));
#endif
}else
f[p * STEP + q] = '0';
}
}
#if DEBUG
cvShowImage("hand",hand);
cvResetImageROI(feature_img);
cvShowImage("feature",feature_img);
cvWaitKey(10);
#endif
}else
rect = cvRect(-1,-1,-1,-1);
return rect;
/*char ch;
if((ch = cvWaitKey(10)) > 0)
{
if(is_train)
{
if(ch >= '0' && ch <= '9')
{
printf("%c:",ch);
for(int p = 0;p < step;p++)
{
for(int q = 0;q < step;q++)
printf("%c",f[p * step + q]);
printf("#\n");
}
}
else if(ch == 13 || ch == 27)
break;
}
else
{
if(ch == 13 || ch == 27)
break;
else
findHand(f);
}
}*/
}
void CvFunctionPlot::PlotLine(const CvPoint& Point1,const CvPoint& Point2,const CvScalar& PlotColour)
{
cvLine(Canvas,Point1,Point2, PlotColour,1,8,0);
}
void bw_track_blobs(StereoCluster *cluster)
{
int *free_blobs = (int *)calloc(cluster->data->NUM_OF_MARKERS,sizeof(int));
double *d_epi = (double *)calloc(cluster->data->NUM_OF_MARKERS,sizeof(int));
double *d_pred = (double *)calloc(cluster->data->NUM_OF_MARKERS,sizeof(int));
for(int k=0; k<cluster->data->NUM_OF_MARKERS; ++k)
{
free_blobs[k] = 1;
}
double nx,ny;
double lx,ly,lz;
double d_min;
double d_thresh = 5.0;
int id;
int d_cum;
double d;
CvPoint2D32f old_marker_pos;
/* compute distances between markers, if markers too close, no stable tracking possible */
for(int m=0; m<cluster->data->NUM_OF_MARKERS-1; ++m)
{
for(int n=m+1; n<cluster->data->NUM_OF_MARKERS; ++n)
{
d = sqrt( (cluster->tracker[1]->marker[m]->blob_pos.x-cluster->tracker[1]->marker[n]->blob_pos.x)*(cluster->tracker[1]->marker[m]->blob_pos.x-cluster->tracker[1]->marker[n]->blob_pos.x) +
(cluster->tracker[1]->marker[m]->blob_pos.y-cluster->tracker[1]->marker[n]->blob_pos.y)*(cluster->tracker[1]->marker[m]->blob_pos.y-cluster->tracker[1]->marker[n]->blob_pos.y) );
if(d<4.0)
{
cluster->tracker[1]->state = OFF_TRACK;
cluster->state = OFF_TRACK;
return;
}
}
}
for(int mc=0; mc<cluster->data->NUM_OF_MARKERS; ++mc)
{
/* read out pixel coordinates from color camera */
nx = cluster->tracker[0]->marker[mc]->pos_measured.x;
ny = cluster->tracker[0]->marker[mc]->pos_measured.y;
/* compute epipolar line for right image*/
lx = cluster->FundamentalMatrix[0][0]*nx + cluster->FundamentalMatrix[0][1]*ny + cluster->FundamentalMatrix[0][2];
ly = cluster->FundamentalMatrix[1][0]*nx + cluster->FundamentalMatrix[1][1]*ny + cluster->FundamentalMatrix[1][2];
lz = cluster->FundamentalMatrix[2][0]*nx + cluster->FundamentalMatrix[2][1]*ny + cluster->FundamentalMatrix[2][2];
lx /= ly;
lz /= ly;
ly = 1.0;
if(mc==0)
{
double x0,x1,y0,y1;
x0 = 0.0;
x1 = 779.0;
y0 = -(lx*x0+lz);
y1 = -(lx*x1+lz);
// printf("%f %f %f %f\n",x0,y0,x1,y1);
cvLine(cluster->tracker[1]->frame,cvPoint((int)x0,(int)y0),cvPoint((int)x1,(int)y1),cvScalarAll(255),1,CV_AA);
}
#if 0
if(cluster->tracker[1]->marker[mc]->pos_is_set)
{
old_marker_pos.x = cluster->tracker[1]->marker[mc]->pos_measured.x;
old_marker_pos.y = cluster->tracker[1]->marker[mc]->pos_measured.y;
// printf("m = (%f %f)\n",tracker->marker[nm]->pos_measured.x,tracker->marker[nm]->pos_measured.y);
if(cluster->tracker[1]->marker[mc]->vel_is_set)
{
/* if marker velocity is known predict new position */
cluster->tracker[1]->marker[mc]->pos_predicted.x = cluster->tracker[1]->marker[mc]->pos_measured.x + cluster->tracker[1]->marker[mc]->vel.x;
cluster->tracker[1]->marker[mc]->pos_predicted.y = cluster->tracker[1]->marker[mc]->pos_measured.y + cluster->tracker[1]->marker[mc]->vel.y;
}
else
{
/* otherwise take last known position for the center of the ROI as best guess */
cluster->tracker[1]->marker[mc]->pos_predicted.x = cluster->tracker[1]->marker[mc]->pos_measured.x;
cluster->tracker[1]->marker[mc]->pos_predicted.y = cluster->tracker[1]->marker[mc]->pos_measured.y;
}
for(int blob=0; blob<cluster->data->NUM_OF_MARKERS; ++blob)
{
/* distance of blob to epipolar line */
d_epi[blob] = abs( (lx*cluster->tracker[1]->marker[blob]->blob_pos.x + cluster->tracker[1]->marker[blob]->blob_pos.y + lz) / sqrt(1.0 + lx*lx) );
/*
d_pred[blob] = sqrt( (cluster->tracker[1]->marker[mc]->pos_predicted.x-cluster->tracker[1]->marker[blob]->blob_pos.x)*(cluster->tracker[1]->marker[mc]->pos_predicted.x-cluster->tracker[1]->marker[blob]->blob_pos.x) +
(cluster->tracker[1]->marker[mc]->blob_pos.y-cluster->tracker[1]->marker[blob]->pos_predicted.y)*(cluster->tracker[1]->marker[mc]->blob_pos.y-cluster->tracker[1]->marker[blob]->pos_predicted.y) );
*/
}
}
#endif
d_cum = 0;
d_min = 1e6;
id = 77;
for(int blob=0; blob<cluster->data->NUM_OF_MARKERS; ++blob)
{
/* distance of blob to epipolar line */
d_epi[blob] = abs( (lx*cluster->tracker[1]->marker[blob]->blob_pos.x + cluster->tracker[1]->marker[blob]->blob_pos.y + lz) / sqrt(1.0 + lx*lx) );
// printf("%f\n",d_epi[blob]);
if(d_epi[blob] < d_thresh)
{
d_cum++;
}
if(d_cum > 1)
{
cluster->state = OFF_TRACK;
cluster->tracker[1]->state = OFF_TRACK;
return;
}
if(d_epi[blob]<d_min)
{
d_min = d_epi[blob];
id = blob;
}
}
if(id == 77)
{
cluster->state = OFF_TRACK;
cluster->tracker[1]->state = OFF_TRACK;
return;
}
else
{
cluster->tracker[1]->marker[mc]->pos_measured.x = cluster->tracker[1]->marker[id]->blob_pos.x;
cluster->tracker[1]->marker[mc]->pos_measured.y = cluster->tracker[1]->marker[id]->blob_pos.y;
cluster->tracker[1]->marker[mc]->pos_is_set = 1;
}
}
cluster->tracker[1]->state = ON_TRACK;
cluster->state = ON_TRACK;
}
void main(int argc, char** argv)
{
cvNamedWindow("src",0 );
cvNamedWindow("warp image",0 );
cvNamedWindow("warp image (grey)",0 );
cvNamedWindow("Smoothed warped gray",0 );
cvNamedWindow("threshold image",0 );
cvNamedWindow("canny",0 );
cvNamedWindow("final",1 );
CvPoint2D32f srcQuad[4], dstQuad[4];
CvMat* warp_matrix = cvCreateMat(3,3,CV_32FC1);
float Z=1;
dstQuad[0].x = 216; //src Top left
dstQuad[0].y = 15;
dstQuad[1].x = 392; //src Top right
dstQuad[1].y = 6;
dstQuad[2].x = 12; //src Bottom left
dstQuad[2].y = 187;
dstQuad[3].x = 620; //src Bot right
dstQuad[3].y = 159;
srcQuad[0].x = 100; //dst Top left
srcQuad[0].y = 120;
srcQuad[1].x = 540; //dst Top right
srcQuad[1].y = 120;
srcQuad[2].x = 100; //dst Bottom left
srcQuad[2].y = 360;
srcQuad[3].x = 540; //dst Bot right
srcQuad[3].y = 360;
cvGetPerspectiveTransform(srcQuad, dstQuad, warp_matrix);
//CvCapture *capture = cvCaptureFromCAM(0);
/*double fps = cvGetCaptureProperty(capture, CV_CAP_PROP_FPS);
IplImage* image = cvRetrieveFrame(capture);
CvSize imgSize;
imgSize.width = image->width;
imgSize.height = image->height;
CvVideoWriter *writer = cvCreateVideoWriter("out.avi", CV_FOURCC('M', 'J', 'P', 'G'), fps, imgSize);*/
int ik=0;
while(1)
{
//IplImage* img = cvQueryFrame(capture);
IplImage* img = cvLoadImage( "../../Data/6 Dec/009.jpg", CV_LOAD_IMAGE_COLOR);
cvShowImage( "src", img );
//cvWriteFrame(writer, img);
//cvSaveImage(nameGen(ik++), img, 0);
IplImage* warp_img = cvCloneImage(img);
CV_MAT_ELEM(*warp_matrix, float, 2, 2) = Z;
cvWarpPerspective(img, warp_img, warp_matrix, CV_INTER_LINEAR | CV_WARP_INVERSE_MAP | CV_WARP_FILL_OUTLIERS);
cvShowImage( "warp image", warp_img );
IplImage* grayimg = cvCreateImage(cvGetSize(warp_img),IPL_DEPTH_8U,1);
cvCvtColor( warp_img, grayimg, CV_RGB2GRAY );
cvShowImage( "warp image (grey)", grayimg );
cvSmooth(grayimg, grayimg, CV_GAUSSIAN, 3, 3, 0.0, 0.0);
cvShowImage( "Smoothed warped gray", grayimg );
IplImage* thresholded_img=simplethreshold(grayimg, 220);
cvShowImage("threshold image",thresholded_img);
//grayimg = doCanny( thresholded_img, 50, 100, 3 );
grayimg = cvCloneImage(thresholded_img);
cvShowImage("canny",grayimg);
IplImage* finalimg = cvCreateImage(cvGetSize(grayimg),IPL_DEPTH_8U,3);
CvMemStorage* line_storage=cvCreateMemStorage(0);
CvSeq* results = cvHoughLines2(grayimg,line_storage,CV_HOUGH_PROBABILISTIC,10,CV_PI/180*5,350,100,10);
double angle = 0.0, temp;
double lengthSqd, wSum=0;
double xc = 0, yc = 0;
for( int i = 0; i < results->total; i++ )
{
CvPoint* line = (CvPoint*)cvGetSeqElem(results,i);
cvLine( finalimg, line[0], line[1], CV_RGB(0,0,255), 1, CV_AA, 0 );
//lengthSqd = (line[0].x - line[1].x)*(line[0].x - line[1].x) + (line[0].y - line[1].y)*(line[0].y - line[1].y);
wSum += 1;//lengthSqd;
if(line[0].y > line[1].y)
temp = atan((line[0].y - line[1].y + 0.0) / (line[0].x - line[1].x));
else
temp = atan((line[1].y - line[0].y + 0.0) / (line[1].x - line[0].x));
if(temp < 0)
angle += (90 + 180/3.14*temp)/* * lengthSqd*/;
else
angle += (180/3.14*temp - 90)/* * lengthSqd*/;
xc += line[0].x + line[1].x;
yc += line[0].y + line[1].y;
}
angle=angle/wSum;
//angle+=10;
printf("total: %d, angle: % f\n", results->total, angle);
xc /= 2*results->total;
yc /= 2*results->total;
double m = (angle != 0) ? 1/tan(angle*3.14/180) : 100; // 100 represents a very large slope (near vertical)
m=-m;
double x1, y1, x2, y2; // The Center Line
y1 = 0;
y2 = finalimg->height;
x1 = xc + (y1-yc)/m;
x2 = xc + (y2-yc)/m;
cvLine(finalimg, cvPoint(x1, y1), cvPoint(x2, y2), CV_RGB(0,255,0), 1, CV_AA, 0);
printf("point: %f\t%f\n", xc, yc);
double lx=0, ly=0, lm=0, lc=0, rx=0, ry=0, rm=0, rc=0;
for( int i = 0; i < results->total; i++ )
{
CvPoint* line = (CvPoint*)cvGetSeqElem(results,i);
double xm = (line[0].x + line[1].x)/2.0, ym = (line[0].y + line[1].y)/2.0;
if(ym - yc - m*(xm - xc) > 0)
{
lx += xm;
ly += ym;
lm += (line[1].y - line[0].y)/(line[1].x - line[0].x+0.0001);
lc++;
}
else
{
rx += xm;
ry += ym;
rm += (line[1].y - line[0].y)/(line[1].x - line[0].x+0.0001);
rc++;
}
}
// The Left Line
lx /= lc; ly /= lc; lm /= lc;
rx /= rc; ry /= rc; rm /= rc;
printf("lins: %f\t%f\t%f\n", lx, ly, lm);
printf("lins: %f\t%f\t%f\n", rx, ry, rm);
y1 = 0;
y2 = finalimg->height-5;
x1 = lx + (y1-ly)/lm;
x2 = lx + (y2-ly)/lm;
cvLine(finalimg, cvPoint(x1, y1), cvPoint(x2, y2), CV_RGB(255,255,0), 1, CV_AA, 0);
// The Right Line
y1 = 0;
y2 = finalimg->height-5;
x1 = rx + (y1-ry)/rm;
x2 = rx + (y2-ry)/rm;
cvLine(finalimg, cvPoint(x1, y1), cvPoint(x2, y2), CV_RGB(0,255,255), 1, CV_AA, 0);
// The Center Point
CvPoint vpt = cvPoint(finalimg->width/2, 416);
printf("center point: %d\t%d\n", vpt.x, vpt.y);
// The Dl and Dr
int dl = vpt.x - lx + (ly-vpt.y+0.0)/lm;
int dr = (vpt.y-ry+0.0)/rm + rx - vpt.x;
printf("dl-dr: %d\n", dl-dr);
cvShowImage("final",finalimg);
if(dl-dr < SAFEZONE_LL) // Assume that the bot lies just on the boundary of the safe zone
{
if(angle < -10)
{
navCommand(7, angle);
}
else
{
navCommand(7, angle);
}
}
else if(dl-dr > SAFEZONE_RL)
{
if(angle > 10)
{
navCommand(-7, angle);
}
else
{
navCommand(-7, angle);
}
}
else
{
if((angle < 10) && (angle > -10))
{
navCommand(angle, angle);
}
else
{
navCommand(0, angle);
}
}
cvWaitKey(0);
}
}
void Figure::DrawAxis(IplImage *output)
{
int bs = border_size;
int h = figure_size.height;
int w = figure_size.width;
// size of graph
int gh = h - bs * 2;
int gw = w - bs * 2;
// draw the horizontal and vertical axis
// let x, y axies cross at zero if possible.
float y_ref = y_min;
if ((y_max > 0) && (y_min <= 0))
y_ref = 0;
int x_axis_pos = h - bs - cvRound((y_ref - y_min) * y_scale);
cvLine(output, cvPoint(bs, x_axis_pos),
cvPoint(w - bs, x_axis_pos),
axis_color);
cvLine(output, cvPoint(bs, h - bs),
cvPoint(bs, h - bs - gh),
axis_color);
// Write the scale of the y axis
CvFont font;
cvInitFont(&font,CV_FONT_HERSHEY_PLAIN,0.55,0.7, 0,1,CV_AA);
int chw = 6, chh = 10;
char text[16];
// y max
if ((y_max - y_ref) > 0.05 * (y_max - y_min))
{
snprintf(text, sizeof(text)-1, "%.1f", y_max);
cvPutText(output, text, cvPoint(bs / 5, bs - chh / 2), &font, text_color);
}
// y min
if ((y_ref - y_min) > 0.05 * (y_max - y_min))
{
snprintf(text, sizeof(text)-1, "%.1f", y_min);
cvPutText(output, text, cvPoint(bs / 5, h - bs + chh), &font, text_color);
}
// x axis
snprintf(text, sizeof(text)-1, "%.1f", y_ref);
cvPutText(output, text, cvPoint(bs / 5, x_axis_pos + chh / 2), &font, text_color);
// Write the scale of the x axis
snprintf(text, sizeof(text)-1, "%.0f", x_max );
cvPutText(output, text, cvPoint(w - bs - strlen(text) * chw, x_axis_pos + chh),
&font, text_color);
// x min
snprintf(text, sizeof(text)-1, "%.0f", x_min );
cvPutText(output, text, cvPoint(bs, x_axis_pos + chh),
&font, text_color);
}
void MainWindow::optical_flow()
{
cvReleaseCapture(&pCapture);
CvCapture *pCapture = cvCaptureFromFile( "/home/kevin/optical_flow_input.avi" );
if (pCapture == NULL)
{
fprintf(stderr, "Error: Can't open video.\n");
}
/* Read the video's frame size out of the AVI. */
CvSize frame_size;
frame_size.height = (int) cvGetCaptureProperty( pCapture, CV_CAP_PROP_FRAME_HEIGHT );
frame_size.width = (int) cvGetCaptureProperty( pCapture, CV_CAP_PROP_FRAME_WIDTH );
/* Determine the number of frames in the AVI. */
long number_of_frames;
/* Go to the end of the AVI (ie: the fraction is "1") */
cvSetCaptureProperty( pCapture, CV_CAP_PROP_POS_AVI_RATIO, 1. );
/* Now that we're at the end, read the AVI position in frames */
number_of_frames = (int) cvGetCaptureProperty( pCapture, CV_CAP_PROP_POS_FRAMES );
/* Return to the beginning */
cvSetCaptureProperty( pCapture, CV_CAP_PROP_POS_FRAMES, 0. );
while(true)
{
static IplImage *frame = NULL, *frame1 = NULL, *frame1_1C = NULL;
static IplImage *frame2_1C = NULL, *eig_image = NULL, *temp_image = NULL;
static IplImage *pyramid1 = NULL, *pyramid2 = NULL;
cvSetCaptureProperty( pCapture, CV_CAP_PROP_POS_FRAMES, current_frame );
frame = cvQueryFrame( pCapture );
if (frame == NULL)
{
/* Why did we get a NULL frame? We shouldn't be at the end. */
fprintf(stderr, "Error: Hmm. The end came sooner than we thought.\n");
}
allocateOnDemand( &frame1_1C, frame_size, IPL_DEPTH_8U, 1 );
cvConvertImage(frame, frame1_1C, CV_CVTIMG_FLIP);
allocateOnDemand( &frame1, frame_size, IPL_DEPTH_8U, 3 );
cvConvertImage(frame, frame1, CV_CVTIMG_FLIP);
/* Get the second frame of video. Same principles as the first. */
frame = cvQueryFrame( pCapture );
if (frame == NULL)
{
fprintf(stderr, "Error: Hmm. The end came sooner than we thought.\n");
}
allocateOnDemand( &frame2_1C, frame_size, IPL_DEPTH_8U, 1 );
cvConvertImage(frame, frame2_1C, CV_CVTIMG_FLIP);
/* Preparation: Allocate the necessary storage. */
allocateOnDemand( &eig_image, frame_size, IPL_DEPTH_32F, 1 );
allocateOnDemand( &temp_image, frame_size, IPL_DEPTH_32F, 1 );
/* Preparation: This array will contain the features found in frame 1. */
CvPoint2D32f frame1_features[400];
int number_of_features;
number_of_features = 400;
cvGoodFeaturesToTrack(frame1_1C, eig_image, temp_image, frame1_features,
&number_of_features, .01, .01, NULL);
/* Pyramidal Lucas Kanade Optical Flow! */
/* This array will contain the locations of the points from frame 1 in frame 2. */
CvPoint2D32f frame2_features[400];
char optical_flow_found_feature[400];
float optical_flow_feature_error[400];
/* This is the window size to use to avoid the aperture problem (see slide "Optical Flow: Overview"). */
CvSize optical_flow_window = cvSize(3,3);
CvTermCriteria optical_flow_termination_criteria
= cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 );
/* This is some workspace for the algorithm.
* (The algorithm actually carves the image into pyramids of different resolutions.)
*/
allocateOnDemand( &pyramid1, frame_size, IPL_DEPTH_8U, 1 );
allocateOnDemand( &pyramid2, frame_size, IPL_DEPTH_8U, 1 );
cvCalcOpticalFlowPyrLK(frame1_1C, frame2_1C, pyramid1, pyramid2,
frame1_features, frame2_features, number_of_features,
optical_flow_window, 5, optical_flow_found_feature,
optical_flow_feature_error, optical_flow_termination_criteria, 0 );
/* For fun (and debugging :)), let's draw the flow field. */
for(int i = 0; i < number_of_features; i++)
{
/* If Pyramidal Lucas Kanade didn't really find the feature, skip it. */
if ( optical_flow_found_feature[i] == 0 ) continue;
int line_thickness;
line_thickness = 1;
/* CV_RGB(red, green, blue) is the red, green, and blue components
* of the color you want, each out of 255.
*/
CvScalar line_color;
line_color = CV_RGB(255,0,0);
CvPoint p,q;
p.x = (int) frame1_features[i].x;
p.y = (int) frame1_features[i].y;
q.x = (int) frame2_features[i].x;
q.y = (int) frame2_features[i].y;
double angle;
angle = atan2( (double) p.y - q.y, (double) p.x - q.x );
double hypotenuse;
hypotenuse = sqrt( square(p.y - q.y) + square(p.x - q.x) );
/* Here we lengthen the arrow by a factor of three. */
q.x = (int) (p.x - 3 * hypotenuse * cos(angle));
q.y = (int) (p.y - 3 * hypotenuse * sin(angle));
cvLine( frame1, p, q, line_color, line_thickness, CV_AA, 0 );
/* Now draw the tips of the arrow. I do some scaling so that the
* tips look proportional to the main line of the arrow.
*/
p.x = (int) (q.x + 9 * cos(angle + pi / 4));
p.y = (int) (q.y + 9 * sin(angle + pi / 4));
cvLine( frame1, p, q, line_color, line_thickness, CV_AA, 0 );
p.x = (int) (q.x + 9 * cos(angle - pi / 4));
p.y = (int) (q.y + 9 * sin(angle - pi / 4));
cvLine( frame1, p, q, line_color, line_thickness, CV_AA, 0 );
}
cv::Mat frame1_mat=MainWindow::image_rotate(frame1);
cv::flip(frame1_mat,frame1_mat,1);
IplImage dst = frame1_mat;
cv::Mat frame2_mat=MainWindow::image_rotate(frame2_1C);
cv::flip(frame2_mat,frame2_mat,1);
IplImage dst1 = frame2_mat;
MainWindow::Display(frame,&dst,&dst1);
int key_pressed;
key_pressed = cvWaitKey(0);
if (key_pressed == 'b' || key_pressed == 'B')
current_frame--;
else
current_frame++;
/* Don't run past the front/end of the AVI. */
if (current_frame < 0)
current_frame = 0;
if (current_frame >= number_of_frames - 1)
current_frame = number_of_frames - 2;
}
}
void MainWindow::OpencvOpticalFlow()
{
cvReleaseCapture(&pCapture);
pCapture=cvCaptureFromCAM(0);
IplImage* pre;
IplImage* next;
int corner_count=MAX_CORNERS;
while(1)
{
//
pre=cvQueryFrame(pCapture);
next=cvQueryFrame(pCapture);
CvSize img_sz=cvGetSize(pre);
IplImage* imgC=cvCreateImage(img_sz,IPL_DEPTH_8U,1);
//
IplImage* imgA=cvCreateImage(img_sz,IPL_DEPTH_8U,1);
IplImage* imgB=cvCreateImage(img_sz,IPL_DEPTH_8U,1);
cvCvtColor(pre,imgA,CV_BGR2GRAY);
cvCvtColor(next,imgB,CV_BGR2GRAY);
//
IplImage* eig_image=cvCreateImage(img_sz,IPL_DEPTH_32F,1);
IplImage* tmp_image=cvCreateImage(img_sz,IPL_DEPTH_32F,1);
CvPoint2D32f* cornersA = new CvPoint2D32f[ MAX_CORNERS ];
//
cvGoodFeaturesToTrack(
imgA,
eig_image,
tmp_image,
cornersA,
&corner_count,
0.01,
5.0,
0,
3,
0,
0.04
);
cvFindCornerSubPix(
imgA,
cornersA,
corner_count,
cvSize(win_size,win_size),
cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)
);
char features_found[ MAX_CORNERS ];
float feature_errors[ MAX_CORNERS ];
//
CvSize pyr_sz = cvSize( imgA->width+8, imgB->height/3 );
IplImage* pyrA = cvCreateImage( pyr_sz, IPL_DEPTH_32F, 1 );
IplImage* pyrB = cvCreateImage( pyr_sz, IPL_DEPTH_32F, 1 );
CvPoint2D32f* cornersB = new CvPoint2D32f[ MAX_CORNERS ];
cvCalcOpticalFlowPyrLK(
imgA,
imgB,
pyrA,
pyrB,
cornersA,
cornersB,
corner_count,
cvSize( win_size,win_size ),
5,
features_found,
feature_errors,
cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, .3 ),
0
);
for( int i=0; i<corner_count; i++ ) {
if( features_found[i]==0|| feature_errors[i]>550 ) {
// printf("Error is %f/n",feature_errors[i]);
continue;
}
// printf("Got it/n");
CvPoint p0 = cvPoint(
cvRound( cornersA[i].x ),
cvRound( cornersA[i].y )
);
CvPoint p1 = cvPoint(
cvRound( cornersB[i].x ),
cvRound( cornersB[i].y )
);
cvLine( imgC, p0, p1, CV_RGB(255,0,0),2 );
}
if(27==cvWaitKey(33))
break;
MainWindow::Display(imgA,imgB,imgC);
}
}
void MainWindow::OpticalFlowDetect()
{
cvReleaseCapture(&pCapture);
pCapture=cvCaptureFromCAM(0);
int corner_count = 1000;
CvTermCriteria criteria;
criteria = cvTermCriteria (CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 64, 0.01);
IplImage *src_img1;
IplImage *src_img2;
IplImage *dst_img;
IplImage *pre_img;
IplImage *result;
IplImage *eig_img;
IplImage *temp_img;
IplImage *prev_pyramid;
IplImage *curr_pyramid;
CvPoint2D32f *corners1;
CvPoint2D32f *corners2;
corners1 = (CvPoint2D32f *) cvAlloc (corner_count * sizeof (CvPoint2D32f));
corners2 = (CvPoint2D32f *) cvAlloc (corner_count * sizeof (CvPoint2D32f));
char *status;
status = (char *) cvAlloc (corner_count);
while (1)
{
pre_img = cvQueryFrame(pCapture);
CvSize img_sz = cvGetSize(pre_img);
src_img1 = cvCreateImage(img_sz, IPL_DEPTH_8U, 1);
cvCvtColor(pre_img, src_img1, CV_RGB2GRAY);
dst_img = cvQueryFrame(pCapture);
src_img2 = cvCreateImage(img_sz, IPL_DEPTH_8U, 1);
cvCvtColor(dst_img, src_img2, CV_RGB2GRAY);
result=cvCreateImage(img_sz,IPL_DEPTH_8U,1);
cvZero(result);
eig_img = cvCreateImage (img_sz, IPL_DEPTH_32F, 1);
temp_img = cvCreateImage (img_sz, IPL_DEPTH_32F, 1);
prev_pyramid = cvCreateImage (cvSize (src_img1->width + 8, src_img1->height / 3), IPL_DEPTH_8U, 1);
curr_pyramid = cvCreateImage (cvSize (src_img1->width + 8, src_img1->height / 3), IPL_DEPTH_8U, 1);
cvGoodFeaturesToTrack (src_img1, eig_img, temp_img, corners1, &corner_count, 0.001, 5, NULL);
cvCalcOpticalFlowPyrLK (src_img1, src_img2, prev_pyramid, curr_pyramid,
corners1, corners2, corner_count, cvSize (10, 10), 4, status, NULL, criteria, 0);
for (int i = 0; i < corner_count; i++)
{
if (status[i])
cvLine (dst_img, cvPointFrom32f (corners1[i]), cvPointFrom32f (corners2[i]), CV_RGB (255, 0, 0), 1, CV_AA, 0);
}
if(27==cvWaitKey(33))
break;
// cvCvtScale(dst_img,result,1.0/255,0);
MainWindow::Display(pre_img,src_img2,dst_img);
}
}
int main(int argc, char** argv)
{
profile_name = (argc > 1 ? argv[1] : (char*)"blue_goal.yml");
int cam = (argc > 2 ? atoi(argv[2]) : 0);
// value loading
fs = cvOpenFileStorage(profile_name, 0, CV_STORAGE_READ, NULL);
Hmax = cvReadIntByName(fs, NULL, "Hmax", Hmax);
Smax = cvReadIntByName(fs, NULL, "Smax", Smax);
Vmax = cvReadIntByName(fs, NULL, "Vmax", Vmax);
Hmin = cvReadIntByName(fs, NULL, "Hmin", Hmin);
Smin = cvReadIntByName(fs, NULL, "Smin", Smin);
Vmin = cvReadIntByName(fs, NULL, "Vmin", Vmin);
minH = cvReadIntByName(fs, NULL, "minH", minH);
cvNamedWindow("img", CV_WINDOW_AUTOSIZE);
cvNamedWindow("treshed", CV_WINDOW_AUTOSIZE);
cvNamedWindow("graph", CV_WINDOW_AUTOSIZE);
cvCreateTrackbar("Hmin", "treshed", &Hmin, 360, onTrack);
cvCreateTrackbar("Smin", "treshed", &Smin, 255, onTrack);
cvCreateTrackbar("Vmin", "treshed", &Vmin, 255, onTrack);
cvCreateTrackbar("Hmax", "treshed", &Hmax, 360, onTrack);
cvCreateTrackbar("Smax", "treshed", &Smax, 255, onTrack);
cvCreateTrackbar("Vmax", "treshed", &Vmax, 255, onTrack);
cvCreateTrackbar("minH", "treshed", &minH, 255, onTrack);
onTrack(0);
CvCapture* camera = cvCaptureFromCAM(cam);
while(1){
img = cvQueryFrame(camera);
allocateCvImage(&imgHSV, cvGetSize(img), 8, 3);
cvCvtColor(img, imgHSV, CV_BGR2HSV);
allocateCvImage(&imgThreshed, cvGetSize(img), 8, 1);
cvInRangeS(imgHSV, cvScalar(Hmin, Smin, Vmin, 0), cvScalar(Hmax,
Smax, Vmax, 0), imgThreshed);
cvErode(imgThreshed, imgThreshed, 0, 2);
int width = imgThreshed->width;
int height = imgThreshed->height;
int nchannels = imgThreshed->nChannels;
int step = imgThreshed->widthStep;
uchar* data = (uchar *)imgThreshed->imageData;
unsigned int graph[width];
int x,y;
for(x = 0; x < width ; x++)
graph[x] = 0;
int sum = 0, notnull = 0;
for(x = 0; x < width; x++){
for( y = 0 ; y < height ; y++ ) {
if(data[y*step + x*nchannels] == 255){
graph[x]++;
}
}
sum += graph[x];
if(graph[x] != 0)
notnull += 1;
// printf("%d\t%d\n", x, graph[x]);
}
if(notnull == 0)
notnull = 1;
int average = sum/notnull;
if(average == 0)
average = 1;
float pix = 12.0/average;
printf("\n sum: %d average: %d\n",sum,average);
int first = 0, last = 0;
// looking for goal
for(x = 0;x < width; x++){
if(graph[x] >= average && graph[x-1] < average){
cvLine(img, cvPoint(x, 0), cvPoint(x, height),
cvScalar(255, 255, 0, 0), 1, 0, 0);
if(first == 0)
first = x;
}
if(graph[x] >= average && graph[x+1] < average){
cvLine(img, cvPoint(x, 0), cvPoint(x, height),
cvScalar(255, 255, 0, 0), 1, 0, 0);
last = x;
}
}
float goal = pix*(last-first);
float error = (goal-60.0)/60.0*100.0;
printf("Pix: %f; Goal in cm: %f; Error: %f%\n", pix, goal, error);
// image center
cvLine(img, cvPoint(width/2, 0), cvPoint(width/2, height),
cvScalar(0, 255, 255, 0), 1, 0, 0);
int gCenter = (last+first) / 2;
// X
float X = ((width/2) - gCenter)*pix;
printf("X: %f +- %f\n",X, abs(error)/100.0*X);
// goal center
cvLine(img,cvPoint(gCenter, 0),cvPoint(gCenter, height),
cvScalar(0, 0, 255, 0), 1, 0, 0);
cvShowImage("img", img);
cvShowImage("treshed", imgThreshed);
//cvShowImage("graph", imgGraph);
cvWaitKey(10);
}
}
/*消除上下边界*/
void remove_border_ul(IplImage * img_plate)
{
int i = 0, j = 0;
/*这两个变量分别为上下边界的高度*/
int up_bound = -1, low_bound = -1;
int white_to_black = 0;
int black_to_white = 0;
/*从i从0 到高度一半进行遍历,进行投影,找到上边界*/
//cvNamedWindow("img_plate", 1);
//cvShowImage("img_plate", img_plate);
//cvWaitKey(0);
for (i = 0; i < (img_plate->height) / 2; i = i + 3) {
unsigned char * prow = (unsigned char *)(img_plate->imageData + i * img_plate->widthStep);
white_to_black = 0;
black_to_white = 0;
/*记录下每一行的black_to_white和w_to_b的个数*/
for (j = 0; j < img_plate->width; j = j + 3) {
if (prow[j] == 0 && prow[j + 3] == 255) {
black_to_white++;
} else if (prow[j] == 255 && prow[j + 3] == 0) {
white_to_black++;
}
}
/*设成6的话对图片的清晰度有很高的要求*/
if (black_to_white >= 6 && white_to_black >= 6 && up_bound < 0) {
up_bound = i;
} else if (black_to_white < 6 && white_to_black < 6 && up_bound > 0) {
// printf("black_to_white : %d whilte_to_black: %d , up_bound %d\n",black_to_white, white_to_black, up_bound);
up_bound = -1;
}
}
/*i从最底端到高度的一半进行遍历*/
for (i = img_plate->height - 1; i > (img_plate->height) / 2; i = i - 3) {
unsigned char * prow = (unsigned char *)(img_plate->imageData + i * img_plate->widthStep);
white_to_black = 0;
black_to_white = 0;
/*记录下每一行的black_to_white和w_to_b的个数*/
for (j = 0; j < img_plate->width; j = j + 3) {
if (prow[j] == 0 && prow[j + 3] == 255) {
black_to_white++;
} else if (prow[j] == 255 && prow[j + 3] == 0) {
white_to_black++;
}
}
if (black_to_white >= 6 && white_to_black >= 6 && low_bound < 0) {
low_bound = i;
} else if (black_to_white < 6 && white_to_black < 6 && low_bound > 0) {
low_bound = -1;
}
//printf("%d\n", low_bound);
}
#if 0
cvNamedWindow("img", 1);
printf("up_bound is %d, low_bound is %d\n", up_bound, low_bound);
/*画直线操作*/
/* void cvLine( CvArr* img, CvPoint pt1, CvPoint pt2, CvScalar color, int thickness=1, int line_type=8, int shift=0 );*/
cvLine(img_plate, cvPoint(0, up_bound), cvPoint(img_plate->width - 3, up_bound), CV_RGB(0xbF, 0xfd, 0xba), 3, 8, 0);
cvLine(img_plate, cvPoint(0, low_bound), cvPoint(img_plate->width - 3, low_bound), CV_RGB(0xbF, 0xfd, 0xba), 3, 8, 0);
cvShowImage("img", img_plate);
cvWaitKey(0);
#endif
/*这里容易出错!*/
//printf("%d %d %d %d", 0, up_bound, img_plate->width - 2, low_bound - up_bound - 2);
// printf("low_bound:%d up_bound:%d\n", low_bound, up_bound);
assert(low_bound >= 0 && up_bound >= 0);
cvSetImageROI(img_plate, cvRect(0, up_bound, img_plate->width - 2, low_bound - up_bound - 2)); /*-2保证不要越界*/
IplImage * tmp_img = cvCreateImage(cvSize(img_plate->width - 2, low_bound - up_bound - 2), img_plate->depth, img_plate->nChannels);
cvCopy(img_plate, tmp_img);
cvSaveImage("image/img_after_border_removed.bmp", tmp_img);
cvResetImageROI(img_plate);
// printf("setROI in remove bound success\n");
}
//============================================================================
void AAM_TDM::SaveSeriesTemplate(const CvMat* AllTextures, const AAM_PAW& m_warp)
{
LOGD("Saving the face template image...\n");
AAM_Common::MkDir("registration");
AAM_Common::MkDir("Modes");
AAM_Common::MkDir("Tri");
char filename[100];
int i;
for(i = 0; i < AllTextures->rows; i++)
{
CvMat oneTexture;
cvGetRow(AllTextures, &oneTexture, i);
sprintf(filename, "registration/%d.jpg", i);
m_warp.SaveWarpTextureToImage(filename, &oneTexture);
}
for(int nmodes = 0; nmodes < nModes(); nmodes++)
{
CvMat oneVar;
cvGetRow(__TextureEigenVectors, &oneVar, nmodes);
sprintf(filename, "Modes/A%03d.jpg", nmodes+1);
m_warp.SaveWarpTextureToImage(filename, &oneVar);
}
IplImage* templateimg = cvCreateImage
(cvSize(m_warp.Width(), m_warp.Height()), IPL_DEPTH_8U, 3);
IplImage* convexImage = cvCreateImage
(cvSize(m_warp.Width(), m_warp.Height()), IPL_DEPTH_8U, 3);
IplImage* TriImage = cvCreateImage
(cvSize(m_warp.Width(), m_warp.Height()), IPL_DEPTH_8U, 3);
m_warp.SaveWarpTextureToImage("Modes/Template.jpg", __MeanTexture);
m_warp.TextureToImage(templateimg, __MeanTexture);
cvSetZero(convexImage);
for(i = 0; i < m_warp.nTri(); i++)
{
CvPoint p, q;
int ind1, ind2;
cvCopy(templateimg, TriImage);
ind1 = m_warp.Tri(i, 0); ind2 = m_warp.Tri(i, 1);
p = cvPointFrom32f(m_warp.Vertex(ind1));
q = cvPointFrom32f(m_warp.Vertex(ind2));
cvLine(TriImage, p, q, CV_RGB(255, 255, 255));
cvLine(convexImage, p, q, CV_RGB(255, 255, 255));
ind1 = m_warp.Tri(i, 1); ind2 = m_warp.Tri(i, 2);
p = cvPointFrom32f(m_warp.Vertex(ind1));
q = cvPointFrom32f(m_warp.Vertex(ind2));
cvLine(TriImage, p, q, CV_RGB(255, 255, 255));
cvLine(convexImage, p, q, CV_RGB(255, 255, 255));
ind1 = m_warp.Tri(i, 2); ind2 = m_warp.Tri(i, 0);
p = cvPointFrom32f(m_warp.Vertex(ind1));
q = cvPointFrom32f(m_warp.Vertex(ind2));
cvLine(TriImage, p, q, CV_RGB(255, 255, 255));
cvLine(convexImage, p, q, CV_RGB(255, 255, 255));
sprintf(filename, "Tri/%03i.jpg", i+1);
cvSaveImage(filename, TriImage);
}
cvSaveImage("Tri/convex.jpg", convexImage);
cvReleaseImage(&templateimg);
cvReleaseImage(&convexImage);
cvReleaseImage(&TriImage);
}
void process_image()
{
int i, j;
int *inliers_index;
CvSize ellipse_axis;
CvPoint gaze_point;
static int lost_frame_num = 0;
Grab_Camera_Frames();
cvZero(ellipse_image);
cvSmooth(eye_image, eye_image, CV_GAUSSIAN, 5, 5);
Reduce_Line_Noise(eye_image);
if (save_image == 1) {
printf("save image %d\n", image_no);
sprintf(eye_file, "./Eye/Eye_%05d.jpg", image_no);
image_no++;
cvSaveImage(eye_file, eye_image);
}
//corneal reflection
remove_corneal_reflection(eye_image, threshold_image, (int)start_point.x, (int)start_point.y, cr_window_size,
(int)eye_image->height/10, corneal_reflection.x, corneal_reflection.y, corneal_reflection_r);
printf("corneal reflection: (%d, %d)\n", corneal_reflection.x, corneal_reflection.y);
Draw_Cross(ellipse_image, corneal_reflection.x, corneal_reflection.y, 15, 15, Yellow);
//starburst pupil contour detection
starburst_pupil_contour_detection((UINT8*)eye_image->imageData, eye_image->width, eye_image->height,
edge_threshold, rays, min_feature_candidates);
inliers_num = 0;
inliers_index = pupil_fitting_inliers((UINT8*)eye_image->imageData, eye_image->width, eye_image->height, inliers_num);
ellipse_axis.width = (int)pupil_param[0];
ellipse_axis.height = (int)pupil_param[1];
pupil.x = (int)pupil_param[2];
pupil.y = (int)pupil_param[3];
Draw_Cross(ellipse_image, pupil.x, pupil.y, 15, 15, Red);
cvLine(eye_image, pupil, corneal_reflection, Red, 4, 8);
cvLine(ellipse_image, pupil, corneal_reflection, Red, 4, 8);
printf("ellipse a:%lf; b:%lf, cx:%lf, cy:%lf, theta:%lf; inliers_num:%d\n\n",
pupil_param[0], pupil_param[1], pupil_param[2], pupil_param[3], pupil_param[4], inliers_num);
bool is_inliers = 0;
for (int i = 0; i < edge_point.size(); i++) {
is_inliers = 0;
for (int j = 0; j < inliers_num; j++) {
if (i == inliers_index[j])
is_inliers = 1;
}
stuDPoint *edge = edge_point.at(i);
if (is_inliers)
Draw_Cross(ellipse_image, (int)edge->x,(int)edge->y, 5, 5, Green);
else
Draw_Cross(ellipse_image, (int)edge->x,(int)edge->y, 3, 3, Yellow);
}
free(inliers_index);
if (ellipse_axis.width > 0 && ellipse_axis.height > 0) {
start_point.x = pupil.x;
start_point.y = pupil.y;
//printf("start_point: %d,%d\n", start_point.x, start_point.y);
Draw_Cross(eye_image, pupil.x, pupil.y, 10, 10, Green);
cvEllipse(eye_image, pupil, ellipse_axis, -pupil_param[4]*180/PI, 0, 360, Red, 2);
cvEllipse(ellipse_image, pupil, ellipse_axis, -pupil_param[4]*180/PI, 0, 360, Green, 2);
diff_vector.x = pupil.x - corneal_reflection.x;
diff_vector.y = pupil.y - corneal_reflection.y;
if (do_map2scene) {
gaze_point = homography_map_point(diff_vector);
printf("gaze_point: (%d,%d)\n", gaze_point.x, gaze_point.y);
Draw_Cross(scene_image, gaze_point.x, gaze_point.y, 60, 60, Red);
}
lost_frame_num = 0;
} else {
lost_frame_num++;
}
if (lost_frame_num > 5) {
start_point.x = FRAMEW/2;
start_point.y = FRAMEH/2;
}
Draw_Cross(ellipse_image, (int)start_point.x, (int)start_point.y, 7, 7, Blue);
Draw_Cross(eye_image, (int)start_point.x, (int)start_point.y, 7, 7, Blue);
if (save_ellipse == 1) {
printf("save ellipse %d\n", ellipse_no);
sprintf(ellipse_file, "./Ellipse/Ellipse_%05d.jpg", ellipse_no);
ellipse_no++;
cvSaveImage(ellipse_file, ellipse_image);
fprintf(ellipse_log, "%.3f\t %8.2lf %8.2lf %8.2lf %8.2lf %8.2lf\n",
Time_Elapsed(), pupil_param[0], pupil_param[1], pupil_param[2], pupil_param[3], pupil_param[4]);
}
printf("Time elapsed: %.3f\n", Time_Elapsed());
fprintf(logfile,"%.3f\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\n",
Time_Elapsed(),
pupil.x,
pupil.y,
corneal_reflection.x,
corneal_reflection.y,
diff_vector.x,
diff_vector.y,
gaze_point.x,
gaze_point.y);
if (view_cal_points) Show_Calibration_Points();
}
// Draw the graph of an array of floats into imageDst or a new image, between minV & maxV if given.
// Remember to free the newly created image if imageDst is not given.
IplImage* drawFloatGraph(const float *arraySrc, int nArrayLength, IplImage *imageDst, float minV, float maxV, int width, int height, char *graphLabel, bool showScale)
{
int w = width;
int h = height;
int b = 10; // border around graph within the image
if (w <= 20)
w = nArrayLength + b*2; // width of the image
if (h <= 20)
h = 220;
int s = h - b*2;// size of graph height
float xscale = 1.0;
if (nArrayLength > 1)
xscale = (w - b*2) / (float)(nArrayLength-1); // horizontal scale
IplImage *imageGraph; // output image
// Get the desired image to draw into.
if (!imageDst) {
// Create an RGB image for graphing the data
imageGraph = cvCreateImage(cvSize(w,h), 8, 3);
// Clear the image
cvSet(imageGraph, WHITE);
}
else {
// Draw onto the given image.
imageGraph = imageDst;
}
if (!imageGraph) {
std::cerr << "ERROR in drawFloatGraph(): Couldn't create image of " << w << " x " << h << std::endl;
exit(1);
}
CvScalar colorGraph = getGraphColor(); // use a different color each time.
// If the user didnt supply min & mav values, find them from the data, so we can draw it at full scale.
if (fabs(minV) < 0.0000001f && fabs(maxV) < 0.0000001f) {
for (int i=0; i<nArrayLength; i++) {
float v = (float)arraySrc[i];
if (v < minV)
minV = v;
if (v > maxV)
maxV = v;
}
}
float diffV = maxV - minV;
if (diffV == 0)
diffV = 0.00000001f; // Stop a divide-by-zero error
float fscale = (float)s / diffV;
// Draw the horizontal & vertical axis
int y0 = cvRound(minV*fscale);
cvLine(imageGraph, cvPoint(b,h-(b-y0)), cvPoint(w-b, h-(b-y0)), BLACK);
cvLine(imageGraph, cvPoint(b,h-(b)), cvPoint(b, h-(b+s)), BLACK);
// Write the scale of the y axis
CvFont font;
cvInitFont(&font,CV_FONT_HERSHEY_PLAIN,0.55,0.7, 0,1,CV_AA); // For OpenCV 1.1
if (showScale) {
//cvInitFont(&font,CV_FONT_HERSHEY_PLAIN,0.5,0.6, 0,1, CV_AA); // For OpenCV 2.0
CvScalar clr = GREY;
char text[16];
sprintf_s(text, sizeof(text)-1, "%.1f", maxV);
cvPutText(imageGraph, text, cvPoint(1, b+4), &font, clr);
// Write the scale of the x axis
sprintf_s(text, sizeof(text)-1, "%d", (nArrayLength-1) );
cvPutText(imageGraph, text, cvPoint(w-b+4-5*strlen(text), (h/2)+10), &font, clr);
}
// Draw the values
CvPoint ptPrev = cvPoint(b,h-(b-y0)); // Start the lines at the 1st point.
for (int i=0; i<nArrayLength; i++) {
int y = cvRound((arraySrc[i] - minV) * fscale); // Get the values at a bigger scale
int x = cvRound(i * xscale);
CvPoint ptNew = cvPoint(b+x, h-(b+y));
cvLine(imageGraph, ptPrev, ptNew, colorGraph, 1, CV_AA); // Draw a line from the previous point to the new point
ptPrev = ptNew;
}
// Write the graph label, if desired
if (graphLabel != NULL && strlen(graphLabel) > 0) {
//cvInitFont(&font,CV_FONT_HERSHEY_PLAIN, 0.5,0.7, 0,1,CV_AA);
cvPutText(imageGraph, graphLabel, cvPoint(30, 10), &font, CV_RGB(0,0,0)); // black text
}
return imageGraph;
}
void MaizeDetector::processLaserScan(
const sensor_msgs::LaserScan::ConstPtr& laser_scan) {
float rthetamean = 0, rrhomean = 0, lthetamean = 0, lrhomean = 0, theta = 0, rho = 0;
double x0 = 0, y0 = 0, a, b;
int lmc = 0, rmc = 0;
static int right_count = 0;
static int left_count = 0;
clearRawImage();
tf::TransformListener listener_;
sensor_msgs::PointCloud cloud;
try
{
projector_.projectLaser(*laser_scan, cloud);
// projector_.transformLaserScanToPointCloud("base_link",*laser_scan,cloud,listener_);
}
catch (tf::TransformException& e)
{
std::cout << e.what();
return;
}
//ROS_INFO("Got it right");
int size = cloud.points.size();
for (int i = 0; i < size; i++) {
//if (((abs(cloud.points[i].y))-((cloud.points[i].x-300)*0.5))<0.65) {
if (abs((cloud.points[i].y))<0.65 && cloud.points[i].x >0 ) {
point1.x = ((int)(cloud.points[i].x * 50) + 300);
point1.y = ((int)(cloud.points[i].y * 50) + 300);
point2.x = point1.x-4;
point2.y = point1.y;
cvLine(rawData_, point1, point2, CV_RGB(255,255,255), 3, CV_AA, 0);
}
}
cvCvtColor(rawData_, workingImg_, CV_BGR2GRAY);
cvDilate(rawData_,rawData_,NULL,6);
cvErode(rawData_,rawData_,NULL,4);
lines_ = cvHoughLines2(workingImg_, storage_, CV_HOUGH_STANDARD, 1, CV_PI/ 180, 60, 0, 0);
//lines_ = cvHoughLines2(workingImg_, storage_, CV_HOUGH_PROBABILISTIC, 1, CV_PI/360, 30, 10, 30);
for(int i = 0; i < MIN(lines_->total,20); i++){
//for (int i = 0; i < MIN(lines_->total,15); i++) {
float* line = (float*) cvGetSeqElem(lines_, i);
rho = line[0];
theta = line[1];
a = cos(theta);
b = sin(theta);
x0 = a * rho;
y0 = b * rho;
point1.x = cvRound(x0 + 600 * (-b));
point1.y = cvRound(y0 + 600 * (a));
point2.x = cvRound(x0 - 600 * (-b));
point2.y = cvRound(y0 - 600 * (a));
point3.x = 300, point3.y = 300;
point4.x = 300, point4.y = 600;
point5.x = 300, point5.y = 0;
//cvLine(rawData_, point1, point2, CV_RGB(255,0,0), 1, CV_AA,0);
cvLine(rawData_, point3, point4, CV_RGB(0,0,255), 1, CV_AA, 0);
cvLine(rawData_, point3, point5, CV_RGB(0,0,255), 1, CV_AA, 0);
if (intersect(point1, point2, point3, point4)) {
{
rrhomean += rho;
rthetamean += theta;
rmc++;
//cvLine(workingImg_, point1, point2, CV_RGB(0,0,255), 1, CV_AA,0);
}
} else if (intersect(point1, point2, point3, point5)) {
{
lrhomean += rho;
lthetamean += theta;
lmc++;
//cvLine(workingImg_, point1, point2, CV_RGB(255,255,255), 1,CV_AA, 0);
}
}
}
theta = lthetamean / lmc;
rho = lrhomean / lmc;
a = cos(theta);
b = sin(theta);
x0 = a * rho;
y0 = b * rho;
point1.x = cvRound(x0 + 600 * (-b)), point1.y = cvRound(y0 + 600 * (a));
point2.x = cvRound(x0 - 600 * (-b)), point2.y = cvRound(y0 - 600 * (a));
//cvLine(rawData_, point1, point2, CV_RGB(255,0,0), 3, CV_AA, 0);
point4.x = 300;
point4.y = 300;
point5.x = point4.x + 800 * sin(CV_PI - (theta - CV_PI / 2));
point5.y = point4.y + 800 * cos(CV_PI - (theta - CV_PI / 2));
cvLine(rawData_, point5, point4, CV_RGB(255,255,255), 1, CV_AA, 0);
rows_.header.stamp = ros::Time::now();
rows_.leftvalid = false;
rows_.rightvalid = false;
//detect valid lines
if (intersect(point1, point2, point4, point5)) {
right_distance_ = sqrt(((intersection_.y - 300) * (intersection_.y- 300)) + ((intersection_.x - 300) * (intersection_.x - 300)))* 2;
right_angle_ = (theta) - CV_PI / 2;
right_count++;
right_angle_rolling_mean_[right_count%10] = right_angle_;
right_dist_rolling_mean_[right_count%10] = right_distance_;
right_angle_ = 0;
right_distance_ = 0;
for(int i=0;i < 10;i++){
right_angle_ += right_angle_rolling_mean_[i];
right_distance_ += right_dist_rolling_mean_[i];
}
right_angle_ = right_angle_/10;
right_distance_ = right_distance_ /10;
ROS_WARN("right_distance_: %f",right_distance_);
cvLine(rawData_, point1, point2, CV_RGB(0,255,0), 1, CV_AA, 0);
marker_r.header.frame_id = "/laser_front_link";
marker_r.header.stamp = ros::Time::now();
marker_r.ns = "kf_shapes";
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
marker_r.type = visualization_msgs::Marker::CUBE;
// Set the marker action. Options are ADD and DELETE
marker_r.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
marker_r.id = 2;
geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(right_angle_);
marker_r.pose.position.x = 0;
marker_r.pose.position.y = -((float) right_distance_) / 100;
marker_r.pose.position.z = 0;
marker_r.pose.orientation = pose_quat;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
marker_r.scale.x = 10.0;
marker_r.scale.y = 0.1;
marker_r.scale.z = 0.5;
marker_r.color.r = 0.0f;
marker_r.color.g = 1.0f;
marker_r.color.b = 0.0f;
marker_r.color.a = 0.5;
marker_r.lifetime = ros::Duration(.1);
marker_r_pub.publish(marker_r);
// Set the color -- be sure to set alpha to something non-zero!
// Publish the marker
rows_.rightvalid = true;
rows_.rightdistance = ((float) right_distance_) / 100;
rows_.rightangle = right_angle_;
}else{
rows_.rightvalid = false;
rows_.rightdistance = 0;
rows_.rightangle = 0;
}
theta = rthetamean / rmc;
rho = rrhomean / rmc;
a = cos(theta);
b = sin(theta);
x0 = a * rho;
y0 = b * rho;
point1.x = cvRound(x0 + 600 * (-b)), point1.y = cvRound(y0 + 600 * (a));
point2.x = cvRound(x0 - 600 * (-b)), point2.y = cvRound(y0 - 600 * (a));
point4.x = 300;
point4.y = 300;
point5.x = point4.x - 800 * sin(CV_PI - (theta - CV_PI / 2));
point5.y = point4.y - 800 * cos(CV_PI - (theta - CV_PI / 2));
cvLine(rawData_, point5, point4, CV_RGB(255,255,255), 1, CV_AA, 0);
//detect valid lines
if (intersect(point1, point2, point4, point5)) {
left_distance_ = sqrt(((intersection_.y - 300) * (intersection_.y- 300)) + ((intersection_.x - 300) * (intersection_.x - 300)))* 2;
left_angle_ = (theta) - CV_PI / 2;
left_count++;
left_angle_rolling_mean_[left_count%10] = left_angle_;
left_dist_rolling_mean_[left_count%10] = left_distance_;
left_angle_ = 0;
left_distance_ = 0;
for(int i=0;i < 10;i++){
left_angle_ += left_angle_rolling_mean_[i];
left_distance_ += left_dist_rolling_mean_[i];
}
left_angle_ = left_angle_/10;
left_distance_ = left_distance_ /10;
ROS_WARN("left_distance_: %f",left_distance_);
marker_r.id = 1;
geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(left_angle_);
marker_r.color.r = 0.0f;
marker_r.color.g = 0.0f;
marker_r.color.b = 1.0f;
marker_r.pose.position.x = 0;
marker_r.pose.position.y = ((float) left_distance_) / 100;
marker_r.pose.position.z = 0;
marker_r.pose.orientation = pose_quat;
marker_r_pub.publish(marker_r);
//rolling_mean_[count%10] = right_angle_;
//left_angle_ = 0;
//for(int i=0;i < 10;i++){
// left_angle_ += rolling_mean_[i];
//}
//right_angle_ = right_angle_/10;
cvLine(rawData_, point1, point2, CV_RGB(0,255,255), 1, CV_AA, 0);
rows_.leftvalid = true;
rows_.leftdistance = ((float) left_distance_) / 100;
rows_.leftangle = left_angle_;
}else{
rows_.leftvalid = false;
rows_.leftdistance = 0;
rows_.leftangle = 0;
}
if (rows_.leftvalid && rows_.rightvalid){
e_angle = (rows_.leftangle + rows_.rightangle) / 2;
e_distance = (rows_.leftdistance - rows_.rightdistance);
row_dist_est =( (rows_.leftdistance + rows_.rightdistance) + row_dist_est)/2;
ROS_INFO("2ROWS row_dist_est %f, e_dist %f",row_dist_est,e_distance);
rows_.error_angle = e_angle;
rows_.error_distance = e_distance;
rows_.var = 5^2;
marker_r.id = 3;
geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(e_angle);
marker_r.color.r = 0.0f;
marker_r.color.g = 1.0f;
marker_r.color.b = 1.0f;
marker_r.pose.orientation = pose_quat;
marker_r.pose.position.x = 0;
marker_r.pose.position.y = (e_distance);
marker_r.pose.position.z = 0;
marker_r_pub.publish(marker_r);
}else if (rows_.leftvalid && !rows_.rightvalid){
e_angle = (rows_.leftangle);
e_distance = 0;//row_dist_est/2-(rows_.leftdistance);
//ROS_INFO("e_angle %f, e_dist %f",e_angle,e_distance);
rows_.error_angle = e_angle;
rows_.error_distance = e_distance;//e_distance-(0.75/2);
rows_.var = 10^2;
marker_r.id = 3;
ROS_INFO("LEFTROW row_dist_est %f, e_dist %f",row_dist_est,e_distance);
geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(e_angle);
marker_r.color.r = 0.0f;
marker_r.color.g = 1.0f;
marker_r.color.b = 1.0f;
marker_r.pose.orientation = pose_quat;
marker_r.pose.position.x = 0;
marker_r.pose.position.y = (e_distance);
marker_r.pose.position.z = 0;
marker_r_pub.publish(marker_r);
}else if (!rows_.leftvalid && rows_.rightvalid){
e_angle = (rows_.rightangle);
e_distance = 0;//row_dist_est/2-(rows_.rightdistance);
//ROS_INFO("e_angle %f, e_dist %f",e_angle,e_distance);
ROS_INFO("LRIGHTROW row_dist_est %f, e_dist %f",row_dist_est,e_distance);
rows_.error_angle = (0.75/2)-e_angle;
rows_.error_distance = e_distance;//e_distance-(0.75/2);
rows_.var = 10^2;
marker_r.id = 3;
geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(e_angle);
marker_r.color.r = 0.0f;
marker_r.color.g = 1.0f;
marker_r.color.b = 1.0f;
marker_r.pose.orientation = pose_quat;
marker_r.pose.position.x = 0;
marker_r.pose.position.y = (e_distance);
marker_r.pose.position.z = 0;
marker_r_pub.publish(marker_r);
}else{
e_angle = 0;
e_distance = 0;
ROS_INFO("e_angle %f, e_dist %f",e_angle,e_distance);
ROS_INFO("NOROW row_dist_est %f, e_dist %f",row_dist_est,e_distance);
rows_.error_angle = e_angle;
rows_.error_distance = e_distance;
rows_.var = 4000^2;
marker_r.id = 3;
geometry_msgs::Quaternion pose_quat = tf::createQuaternionMsgFromYaw(e_angle);
marker_r.color.r = 0.0f;
marker_r.color.g = 1.0f;
marker_r.color.b = 1.0f;
marker_r.pose.orientation = pose_quat;
marker_r.pose.position.x = 0;
marker_r.pose.position.y = (e_distance);
marker_r.pose.position.z = 0;
marker_r_pub.publish(marker_r);
}
//cvLine(rawData_, cvPoint(0,300+150), cvPoint(600,300+150), CV_RGB(255,255,255), 1, CV_AA, 0);
//cvLine(rawData_, cvPoint(0,300-150), cvPoint(600,300-150), CV_RGB(255,255,255), 1, CV_AA, 0);
row_pub.publish(rows_);
//cvShowImage("TEST", rawData_);
//cvWaitKey(10);
//pc_pub.publish(cloud);
}
int testfaceLib_sThread ( const char* str_video, int trackerType, int multiviewType, int recognizerType, const char* str_facesetxml, int threads,
bool blink, bool smile, bool gender, bool age, bool recog, bool quiet, bool saveface, const char* sfolder, bool bEnableAutoCluster)
{
int faceimgID = 0;
char driver[8];
char dir[1024];
char fname[1024];
char ext[8];
char sImgPath[1024];
if(sfolder)
{
char sysCommand[128];
sprintf (sysCommand, "mkdir %s", sfolder);
system (sysCommand);
sprintf(sImgPath, "%s//%s", sfolder, "imaginfo.txt");
sprintf(fname, "%s//%s", sfolder, "faceinfo.txt");
}
else
{
sprintf(sImgPath, "%s", "imaginfo.txt");
sprintf(fname, "%s", "faceinfo.txt");
}
FILE* fp_imaginfo = fopen( sImgPath, "wt" );
FILE* fp_faceinfo = fopen( fname, "wt" );
bool bAutoFocus = false;
IplImage *imgAutoFocus = NULL;
/////////////////////////////////////////////////////////////////////////////////////
// init GUI window
const char* str_title = "Face Tester";
if( ! quiet )
cvNamedWindow( str_title, CV_WINDOW_AUTOSIZE );
char sCaptionInfo[256]="";
CvFont *pFont = new CvFont;
cvInitFont(pFont, CV_FONT_HERSHEY_PLAIN, 0.85, 0.85, 0, 1);
// load GUI smile icon images
IplImage *pImgSmileBGR;
IplImage *pImgSmileMask;
if(age == 0)
{
pImgSmileBGR = cvLoadImage( "smile.bmp" );
pImgSmileMask = cvLoadImage( "smilemask.bmp", 0 );
}
else
{
pImgSmileBGR = cvLoadImage( "faceicon.bmp" );
pImgSmileMask = cvLoadImage( "faceiconMask.bmp", 0 );
}
IplImage *pImgSmileBGRA = cvCreateImage( cvSize(pImgSmileBGR->width, pImgSmileBGR->height), IPL_DEPTH_8U, 4 );
cvCvtColor(pImgSmileBGR, pImgSmileBGRA, CV_BGR2BGRA );
// open video source
size_t len = strlen( str_video );
bool is_piclist = (0 == stricmp( str_video + len - 4, ".txt" ));
CxImageSeqReader* vidcap = NULL;
if( is_piclist )
vidcap = new CxPicListReader( str_video );
else
vidcap = new CxVideoReader( str_video );
if( cvGetErrStatus() < 0 )
{
cvSetErrStatus( CV_StsOk );
return -1;
}
// when using camera, set to 640x480, 30fps
if( isdigit(str_video[0]) != 0 && str_video[1] == '\0' )
{
vidcap->width( 640 );
vidcap->height( 480 );
vidcap->fps( 30 );
}
// print beginning info
printf( "tracker cascade: '%s'\n", trackerType == TRA_HAAR ? "haar" : (trackerType== TRA_SURF ? "surf" : "pf tracker SURF"));
printf( "face recognizer: '%s'\n", recognizerType == RECOGNIZER_BOOST_GB240 ? "boost gabor240" : "cascade gloh" );
printf( "video: '%s', %dx%d, %2.1f fps\n", str_video,
vidcap->width(), vidcap->height(), vidcap->fps() );
// config face tracker
const int face_max = 16;
CvRectItem rects[face_max];
tagDetectConfig configParam;
EnumViewAngle viewAngle = (EnumViewAngle)multiviewType;
CxlibFaceDetector detector;
detector.init(viewAngle, (EnumFeaType)trackerType);
detector.config( configParam );
CxlibFaceTracker tracker;
tracker.init(viewAngle, (EnumTrackerType)trackerType);
tracker.config( configParam, TR_NLEVEL_3 );
if( cvGetErrStatus() < 0 )
{
cvSetErrStatus( CV_StsOk );
return -1;
}
// config landmark detector
CvPoint2D32f landmark6[6+1]; // consider both 6-pt and 7-pt
float parameters[16];
bool bLandmark = false;
CxlibLandmarkDetector landmarkDetector(LDM_6PT);
int size_smallface = 64;
int size_bigface = 128;
CxlibAlignFace cutFace(size_smallface, size_bigface);
// config blink/smile/gender detector
int bBlink = 0, bSmile = 0, bGender = 0, bAge = 0; //+1, -1, otherwise 0: no process
float probBlink = 0, probSmile = 0, probGender = 0, probAge[4];
int nAgeID = 0;
CxlibBlinkDetector blinkDetector(size_smallface);
CxlibSmileDetector smileDetector(size_smallface);
CxlibGenderDetector genderDetector(size_smallface);
CxlibAgeDetector ageDetector(size_bigface);
// config face recognizer
float probFaceID = 0;
if(str_facesetxml == NULL)
str_facesetxml = "faceset_model.xml";
CxlibFaceRecognizer faceRecognizer( size_bigface, recognizerType );
if(recog) faceRecognizer.loadFaceModelXML(str_facesetxml);
// set mouse event process
CxMouseParam mouse_faceparam;
mouse_faceparam.updated = false;
mouse_faceparam.play = true;
mouse_faceparam.ret_online_collecting = 0;
if(! quiet)
{
mouse_faceparam.face_num = face_max;
mouse_faceparam.rects = rects;
mouse_faceparam.image = NULL;
mouse_faceparam.cut_big_face= cutFace.getBigCutFace();
mouse_faceparam.typeRecognizer = 1;
mouse_faceparam.faceRecognizer = &faceRecognizer;
cvSetMouseCallback( str_title, my_mouse_callback, (void*)&mouse_faceparam );
}
// init count ticks
int64 ticks, start_ticks, total_ticks;
int64 tracker_total_ticks, landmark_total_ticks, align_total_ticks,
blink_total_ticks, smile_total_ticks, gender_total_ticks, age_total_ticks, recg_total_ticks;
double frame_fps, tracker_fps, landmark_fps, align_fps, blink_fps, smile_fps, gender_fps, age_fps, recg_fps, total_fps;
start_ticks = total_ticks = 0;
tracker_total_ticks = landmark_total_ticks = align_total_ticks = 0;
blink_total_ticks = smile_total_ticks = gender_total_ticks = age_total_ticks = recg_total_ticks = 0;
tracker_fps = landmark_fps = align_fps = blink_fps = smile_fps = gender_fps = age_fps = recg_fps = total_fps = 0.0;
// loop for each frame of a video/camera
int frames = 0;
IplImage *pImg = NULL;
int print_faceid=-1;
float print_score = 0;
std::string print_facename;
bool bRunLandmark = blink || smile|| gender|| age|| recog || saveface;
IplImage *thumbnailImg = cvCreateImage(cvSize(THUMBNAIL_WIDTH, THUMBNAIL_HEIGHT), IPL_DEPTH_8U, 3);
//dynamic clustering for smooth ID registration
//bEnableAutoCluster = true;
if( is_piclist ) bEnableAutoCluster = false;
while( ! vidcap->eof() )
{
// capture a video frame
if( mouse_faceparam.play == true)
pImg = vidcap->query();
else
continue;
if ( pImg == NULL )
continue;
// make a copy, flip if upside-down
CvImage image( cvGetSize(pImg), pImg->depth, pImg->nChannels );
if( pImg->origin == IPL_ORIGIN_BL ) //flip live camera's frame
cvFlip( pImg, image );
else
cvCopy( pImg, image );
// convert to gray_image for face analysis
CvImage gray_image( image.size(), image.depth(), 1 );
if( image.channels() == 3 )
cvCvtColor( image, gray_image, CV_BGR2GRAY );
else
cvCopy( image, gray_image );
// do face tracking
start_ticks = ticks = cvGetTickCount();
int face_num = 0;
if( is_piclist )
face_num = detector.detect( gray_image, rects, face_max );
else
face_num = tracker.track( gray_image, rects, face_max, image ); // track in a video for faster speed
//face_num = tracker.detect( gray_image, rects, face_max ); // detect in an image
//set param for mouse event processing
if(!quiet)
{
mouse_faceparam.face_num = face_num;
mouse_faceparam.image = image;
}
ticks = cvGetTickCount() - ticks;
tracker_fps = 1000.0 / ( 1e-3 * ticks / cvGetTickFrequency() );
tracker_total_ticks += ticks;
if( fp_imaginfo != NULL )
fprintf( fp_imaginfo, "%s %d", vidcap->filename(), face_num );
// blink/smile/gender/age/face recognize section
for( int i=0; i<face_num; i++ )
//for( int i=0; i< MIN(1,face_num); i++ )
{
// get face rect and id from face tracker
CvRect rect = rects[i].rc;
if( fp_imaginfo != NULL )
fprintf( fp_imaginfo, " %d %d %d %d %f", rect.x, rect.y, rect.width, rect.height, rects[i].prob );
int face_trackid = rects[i].fid;
float like = rects[i].prob;
int angle= rects[i].angle;
// filter out outer faces
if( rect.x+rect.width > gray_image.width() || rect.x < 0 ) continue;
if( rect.y+rect.height > gray_image.height() || rect.y < 0 ) continue;
//tracker.getThumbnail(image, rect, thumbnailImg);
// detect landmark points
ticks = cvGetTickCount();
if(bRunLandmark)
{
if( is_piclist )
bLandmark = landmarkDetector.detect( gray_image, &rect, landmark6, parameters, angle ); //detect in an image
else
bLandmark = landmarkDetector.track( gray_image, &rect, landmark6, parameters, angle ); // track in a video for faster speed
ticks = cvGetTickCount() - ticks;
landmark_fps = 1000.0 / ( 1e-3 * ticks / cvGetTickFrequency() );
landmark_total_ticks += ticks;
}
else
bLandmark = false;
if(quiet == false && bLandmark == false)
{
//DrawFaceRect
cxlibDrawFaceRect(image, rect);
continue;
}
// warped align face and hist eq to delighting
ticks = cvGetTickCount();
cutFace.init(gray_image, rect, landmark6);
ticks = cvGetTickCount() - ticks;
if(ticks > 1)
align_fps = 1000.0 / ( 1e-3 * ticks / cvGetTickFrequency() );
else
{ align_fps = 0;
ticks = 0;
}
align_total_ticks += ticks;
if(saveface) //save face icon for training later
{
//save cutfaces
if(sfolder)
{
#ifdef WIN32
_splitpath(vidcap->filename(),driver,dir,fname,ext);
sprintf(sImgPath, "%s//%s%s", sfolder, fname,ext);
#else
sprintf(sImgPath, "%s//%06d.jpg", sfolder, faceimgID++);
#endif
}
else
sprintf(sImgPath, "%s#.jpg", vidcap->filename());
cvSaveImage(sImgPath, cutFace.getBigCutFace());
}
// detect blink
bBlink = 0;
probBlink = 0;
if(blink && bLandmark)
{
ticks = cvGetTickCount();
float blink_threshold = blinkDetector.getDefThreshold();//0.5;
int ret = blinkDetector.predict( &cutFace, &probBlink);
if(probBlink > blink_threshold )
bBlink = 1; //eye close
else
bBlink = -1; //eye open
ticks = cvGetTickCount() - ticks;
blink_fps = 1000.0/(1e-3*ticks/cvGetTickFrequency());
blink_total_ticks += ticks;
print_score = probBlink;
}
else blink_fps = 0;
// detect smile
bSmile = 0;
probSmile = 0;
if ( smile && bLandmark )
{
ticks = cvGetTickCount();
float smile_threshold = smileDetector.getDefThreshold(); //0.48;
int ret = smileDetector.predict(&cutFace, &probSmile);
if(probSmile > smile_threshold)
bSmile = 1; //smile
else
bSmile = -1; //not smile
ticks = cvGetTickCount() - ticks;
smile_fps = 1000.0 /( 1e-3 * ticks / cvGetTickFrequency() );
smile_total_ticks += ticks;
print_score = probSmile;
}
else smile_fps = 0;
//detect gender
bGender = 0;
probGender = 0;
if(gender && bLandmark)
{
ticks = cvGetTickCount();
float gender_threshold = genderDetector.getDefThreshold(); // 0.42;
int ret = genderDetector.predict(&cutFace, &probGender);
if(probGender > gender_threshold)
bGender = 1; //female
else
bGender = -1; //male
//bGender = -1:male, 1:female, 0: null
// smooth prediction result
if( ! is_piclist )
bGender = genderDetector.voteLabel(face_trackid, bGender);
ticks = cvGetTickCount() - ticks;
gender_fps = 1000.0/(1e-3*ticks/cvGetTickFrequency());
gender_total_ticks += ticks;
print_score = probGender;
}
else gender_fps = 0;
//detect age
nAgeID = -1;
if(age && bLandmark && rect.width*rect.height > 40*40)
{
ticks = cvGetTickCount();
//nAgeID = 0:"Baby", 1:"Kid", 2:"Adult", 3:"Senior"
nAgeID = ageDetector.predict(&cutFace, probAge);
// smooth prediction result
if( ! is_piclist )
nAgeID = ageDetector.voteLabel(face_trackid, nAgeID);
ticks = cvGetTickCount() - ticks;
age_fps = 1000.0/(1e-3*ticks/cvGetTickFrequency());
age_total_ticks += ticks;
print_score = probAge[nAgeID];
//if( ! quiet ) cxDrawAignFace2Image(image, pCutFace2);
}
else
{
age_fps = 0;
}
// recognize the face id
// we only do recognition every 5 frames,interval
char *sFaceCaption = NULL;
char sFaceCaptionBuff[256];
int face_id = 0;
probFaceID = 0;
if ( recog && bLandmark )
{
ticks = cvGetTickCount();
float face_threshold = faceRecognizer.getDefThreshold();
/////////////////////////////////////////////////////////////////////////////////////////
int face_id = -1;
if(bEnableAutoCluster & !is_piclist)
{
bool bAutocluster = true;
if(mouse_faceparam.ret_online_collecting) bAutocluster = false;
//face clustering
face_id = faceRecognizer.predict(&cutFace, &probFaceID, bAutocluster, face_trackid, frames);
}
else//face recognition
face_id = faceRecognizer.predict(&cutFace, &probFaceID);
/////////////////////////////////////////////////////////////////////////////////////////
ticks = cvGetTickCount() - ticks;
recg_fps = 1000.0f / ( 1e-3 * ticks / cvGetTickFrequency() );
recg_total_ticks += ticks;
// smooth prediction result
if( ! is_piclist && !bEnableAutoCluster)
{
if(probFaceID > face_threshold*1.0)
face_id = faceRecognizer.voteLabel(face_trackid, face_id);
else
face_id = faceRecognizer.voteLabel(face_trackid, -1);
}
else if(probFaceID <= face_threshold)
{
face_id =-1;
}
//set face name caption
if(face_id >= 0)
{
// recognized face name
const char* sFaceName = faceRecognizer.getFaceName(face_id);
sprintf(sFaceCaptionBuff, "%s %.2f", sFaceName, probFaceID);
//sprintf(sFaceCaptionBuff, "%s", sFaceName); //dispaly score
sFaceCaption = sFaceCaptionBuff;
print_score = probFaceID;
print_faceid = face_id;
}
else
{ // failed to recognize
//sprintf(sFaceCaptionBuff, "N\A %.2f", probFaceID);
//sFaceCaption = sFaceCaptionBuff;
}
// collect and save unknown face exemplars
if(probFaceID < face_threshold*0.9 || face_id != mouse_faceparam.ret_faceset_id )
{
if(mouse_faceparam.ret_online_collecting && (face_num ==1 || face_trackid == mouse_faceparam.ret_facetrack_id))
{
if( rect.x > gray_image.width()/4 && rect.x+rect.width < gray_image.width()*3/4 )
{
mouse_faceparam.updated = true;
int nFaceSetIdx = faceRecognizer.getFaceSetIdx(mouse_faceparam.ret_faceset_id);
bool bflag = faceRecognizer.tryInsertFace(cutFace.getBigCutFace(), nFaceSetIdx);
//printf("insert flag %d\n", bflag);
}
}
}
}
else recg_fps = 0;
if( ! quiet )
{
sprintf(sCaptionInfo, "FPS: %03d Fd:%04d Ld:%04d Fa:%04d Bl:%04d Sm:%04d Ge:%04d Ag:%03d Rc:%03d",
(int)frame_fps, (int)tracker_fps, (int)landmark_fps, (int)align_fps,
(int)blink_fps, (int)smile_fps, (int)gender_fps, (int)age_fps, (int)recg_fps);
//sprintf(sFaceCaptionBuff, "%.2f", print_score);
//sFaceCaption = sFaceCaptionBuff;
int trackid = -1; //face_trackid. don't display trackid if -1
cxlibDrawFaceBlob( image, pFont, trackid, rect, landmark6, probSmile,
bBlink, bSmile, bGender, nAgeID, sFaceCaption, NULL,
pImgSmileBGR, pImgSmileBGRA, pImgSmileMask);
}
// log file
if( fp_faceinfo != NULL )
{
// index, rect, landmark6, bBlink, probBlink, bSmile, probSmile, bGender, probGender, nAgeID, probAge[nAgeID], face_id, probFaceID
//fprintf( fp_faceinfo, "#%s# @%s@ ", vidcap->filename(), sImgPath);
fprintf( fp_faceinfo, "#%s# ", vidcap->filename());
fprintf( fp_faceinfo, "faceidx=( %06d %02d )", vidcap->index(), i+1 );
fprintf( fp_faceinfo, " rect=( %3d %3d %3d %3d )", rect.x, rect.y, rect.width, rect.height );
fprintf( fp_faceinfo, " landmark6=(" );
int l;
for( l = 0; l < 6; l++ )
fprintf( fp_faceinfo, " %3.0f %3.0f", landmark6[l].x, landmark6[l].y );
fprintf( fp_faceinfo, " )");
fprintf( fp_faceinfo, " blink=( %+d %f )", bBlink, probBlink );
fprintf( fp_faceinfo, " smile=( %+d %f )", bSmile, probSmile );
fprintf( fp_faceinfo, " gender=( %+d %f )", bGender, probGender );
fprintf( fp_faceinfo, " agegroup=( %+d %f )", nAgeID, (nAgeID >= 0 && nAgeID < 4) ? probAge[nAgeID] : 1.0f );
fprintf( fp_faceinfo, " identity=( %+d %f )", face_id, probFaceID );
fprintf( fp_faceinfo, "\n" );
}
}
if( fp_imaginfo != NULL )
fprintf( fp_imaginfo, "\n" );
ticks = cvGetTickCount() - start_ticks;
total_ticks += (ticks);
frame_fps = 1000.0f / ( 1e-3 * ticks / cvGetTickFrequency() );
// frame face_num
frames++;
//auto focus faces
if(quiet == false && bAutoFocus)
{
if(imgAutoFocus)
cvCopy(image, imgAutoFocus);
else
imgAutoFocus = cvCloneImage(image);
cxlibAutoFocusFaceImage(imgAutoFocus, image, rects, face_num);
}
// next frame if quiet
if( quiet )
continue;
else
{
// draw status info for custom interaction
if(mouse_faceparam.ret_online_collecting == 1)
{
sprintf(sCaptionInfo, "Collecting faces for track_id = %d", mouse_faceparam.ret_facetrack_id);
//draw face collecting region
cvLine(image, cvPoint(image.width()/4, 0), cvPoint(image.width()/4, image.height()-1), CV_RGB(255,255,0), 2);
cvLine(image, cvPoint(image.width()*3/4, 0), cvPoint(image.width()*3/4, image.height()-1), CV_RGB(255,255,0), 2);
}
else
sprintf(sCaptionInfo, "FPS: %03d Fd:%04d Ld:%04d Fa:%04d Bl:%04d Sm:%04d Ge:%04d Ag:%03d Rc:%03d",
(int)frame_fps, (int)tracker_fps, (int)landmark_fps, (int)align_fps,
(int)blink_fps, (int)smile_fps, (int)gender_fps, (int)age_fps, (int)recg_fps);
cxlibDrawCaption( image, pFont, sCaptionInfo);
}
//show Image
if (image.width() <= 800)
{
//show image
cvShowImage( str_title, image );
}
else
{ // show scaled smaller image
CvImage scale_image (cvSize(800, image.height()*800/image.width()), image.depth(), 3 );
cvResize (image, scale_image);
cvShowImage( str_title, scale_image );
}
// user interaction
int key = cvWaitKey( 30 );
//int key = cvWaitKey( );
if( key == ' ' ) // press the spacebar to pause the video play
cvWaitKey( 0 );
else if( key == 27 )
break; // press 'esc' to exit
else if( key == 'a' )
{ // add new face name
if(face_num > 0)
{
CvRect rect = rects[0].rc;
int x = rect.x+rect.width/2;
int y = rect.y+rect.height/2;
addFaceSet( x, y, &mouse_faceparam);
}
}
else if( key == 'c' )
{ // collect face exemplars for current selected facename
mouse_faceparam.ret_online_collecting = 1; //enable online face exemplar collecting
}
else if( key == 'z' )
// turn on/off the autofocus flag
bAutoFocus = !bAutoFocus;
else if(key >= 0)
{
if(mouse_faceparam.ret_online_collecting == 1)
{ // stop collecting faces
mouse_faceparam.ret_online_collecting = 0; //disable online face exemplar collecting
mouse_faceparam.ret_facetrack_id = -1;
}
if( key == 's')
{ // save face models
faceRecognizer.saveFaceModelXML("faceset_model.xml");
sprintf(sCaptionInfo, "%s", "saved the face model");
cxlibDrawCaption( pImg, pFont, sCaptionInfo);
cvShowImage( str_title, pImg );
cvWaitKey( 400 );
}
}
}
// print speed info about fps
float temp = 1e-6f / cvGetTickFrequency();
tracker_fps = 1.0f / ( tracker_total_ticks * temp / frames );
if (landmark_total_ticks != 0.0)
landmark_fps = 1.0f / ( landmark_total_ticks * temp / frames );
if (align_total_ticks != 0.0)
align_fps = 1.0f / ( align_total_ticks * temp / frames );
if (blink_total_ticks != 0.0)
blink_fps = 1.0f / (blink_total_ticks * temp / frames);
if (smile_total_ticks != 0.0)
smile_fps = 1.0f / (smile_total_ticks * temp / frames);
if (gender_total_ticks != 0.0)
gender_fps = 1.0f / (gender_total_ticks * temp / frames);
if (age_total_ticks != 0.0)
age_fps = 1.0f / (age_total_ticks * temp / frames);
if (recg_total_ticks != 0.0)
recg_fps = 1.0f / (recg_total_ticks * temp / frames);
total_fps = 1.0f / (total_ticks * temp / frames);
printf( "Total frames:%d Speed:%.1f fps\n", frames, total_fps);
printf( "FPS: Fd:%.1f Ld:%.1f Fa:%.1f Bl:%.1f Sm:%.1f Ge:%.1f Ag:%.1f Rc:%.1f",
tracker_fps, landmark_fps, align_fps,
blink_fps, smile_fps, gender_fps, age_fps, recg_fps);
//save updated face model
if(mouse_faceparam.updated == true)
{
sprintf(sCaptionInfo, "%s", "press key 's' to save updated face model or other keys to cancel");
cxlibDrawCaption( pImg, pFont, sCaptionInfo);
cvShowImage( str_title, pImg );
int key = cvWaitKey();
if( key == 's')
faceRecognizer.saveFaceModelXML("faceset_model.xml");
}
//save merged face model for dynamic clustering of smoothID
vFaceSet vMergedFaceSet;
int minWeight = 10;
faceRecognizer.getMergedFaceSet(vMergedFaceSet, minWeight);
faceRecognizer.saveFaceModelXML("faceset_modelMerged.xml", &vMergedFaceSet);
//faceRecognizer.saveFaceModelXML("faceset_modelMerged#.xml");
//release buff
//release global GUI data
if( !quiet )
cvDestroyWindow( str_title );
cvReleaseImage(&thumbnailImg);
cvReleaseImage(&pImgSmileBGR);
cvReleaseImage(&pImgSmileBGRA);
cvReleaseImage(&pImgSmileMask);
delete pFont;
delete vidcap;
if( fp_imaginfo != NULL )
fclose( fp_imaginfo );
if( fp_faceinfo != NULL )
fclose( fp_faceinfo );
return 0;
}
int main()
{
IplImage* img = cvLoadImage("goal_arena.bmp");
CvSize imgSize = cvGetSize(img);
IplImage* detected = cvCreateImage(imgSize, 8, 1);
IplImage* imgBlue = cvCreateImage(imgSize, 8, 1);
IplImage* imgGreen = cvCreateImage(imgSize, 8, 1);
IplImage* imgRed = cvCreateImage(imgSize, 8, 1);
cvSplit(img, imgBlue, imgGreen, imgRed, NULL);
cvAnd(imgGreen, imgBlue, detected);
cvAnd(detected, imgRed, detected);
cvErode(detected, detected);
cvDilate(detected, detected); // Opening
// cvThreshold(detected, detected, 100, 250, CV_THRESH_BINARY);
CvMat* lines = cvCreateMat(100, 1, CV_32FC2);
cvHoughLines2(detected, lines, CV_HOUGH_STANDARD, 1, 0.001, 100);
// CvMat* lines = cvCreateMat(100, 1, CV_32FC2);
// cvHoughLines2(detected, lines, CV_HOUGH_STANDARD, 1, 0.001, 100);
CvPoint left1 = cvPoint(0, 0);
CvPoint left2 = cvPoint(0, 0);
CvPoint right1 = cvPoint(0, 0);
CvPoint right2 = cvPoint(0, 0);
CvPoint top1 = cvPoint(0, 0);
CvPoint top2 = cvPoint(0, 0);
CvPoint bottom1 = cvPoint(0, 0);
CvPoint bottom2 = cvPoint(0, 0);
int numLines = lines->rows;
int numTop = 0;
int numBottom = 0;
int numLeft = 0;
int numRight = 0;
for(int i=0;i<numLines;i++)
{
CvScalar dat = cvGet1D(lines, i);
double rho = dat.val[0];
double theta = dat.val[1];
if(theta==0.0)
continue;
double degrees = theta*180.0/(3.1412);
CvPoint pt1 = cvPoint(0, rho/sin(theta));
CvPoint pt2 = cvPoint(img->width, (-img->width/tan(theta)) + rho/sin(theta));
if(abs(rho)<50.0)
{
if(degrees>45.0 && degrees<135.0)
{
numTop++;
// The line is vertical and near the top
top1.x+=pt1.x;
top1.y+=pt1.y;
top2.x+=pt2.x;
top2.y+=pt2.y;
}
else
{
numLeft++;
// The line is vertical and near the left
left1.x+=pt1.x;
left1.y+=pt1.y;
left2.x+=pt2.x;
left2.y+=pt2.y;
}
}
else
{
// We're in the right portion
if(degrees>45.0 && degrees<135.0)
{
numBottom++;
//The line is horizontal and near the bottom
bottom1.x+=pt1.x;
bottom1.y+=pt1.y;
bottom2.x+=pt2.x;
bottom2.y+=pt2.y;
}
else
{
numRight++;
// The line is vertical and near the right
right1.x+=pt1.x;
right1.y+=pt1.y;
right2.x+=pt2.x;
right2.y+=pt2.y;
}
}
}
left1.x/=numLeft;
left1.y/=numLeft;
left2.x/=numLeft;
left2.y/=numLeft;
right1.x/=numRight;
right1.y/=numRight;
right2.x/=numRight;
right2.y/=numRight;
top1.x/=numTop;
top1.y/=numTop;
top2.x/=numTop;
top2.y/=numTop;
bottom1.x/=numBottom;
bottom1.y/=numBottom;
bottom2.x/=numBottom;
bottom2.y/=numBottom;
cvLine(img, left1, left2, CV_RGB(255, 0,0), 1);
cvLine(img, right1, right2, CV_RGB(255, 0,0), 1);
cvLine(img, top1, top2, CV_RGB(255, 0,0), 1);
cvLine(img, bottom1, bottom2, CV_RGB(255, 0,0), 1);
// Next, we need to figure out the four intersection points
double leftA = left2.y-left1.y;
double leftB = left1.x-left2.x;
double leftC = leftA*left1.x + leftB*left1.y;
double rightA = right2.y-right1.y;
double rightB = right1.x-right2.x;
double rightC = rightA*right1.x + rightB*right1.y;
double topA = top2.y-top1.y;
double topB = top1.x-top2.x;
double topC = topA*top1.x + topB*top1.y;
double bottomA = bottom2.y-bottom1.y;
double bottomB = bottom1.x-bottom2.x;
double bottomC = bottomA*bottom1.x + bottomB*bottom1.y;
// Intersection of left and top
double detTopLeft = leftA*topB - leftB*topA;
CvPoint ptTopLeft = cvPoint((topB*leftC - leftB*topC)/detTopLeft, (leftA*topC - topA*leftC)/detTopLeft);
// Intersection of top and right
double detTopRight = rightA*topB - rightB*topA;
CvPoint ptTopRight = cvPoint((topB*rightC-rightB*topC)/detTopRight, (rightA*topC-topA*rightC)/detTopRight);
// Intersection of right and bottom
double detBottomRight = rightA*bottomB - rightB*bottomA;
CvPoint ptBottomRight = cvPoint((bottomB*rightC-rightB*bottomC)/detBottomRight, (rightA*bottomC-bottomA*rightC)/detBottomRight);
// Intersection of bottom and left
double detBottomLeft = leftA*bottomB-leftB*bottomA;
CvPoint ptBottomLeft = cvPoint((bottomB*leftC-leftB*bottomC)/detBottomLeft, (leftA*bottomC-bottomA*leftC)/detBottomLeft);
cvLine(img, ptTopLeft, ptTopLeft, CV_RGB(0,255,0), 5);
cvLine(img, ptTopRight, ptTopRight, CV_RGB(0,255,0), 5);
cvLine(img, ptBottomRight, ptBottomRight, CV_RGB(0,255,0), 5);
cvLine(img, ptBottomLeft, ptBottomLeft, CV_RGB(0,255,0), 5);
IplImage* imgMask = cvCreateImage(imgSize, 8, 3);
cvZero(imgMask);
CvPoint* pts = new CvPoint[4];
pts[0] = ptTopLeft;
pts[1] = ptTopRight;
pts[2] = ptBottomRight;
pts[3] = ptBottomLeft;
cvFillConvexPoly(imgMask, pts, 4, cvScalar(255,255,255));
cvAnd(img, imgMask, img);
cvNamedWindow("Original");
cvNamedWindow("Detected");
cvShowImage("Original", img);
cvShowImage("Detected", detected);
cvWaitKey(0);
return 0;
}
void PixelFlow::computeFlow( const Image& image, Math::Vector< int, 2 >& result, int& difference, Image* pDebugImg)
{
//decliaring variables
int xCut = 0;
int yCut = 0;
int temp = 0;
int xIndex = 0;
int yIndex = 0;
int xBufSize = 0;
int yBufSize = 0;
int width= image.widthStep;
int hight = image.height;
int xSize = int(x.size());
int ySize = int(y.size());
Math::Vector< int, 4 > xRec;
Math::Vector< int, 4 > yRec;
calculateIncrisedCoordiants(width, hight, 0.5, topL, bottomR, xRec, yRec);
std::vector<int> newX( xRec(2) - xRec(0) + 1, 0 );
std::vector<int> newY( yRec(3) - yRec(1) + 1, 0 );
if(xRec(2) != width)
{
if(((int)x.size())%2 != 0)
xBufSize = int(x.size());
else
xBufSize = int(x.size())+1;
}
else
{
xBufSize = int(newX.size()) - int(x.size()/2);
}
if(yRec(3) != hight)
{
if(((int)y.size())%2 != 0)
yBufSize = int(y.size());
else
yBufSize = int(y.size())+1;
}
else
{
yBufSize = int(newY.size()) - int(y.size()/2) ;
}
std::vector<int> xErrorBuf(xBufSize, 255);
std::vector<int> yErrorBuf(yBufSize, 255);
//calculating the updated buffer
for (int i = 0; i < xRec(2) - xRec(0); i++)
for(int j = 0; j < xRec(3) - xRec(1); j++)
{
int temp = (xRec(1) + j)*image.width + xRec(0) + i;
newX[i] += ((unsigned char*)image.imageData)[temp];
}
for(int i = 0; i < int(newX.size()); i++)
newX[i] /= xRec(3) - xRec(1);
for (int i = 0; i < yRec(2) - yRec(0); i++)
for(int j = 0; j < yRec(3) - yRec(1); j++)
{
int temp = (yRec(1) + j)*image.width + yRec(0) + i;
newY[j] += ((unsigned char*)image.imageData)[temp];
}
for(int i = 0; i < int(newY.size()); i++)
newY[i] /= yRec(2) - yRec(0);
//check whether there was a cut
if(xRec(2) == width)
xCut = 1;
if(xRec(0)== 0)
if(xCut != 1)
xCut = -1;
else
xCut = 2;
//calculating the error buffer
calculateErrorBuffer(x,newX, xErrorBuf,xCut);
//check whether there was a cut
if(yRec(3) == hight)
yCut = 1;
if(yRec(1)== 0)
if(yCut != 1)
yCut = -1;
else
yCut = 2;
//calculating the error buffer
calculateErrorBuffer(y,newY, yErrorBuf,yCut);
//finding the best shift in x direction
temp = xErrorBuf[0];
for (int i = 1; i <int(xErrorBuf.size()); i++)
if(temp > xErrorBuf[i])
{
temp = xErrorBuf[i];
xIndex = i;
}
//finding the best shift in y direcion
temp = yErrorBuf[0];
for (int i = 1; i < int(yErrorBuf.size()); i++)
if(temp > yErrorBuf[i])
{
temp = yErrorBuf[i];
yIndex = i;
}
//if the nuew buffer is cut from left, the error buffer is shifting on xdif, or ydif
int xdif, ydif;
xdif = int(newX.size()) - 2*int(x.size()/2) - int(x.size());
ydif = int(newY.size()) - 2*int(y.size()/2) - int(y.size());
//returning the reslt
if(xCut == 0 || xCut == 1)
{
result(0) = xIndex- int(x.size()/2);
}
else
if(xCut = -1)
{
result(0) = xIndex - 2*int(x.size()/2)-xdif;
}
if(yCut == 0 || yCut == 1)
{
result(1) = yIndex - int(y.size()/2) ;
}
else
if(yCut = -1)
{
result(1) = yIndex - 2*int(y.size()/2) -ydif;
}
//TO BE MODIFIED::
difference = 0;
//visualizing the error buffer
if(pDebugImg)
{
CvPoint p1,p2;
if(int(xErrorBuf.size()) < width -50)
{
p1.y = 10;
p2.y = 20;
for (int i = 0; i < int(xErrorBuf.size()); i++)
{
p1.x = width - int(xErrorBuf.size()) - 50 + i ;
p2.x = width - int(xErrorBuf.size()) - 50 + i ;
int color = std::min( int(xErrorBuf[i]) + 50, 255 );
cvLine(pDebugImg,p1,p2,cvScalar(0,0,color),10,CV_AA,0);
}
p1.x = width -10;
p2.x = width -20;
for (int i = 0; i < int(yErrorBuf.size()); i++)
{
p1.y = i + 50;
p2.y = i + 50;
int color = std::min( int(yErrorBuf[i]) + 50, 255 );
cvLine(pDebugImg,p1,p2,cvScalar(0,0,color),10,CV_AA,0);
}
}
}
}
IplImage * Transformation::getMatchImg(){
IplImage* rgb_img_src = src->input->rgb_img;//cvLoadImage(src->input->rgb_path.c_str(),CV_LOAD_IMAGE_UNCHANGED);
char * data_src = (char *)rgb_img_src->imageData;
unsigned short * depth_data_src = (unsigned short *)(src->input->depth_img->imageData);
IplImage* rgb_img_dst = dst->input->rgb_img;//cvLoadImage(dst->input->rgb_path.c_str(),CV_LOAD_IMAGE_UNCHANGED);
char * data_dst = (char *)rgb_img_dst->imageData;
unsigned short * depth_data_dst = (unsigned short *)(dst->input->depth_img->imageData);
int width = rgb_img_src->width;
int height = rgb_img_src->height;
IplImage* img_combine = cvCreateImage(cvSize(2*width,height), IPL_DEPTH_8U, 3);
char * data = (char *)img_combine->imageData;
for (int j = 0; j < height; j++){
for (int i = 0; i < width; i++){
int ind = 3*(640*j+i);
int dst_ind = 3 * (j * (2*width) + (width+i));
int src_ind = 3 * (j * (2*width) + (i));
int d_dst = depth_data_dst[(640*j+i)];
int d_src = depth_data_src[(640*j+i)];
if(d_dst == 0 && (i % 2 == 0) && (j % 2 == 0)){
data[dst_ind + 0] = 255;
data[dst_ind + 1] = 0;
data[dst_ind + 2] = 255;
}else{
data[dst_ind + 0] = data_dst[ind +0];
data[dst_ind + 1] = data_dst[ind +1];
data[dst_ind + 2] = data_dst[ind +2];
}
if(d_src == 0 && (i % 2 == 0) && (j % 2 == 0)){
data[src_ind + 0] = 255;
data[src_ind + 1] = 0;
data[src_ind + 2] = 255;
}else{
data[src_ind + 0] = data_src[ind +0];
data[src_ind + 1] = data_src[ind +1];
data[src_ind + 2] = data_src[ind +2];
}
}
}
//cvReleaseImage( &rgb_img_src );
//cvReleaseImage( &rgb_img_dst );
for(unsigned int i = 0; i < src->keypoints->valid_key_points.size(); i++){
KeyPoint * src_kp = src->keypoints->valid_key_points.at(i);
cvCircle(img_combine,cvPoint(src_kp->point->w , src_kp->point->h), 3,cvScalar(0, 255, 0, 0),1, 8, 0);
}
for(unsigned int i = 0; i < src->keypoints->invalid_key_points.size(); i++){
KeyPoint * kp = src->keypoints->invalid_key_points.at(i);
cvCircle(img_combine,cvPoint(kp->point->w , kp->point->h), 3,cvScalar(0, 255, 255, 0),1, 8, 0);
}
for(unsigned int i = 0; i < dst->keypoints->valid_key_points.size(); i++){
KeyPoint * dst_kp = dst->keypoints->valid_key_points.at(i);
cvCircle(img_combine,cvPoint(dst_kp->point->w + width , dst_kp->point->h), 3,cvScalar(0, 255, 0, 0),1, 8, 0);
}
for(unsigned int i = 0; i < dst->keypoints->invalid_key_points.size(); i++){
KeyPoint * kp = dst->keypoints->invalid_key_points.at(i);
cvCircle(img_combine,cvPoint(kp->point->w + width , kp->point->h), 3,cvScalar(0, 255, 255, 0),1, 8, 0);
}
for(unsigned int i = 0; i < matches.size(); i++){
KeyPoint * src_kp = matches.at(i).first;
KeyPoint * dst_kp = matches.at(i).second;
cvLine(img_combine,cvPoint(dst_kp->point->w + width ,dst_kp->point->h),cvPoint(src_kp->point->w,src_kp->point->h),cvScalar(0, 0, 255, 0),1, 8, 0);
}
return img_combine;
}
void jl_cv_draw_line(jl_cv_t* jl_cv, jl_cv_line_t line) {
jl_cv_getoutput(jl_cv);
cvLine(jl_cv->image_rgb, line.p1, line.p2, CV_RGB(0xFF,0x00,0xFF),
1, 8, 0);
jl_cv_disp_gray_(jl_cv);
}
void Transformation::saveProblem(string path){
printf("saveProblem(%s)\n",path.c_str());
IplImage* rgb_img_src = cvLoadImage(src->input->rgb_path.c_str(),CV_LOAD_IMAGE_UNCHANGED);
char * data_src = (char *)rgb_img_src->imageData;
unsigned short * depth_data_src = (unsigned short *)(src->input->depth_img->imageData);
IplImage* rgb_img_dst = cvLoadImage(dst->input->rgb_path.c_str(),CV_LOAD_IMAGE_UNCHANGED);
char * data_dst = (char *)rgb_img_dst->imageData;
unsigned short * depth_data_dst = (unsigned short *)(dst->input->depth_img->imageData);
int width = rgb_img_src->width;
int height = rgb_img_src->height;
IplImage* img_combine = cvCreateImage(cvSize(2*width,height), IPL_DEPTH_8U, 3);
char * data = (char *)img_combine->imageData;
for (int j = 0; j < height; j++){
for (int i = 0; i < width; i++){
int ind = 3*(640*j+i);
int dst_ind = 3 * (j * (2*width) + (width+i));
int src_ind = 3 * (j * (2*width) + (i));
int d_dst = depth_data_dst[(640*j+i)];
int d_src = depth_data_src[(640*j+i)];
if(d_dst == 0 && (i % 2 == 0) && (j % 2 == 0)){
data[dst_ind + 0] = 255;
data[dst_ind + 1] = 0;
data[dst_ind + 2] = 255;
}else{
data[dst_ind + 0] = data_dst[ind +0];
data[dst_ind + 1] = data_dst[ind +1];
data[dst_ind + 2] = data_dst[ind +2];
}
if(d_src == 0 && (i % 2 == 0) && (j % 2 == 0)){
data[src_ind + 0] = 255;
data[src_ind + 1] = 0;
data[src_ind + 2] = 255;
}else{
data[src_ind + 0] = data_src[ind +0];
data[src_ind + 1] = data_src[ind +1];
data[src_ind + 2] = data_src[ind +2];
}
}
}
cvReleaseImage( &rgb_img_src );
cvReleaseImage( &rgb_img_dst );
for(unsigned int i = 0; i < src->keypoints->valid_key_points.size(); i++){
KeyPoint * src_kp = src->keypoints->valid_key_points.at(i);
cvCircle(img_combine,cvPoint(src_kp->point->w , src_kp->point->h), 3,cvScalar(0, 255, 0, 0),1, 8, 0);
}
for(unsigned int i = 0; i < src->keypoints->invalid_key_points.size(); i++){
KeyPoint * kp = src->keypoints->invalid_key_points.at(i);
cvCircle(img_combine,cvPoint(kp->point->w , kp->point->h), 3,cvScalar(0, 255, 255, 0),1, 8, 0);
}
for(unsigned int i = 0; i < dst->keypoints->valid_key_points.size(); i++){
KeyPoint * dst_kp = dst->keypoints->valid_key_points.at(i);
cvCircle(img_combine,cvPoint(dst_kp->point->w + width , dst_kp->point->h), 3,cvScalar(0, 255, 0, 0),1, 8, 0);
}
for(unsigned int i = 0; i < dst->keypoints->invalid_key_points.size(); i++){
KeyPoint * kp = dst->keypoints->invalid_key_points.at(i);
cvCircle(img_combine,cvPoint(kp->point->w + width , kp->point->h), 3,cvScalar(0, 255, 255, 0),1, 8, 0);
}
for(unsigned int i = 0; i < matches.size() ; i++){
KeyPoint * src_kp = matches.at(i).first;
KeyPoint * dst_kp = matches.at(i).second;
cvLine(img_combine,cvPoint(dst_kp->point->w + width ,dst_kp->point->h),cvPoint(src_kp->point->w,src_kp->point->h),cvScalar(0, 0, 255, 0),1, 8, 0);
}
cvShowImage("combined image", img_combine);
char buf[1024];
sprintf(buf,"%i_%s.png",(int)matches.size(),path.c_str());
if(!cvSaveImage(buf,img_combine)){printf("Could not save: %s\n",buf);}
cvWaitKey(30);
cvReleaseImage( &img_combine);
}
void rotate(IplImage *img)
{
CvRect rect;
IplImage *imgLine;
rect.x=GlobalGandhiji.x+GlobalGandhiji.width;
rect.y=GlobalGandhiji.y-5;
rect.width=(int)((GlobalGandhiji.width)-5);
rect.height=GlobalGandhiji.height+15;
if(GlobalGandhiji.matchval!=-1 && rect.x>0 && rect.y>0 && rect.y+rect.height<= img->height && rect.x+rect.width<= img->width)
{
imgLine=cropRectangle(img,rect);
cvNamedWindow("imgLine",1);
cvShowImage("imgLine",imgLine);
IplImage* src1 = cvCreateImage(
cvGetSize(imgLine), 8, 1 );
cvCvtColor( imgLine, src1, CV_RGB2GRAY );
IplImage* dst = cvCreateImage(
cvGetSize(src1), 8, 1 );
IplImage* color_dst = cvCreateImage(
cvGetSize(src1), 8, 3 );
CvMemStorage* storage =
cvCreateMemStorage(0);
CvSeq* lines = 0;
int i;
cvCanny( src1, dst,50, 150, 3 );
//cvDilate( dst, dst, 0, 1 );
cvNamedWindow("edgedest",1);
cvShowImage("edgedest",dst);
cvCvtColor( dst, color_dst, CV_GRAY2BGR );
#if 1
lines = cvHoughLines2( dst, storage,
CV_HOUGH_STANDARD, 1, CV_PI/180, 30, 0, 0 );
for( i = 0; i < MIN(lines->total,100); i++ )
{
float* line =
(float*)cvGetSeqElem(lines,i);
float rho = line[0];
float theta = line[1];
printf("theta = %f",(theta*180/3.142));
CvPoint pt1, pt2;
double a = cos(theta), b = sin(theta);
double x0 = a*rho, y0 = b*rho;
printf("a= %f b=%f x0=%f y0=%f roh=%f\n", a,b,x0,y0,rho);
pt1.x = cvRound(x0 + 1000*(-b));
pt1.y = cvRound(y0 + 1000*(a));
pt2.x = cvRound(x0 - 1000*(-b));
pt2.y = cvRound(y0 - 1000*(a));
printf(" x1 = %d, y1 = %d",pt1.x,pt1.y);
printf(" x2 = %d, y2 = %d\n\n",pt2.x,pt2.y);
//if((theta*180/3.142) < 100 && (theta*180/3.142) > 79 )
cvLine( color_dst, pt1, pt2,
CV_RGB(255,0,0), 3, 8 );
}
#else
lines = cvHoughLines2( dst, storage,
CV_HOUGH_PROBABILISTIC, 1, CV_PI/180, 30, 0, 0 );
for( i = 0; i < lines->total; i++ )
{
CvPoint* line =
(CvPoint*)cvGetSeqElem(lines,i);
cvLine( color_dst, line[0], line[1],
CV_RGB(255,0,0), 3, 8 );
}
#endif
cvNamedWindow( "Hough", 1 );
cvShowImage( "Hough", color_dst );
}
/*
*/
}
int main(int argc, const char * argv[]) {
IplImage* img = cvCreateImage( cvSize( 500, 500 ), 8, 3 );
CvRNG rng = cvRNG(-1);
cvNamedWindow( "fitline", 1 );
for(;;) {
char key;
int i, count = cvRandInt(&rng) % 100 + 1, outliers = count/5;
float a = cvRandReal(&rng) * 200;
float b = cvRandReal(&rng) * 40;
float angle = cvRandReal(&rng) * CV_PI;
float cos_a = cos(angle), sin_a = sin(angle);
CvPoint pt1, pt2;
CvPoint* points = (CvPoint*)malloc( count * sizeof(points[0]));
CvMat pointMat = cvMat( 1, count, CV_32SC2, points );
float line[4];
float d, t;
b = MIN(a*0.3, b);
// generate some points that are close to the line
for( i = 0; i < count - outliers; i++ ) {
float x = (cvRandReal(&rng)*2-1)*a;
float y = (cvRandReal(&rng)*2-1)*b;
points[i].x = cvRound(x*cos_a - y*sin_a + img->width/2);
points[i].y = cvRound(x*sin_a + y*cos_a + img->height/2);
}
// generate "completely off" points
for( ; i < count; i++ ) {
points[i].x = cvRandInt(&rng) % img->width;
points[i].y = cvRandInt(&rng) % img->height;
}
// find the optimal line
cvFitLine( &pointMat, CV_DIST_L1, 1, 0.001, 0.001, line );
cvZero( img );
// draw the points
for( i = 0; i < count; i++ ) {
cvCircle( img, points[i], 2, i < count - outliers ? CV_RGB(255, 0, 0) : CV_RGB(255,255,0), CV_FILLED, CV_AA, 0 );
}
d = sqrt((double)line[0]*line[0] + (double)line[1]*line[1]);
line[0] /= d;
line[1] /= d;
t = (float)(img->width + img->height);
pt1.x = cvRound(line[2] - line[0]*t);
pt1.y = cvRound(line[3] - line[1]*t);
pt2.x = cvRound(line[2] + line[0]*t);
pt2.y = cvRound(line[3] + line[1]*t);
cvLine( img, pt1, pt2, CV_RGB(0,255,0), 3, CV_AA, 0 );
cvShowImage( "fitline", img );
key = (char) cvWaitKey(0);
if( key == 27 ) break;
free( points );
}
cvDestroyWindow( "fitline" );
return 0;
}
int main(int argc, char **argv) {
// Check parameters
if (argc < 2) {
fprintf(stderr, "%s: %s\n", APP_NAME, "No video name given");
fprintf(stderr, "Usage: %s <video file name> [output file name]\n", APP_NAME);
exit(EXIT_FAILURE);
}
char *output_file_name;
if (argc == 3) {
output_file_name = argv[2];
}
else {
output_file_name = OUTPUT_FILE_NAME;
}
// Load video
char *file_name = argv[1];
CvCapture *video = cvCaptureFromFile(file_name);
if (!video) {
exit(EXIT_FAILURE);
}
// Extract video parameters
CvSize video_frame_size;
video_frame_size.width = cvGetCaptureProperty(video, CV_CAP_PROP_FRAME_WIDTH);
video_frame_size.height = cvGetCaptureProperty(video, CV_CAP_PROP_FRAME_HEIGHT);
double video_fps = cvGetCaptureProperty(video, CV_CAP_PROP_FPS);
long video_frame_count = cvGetCaptureProperty(video, CV_CAP_PROP_FRAME_COUNT);
// Initialize video writer
CvVideoWriter *video_writer = cvCreateVideoWriter(output_file_name,
FOURCC, video_fps, video_frame_size, true);
// Initialize variables for optical flow calculation
IplImage *current_frame = cvCreateImage(video_frame_size, IPL_DEPTH_8U, 3);
IplImage *eigen_image = cvCreateImage(video_frame_size, IPL_DEPTH_32F, 1);
IplImage *temp_image = cvCreateImage(video_frame_size, IPL_DEPTH_32F, 1);
int corner_count = MAX_CORNERS;
CvPoint2D32f corners[2][MAX_CORNERS];
char features_found[MAX_CORNERS];
float feature_errors[MAX_CORNERS];
IplImage *frame_buffer[2];
IplImage *pyramid_images[2];
CvSize pyramid_size = cvSize(video_frame_size.width + 8, video_frame_size.height / 3);
int i;
for (i = 0; i < 2; i++) {
frame_buffer[i] = cvCreateImage(video_frame_size, IPL_DEPTH_8U, 1);
pyramid_images[i] = cvCreateImage(pyramid_size, IPL_DEPTH_32F, 1);
}
// Process video
while (query_frame(video, frame_buffer, current_frame)) {
// Corner finding with Shi and Thomasi
cvGoodFeaturesToTrack(
frame_buffer[0],
eigen_image,
temp_image,
corners[0],
&corner_count,
0.01,
5.0,
0,
3,
0,
0.4);
cvFindCornerSubPix(
frame_buffer[0],
corners[0],
corner_count,
cvSize(WINDOW_SIZE, WINDOW_SIZE),
cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.3));
// Pyramid Lucas-Kanade
cvCalcOpticalFlowPyrLK(
frame_buffer[0],
frame_buffer[1],
pyramid_images[0],
pyramid_images[1],
corners[0],
corners[1],
corner_count,
cvSize(WINDOW_SIZE, WINDOW_SIZE),
5,
features_found,
feature_errors,
cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.3),
0);
// Draw optical flow vectors
int i;
double l_max, l;
for (i = 0; i < corner_count; i++) {
if (features_found[i] == 0 || feature_errors[i] > 550) {
continue;
}
l = sqrt(corners[1][i].x*corners[1][i].x+corners[1][i].y*corners[1][i].y);
if(l>l_max) l_max = l;
}
for (i = 0; i < corner_count; i++) {
if (features_found[i] == 0 || feature_errors[i] > 550) {
continue;
}
double spinSize = 5.0 * l/l_max;
CvPoint points[2];
points[0] = cvPoint(cvRound(corners[0][i].x), cvRound(corners[0][i].y));
points[1] = cvPoint(cvRound(corners[1][i].x), cvRound(corners[1][i].y));
cvLine(current_frame, points[0], points[1], CV_RGB(0, 255, 0), 1, 8, 0);
double angle;
angle = atan2( (double) points[0].y - points[1].y, (double) points[0].x - points[1].x );
points[0].x = (int) (points[1].x + spinSize * cos(angle + 3.1416 / 4));
points[0].y = (int) (points[1].y + spinSize * sin(angle + 3.1416 / 4));
cvLine(current_frame, points[0], points[1], CV_RGB(0, 255, 0), 1, 8, 0);
points[0].x = (int) (points[1].x + spinSize * cos(angle - 3.1416 / 4));
points[0].y = (int) (points[1].y + spinSize * sin(angle - 3.1416 / 4));
cvLine( current_frame, points[0], points[1], CV_RGB(0, 255, 0), 1, 8, 0);
}
cvWriteFrame(video_writer, current_frame);
}
// Clean up
cvReleaseImage(¤t_frame);
cvReleaseImage(&eigen_image);
cvReleaseImage(&temp_image);
for (i = 0; i < 2; i++) {
cvReleaseImage(&frame_buffer[0]);
cvReleaseImage(&pyramid_images[0]);
}
cvReleaseCapture(&video);
cvReleaseVideoWriter(&video_writer);
return 0;
}
void MCRenderer::showCurrentResult(const vector<vec3f>& pixelColors , unsigned* time , unsigned* iter)
{
IplImage* image = cvCreateImage(cvSize(renderer->camera.width, renderer->camera.height), IPL_DEPTH_32F, 3);
for(int x=0; x<renderer->camera.width; x++)
{
for(unsigned y=0; y<renderer->camera.height; y++)
{
vec3f rgb = pixelColors[y*renderer->camera.width + x];
vec3f &bgr = ((vec3f*)image->imageData)[y*renderer->camera.width + x];
bgr = vec3f(rgb.z, rgb.y, rgb.x);
}
}
cvSetMouseCallback("Renderer", mouseEvent, this);
//printf("savePath = %s\n" , savePath.c_str());
if(savePath != "")
{
saveImagePFM(savePath, image);
}
if (iter && time)
{
char timeStr[100] , iterStr[100];
memset(timeStr , 0 , sizeof(timeStr));
memset(iterStr , 0 , sizeof(iterStr));
sprintf(timeStr , "%d" , *time);
sprintf(iterStr , "%d" , *iter);
//itoa(*time , timeStr , 10);
//itoa(*iter , iterStr , 10);
string fileName(savePath);
for (int i = savePath.length() - 1; i >= 0; i--)
{
fileName.pop_back();
if (savePath[i] == '.')
break;
}
fileName += "_time_";
for (int i = 0; i < strlen(timeStr); i++)
fileName.push_back(timeStr[i]);
fileName += "_iter_";
for (int i = 0; i < strlen(iterStr); i++)
fileName.push_back(iterStr[i]);
fileName += ".pfm";
saveImagePFM(fileName , image);
printf("savePath = %s\n" , fileName.c_str());
}
for(int p=0; p<3*image->width*image->height; p++)
((float*)image->imageData)[p] = powf(((float*)image->imageData)[p], 1/2.2);
//cvShowImage("Renderer", image);
//response(image);
cvReleaseImage(&image);
switch(showEvent)
{
case SHOW_PATH:
if(showPath.size() > 1)
{
for(unsigned i=0; i<showPath.size()-1; i++)
{
vec3f point1 = showPath[i].origin;
vec3f point2 = showPath[i+1].origin;
vec2<float> pixel1 = renderer->getCamera().transToPixel(point1);
vec2<float> pixel2 = renderer->getCamera().transToPixel(point2);
CvPoint p1, p2;
p1.x = pixel1.x;
p1.y = pixel1.y;
p2.x = pixel2.x;
p2.y = pixel2.y;
if(i==0)
p1 = p2;
cvLine(image, p1, p2, cvScalar(0, 1, 0));
cvCircle(image, p2, 5, cvScalar(0, 0, 1), 2);
}
}
break;
default:
break;
/*case SHOW_PIXEL_VALUE:
printf("%lf, %lf, %lf\n", pixelColors[pathPixelID].x, pixelColors[pathPixelID].y, pixelColors[pathPixelID].z);
int x = pathPixelID % renderer->camera.width;
int y = pathPixelID / renderer->camera.width;
cvRectangle(image, cvPoint(x-1, y-1), cvPoint(x+1, y+1), cvScalar(0, 0, 1));
break;*/
}
}