void normCrossCorrelation(IplImage *imgI, IplImage *imgJ,
CvPoint2D32f *points0, CvPoint2D32f *points1, int nPts, char *status,
float *match, int winsize, int method) {
IplImage *rec0 = cvCreateImage(cvSize(winsize, winsize), 8, 1);
IplImage *rec1 = cvCreateImage(cvSize(winsize, winsize), 8, 1);
IplImage *res = cvCreateImage(cvSize(1, 1), IPL_DEPTH_32F, 1);
for (int i = 0; i < nPts; i++) {
if (status[i] == 1) {
cvGetRectSubPix(imgI, rec0, points0[i]);
cvGetRectSubPix(imgJ, rec1, points1[i]);
cvMatchTemplate(rec0, rec1, res, method);
match[i] = ((float *) (res->imageData))[0];
} else {
match[i] = 0.0;
}
}
cvReleaseImage(&rec0);
cvReleaseImage(&rec1);
cvReleaseImage(&res);
}
int opticaltri( CvMat * &clean_texture, int verts )
{
char * im1fname = "conhull-dirty-thresh.jpg";
char * im2fname = "conhull-clean-thresh.jpg";
int count = MAX_COUNT;
char * status;
CvPoint2D32f * source_points;
CvPoint2D32f * dest_points;
CvPoint2D32f * delaunay_points = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(CvPoint2D32f));
// count = opticalflow( im1fname, im2fname, source_points, dest_points, status );
count = findsiftpoints( "conhull-dirty.jpg", "conhull-clean.jpg", source_points, dest_points, status );
IplImage * image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_COLOR);
CvMemStorage * storage = cvCreateMemStorage(0);
CvSubdiv2D * delaunay = cvCreateSubdivDelaunay2D( cvRect(0,0,image1->width,image1->height), storage);
IplImage * image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_COLOR);
cvSet( image1, cvScalarAll(255) );
std::map<CvPoint, CvPoint> point_lookup_map;
std::vector<std::pair<CvPoint, CvPoint> > point_lookup;
int num_matches = 0;
int num_out_matches = 0;
int max_dist = 50;
int offset = 200;
// put corners in the point lookup as going to themselves
point_lookup_map[cvPoint(0,0)] = cvPoint(0,0);
point_lookup_map[cvPoint(0,image1->height-1)] = cvPoint(0,image1->height-1);
point_lookup_map[cvPoint(image1->width-1,0)] = cvPoint(image1->width-1,0);
point_lookup_map[cvPoint(image1->width-1,image1->height-1)] = cvPoint(image1->width-1,image1->height-1);
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(0,0), cvPoint(0,0)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(0,image1->height-1), cvPoint(0,image1->height-1)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(image1->width-1,0), cvPoint(image1->width-1,0)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(image1->width-1,image1->height-1), cvPoint(image1->width-1,image1->height-1)));
printf("Inserting corners...");
// put corners in the Delaunay subdivision
for(unsigned int i = 0; i < point_lookup.size(); i++) {
cvSubdivDelaunay2DInsert( delaunay, cvPointTo32f(point_lookup[i].first) );
}
printf("done.\n");
CvSubdiv2DEdge proxy_edge;
for(int i = 0; i < count; i++) {
if(status[i]) {
CvPoint source = cvPointFrom32f(source_points[i]);
CvPoint dest = cvPointFrom32f(dest_points[i]);
if((((int)fabs((double)(source.x - dest.x))) > max_dist) ||
(((int)fabs((double)(source.y - dest.y))) > max_dist)) {
num_out_matches++;
}
else if((dest.x >= 0) && (dest.y >= 0) && (dest.x < (image1->width)) && (dest.y < (image1->height))) {
if(point_lookup_map.find(source) == point_lookup_map.end()) {
num_matches++;
point_lookup_map[source] = dest;
point_lookup.push_back(std::pair<CvPoint,CvPoint>(source,dest));
// delaunay_points[i] =
(cvSubdivDelaunay2DInsert( delaunay, cvPointTo32f(source) ))->pt;
cvSetImageROI( image1, cvRect(source.x-8,source.y-8,8*2,8*2) );
cvResetImageROI( image2 );
cvGetRectSubPix( image2, image1, dest_points[i] );
}
/*
cvSet2D( image1, source.y, source.x, cvGet2D( image2, dest.y, dest.x ) );
cvSet2D( image1, source.y, source.x+1, cvGet2D( image2, dest.y, dest.x+1 ) );
cvSet2D( image1, source.y, source.x-1, cvGet2D( image2, dest.y, dest.x-1 ) );
cvSet2D( image1, source.y+1, source.x, cvGet2D( image2, dest.y+1, dest.x ) );
cvSet2D( image1, source.y-1, source.x, cvGet2D( image2, dest.y-1, dest.x ) );
cvSet2D( image1, source.y+1, source.x+1, cvGet2D( image2, dest.y+1, dest.x+1 ) );
cvSet2D( image1, source.y-1, source.x-1, cvGet2D( image2, dest.y-1, dest.x-1 ) );
cvSet2D( image1, source.y+1, source.x-1, cvGet2D( image2, dest.y+1, dest.x-1 ) );
cvSet2D( image1, source.y-1, source.x+1, cvGet2D( image2, dest.y-1, dest.x+1 ) );
*/
// cvCircle( image1, source, 4, CV_RGB(255,0,0), 2, CV_AA );
// cvCircle( image2, dest, 4, CV_RGB(255,0,0), 2, CV_AA );
}
/*
cvSetImageROI( image1, cvRect(source.x-offset,source.y-offset,offset*2,offset*2) );
cvSetImageROI( image2, cvRect(dest.x-offset,dest.y-offset,offset*2,offset*2) );
cvNamedWindow("image1",0);
cvNamedWindow("image2",0);
cvShowImage("image1",image1);
cvShowImage("image2",image2);
printf("%d,%d -> %d,%d\n",source.x,source.y,dest.x,dest.y);
cvWaitKey(0);
cvDestroyAllWindows();
*/
}
}
printf("%d %d\n",num_matches,num_out_matches);
printf("%d lookups\n",point_lookup_map.size());
cvResetImageROI( image1 );
cvSaveImage("sparse.jpg", image1);
cvReleaseImage(&image1);
image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_COLOR);
cvSet( image1, cvScalarAll(255) );
printf("Warping image...");
CvSeqReader reader;
int total = delaunay->edges->total;
int elem_size = delaunay->edges->elem_size;
std::vector<Triangle> trivec;
std::vector<CvMat *> baryinvvec;
for( int i = 0; i < total*2; i++ ) {
if((i == 0) || (i == total)) {
cvStartReadSeq( (CvSeq*)(delaunay->edges), &reader, 0 );
}
CvQuadEdge2D* edge = (CvQuadEdge2D*)(reader.ptr);
if( CV_IS_SET_ELEM( edge )) {
CvSubdiv2DEdge curedge = (CvSubdiv2DEdge)edge;
CvSubdiv2DEdge t = curedge;
Triangle temptri;
int count = 0;
// construct a triangle from this edge
do {
CvSubdiv2DPoint* pt = cvSubdiv2DEdgeOrg( t );
if(count < 3) {
pt->pt.x = pt->pt.x >= image1->width ? image1->width-1 : pt->pt.x;
pt->pt.y = pt->pt.y >= image1->height ? image1->height-1 : pt->pt.y;
pt->pt.x = pt->pt.x < 0 ? 0 : pt->pt.x;
pt->pt.y = pt->pt.y < 0 ? 0 : pt->pt.y;
temptri.points[count] = cvPointFrom32f( pt->pt );
}
else {
printf("More than 3 edges\n");
}
count++;
if(i < total)
t = cvSubdiv2DGetEdge( t, CV_NEXT_AROUND_LEFT );
else
t = cvSubdiv2DGetEdge( t, CV_NEXT_AROUND_RIGHT );
} while( t != curedge );
// check that triangle is not already in
if( std::find(trivec.begin(), trivec.end(), temptri) == trivec.end() ) {
// push triangle in and draw
trivec.push_back(temptri);
cvLine( image1, temptri.points[0], temptri.points[1], CV_RGB(255,0,0), 1, CV_AA, 0 );
cvLine( image1, temptri.points[1], temptri.points[2], CV_RGB(255,0,0), 1, CV_AA, 0 );
cvLine( image1, temptri.points[2], temptri.points[0], CV_RGB(255,0,0), 1, CV_AA, 0 );
// compute barycentric computation vector for this triangle
CvMat * barycen = cvCreateMat( 3, 3, CV_32FC1 );
CvMat * baryceninv = cvCreateMat( 3, 3, CV_32FC1 );
barycen->data.fl[3*0+0] = temptri.points[0].x;
barycen->data.fl[3*0+1] = temptri.points[1].x;
barycen->data.fl[3*0+2] = temptri.points[2].x;
barycen->data.fl[3*1+0] = temptri.points[0].y;
barycen->data.fl[3*1+1] = temptri.points[1].y;
barycen->data.fl[3*1+2] = temptri.points[2].y;
barycen->data.fl[3*2+0] = 1;
barycen->data.fl[3*2+1] = 1;
barycen->data.fl[3*2+2] = 1;
cvInvert( barycen, baryceninv, CV_LU );
baryinvvec.push_back(baryceninv);
cvReleaseMat( &barycen );
}
}
CV_NEXT_SEQ_ELEM( elem_size, reader );
}
printf("%d triangles...", trivec.size());
cvSaveImage("triangles.jpg", image1);
cvSet( image1, cvScalarAll(255) );
IplImage * clean_nonthresh = cvLoadImage( "conhull-clean.jpg", CV_LOAD_IMAGE_COLOR );
// for each triangle
for(unsigned int i = 0; i < trivec.size(); i++) {
Triangle curtri = trivec[i];
CvMat * curpoints = cvCreateMat( 1, 3, CV_32SC2 );
Triangle target;
std::map<CvPoint,CvPoint>::iterator piter[3];
printf("Triangle %d / %d\n",i,trivec.size());
int is_corner = 0;
for(int j = 0; j < 3; j++) {
/*
curpoints->data.i[2*j+0] = curtri.points[j].x;
curpoints->data.i[2*j+1] = curtri.points[j].y;
*/
CV_MAT_ELEM( *curpoints, CvPoint, 0, j ) = curtri.points[j];
printf("%d,%d\n",curtri.points[j].x,curtri.points[j].y);
if((curtri.points[j] == cvPoint(0,0)) || (curtri.points[j] == cvPoint(0,image1->height - 1)) ||(curtri.points[j] == cvPoint(image1->width - 1,0)) ||(curtri.points[j] == cvPoint(image1->width - 1,image1->height - 1))) {
is_corner++;
}
for(unsigned int k = 0; k < point_lookup.size(); k++) {
std::pair<CvPoint,CvPoint> thispair = point_lookup[k];
if(thispair.first == curtri.points[j]) {
target.points[j] = thispair.second;
break;
}
}
/*
piter[j] = point_lookup_map.find(curtri.points[j]);
if(piter[j] != point_lookup_map.end() ) {
target.points[j] = piter[j]->second;
}
*/
}
// if((piter[0] != point_lookup_map.end()) && (piter[1] != point_lookup_map.end()) && (piter[2] != point_lookup_map.end())) {
if(is_corner < 3) {
CvMat * newcorners = cvCreateMat( 3, 3, CV_32FC1 );
newcorners->data.fl[3*0+0] = target.points[0].x;
newcorners->data.fl[3*0+1] = target.points[1].x;
newcorners->data.fl[3*0+2] = target.points[2].x;
newcorners->data.fl[3*1+0] = target.points[0].y;
newcorners->data.fl[3*1+1] = target.points[1].y;
newcorners->data.fl[3*1+2] = target.points[2].y;
newcorners->data.fl[3*2+0] = 1;
newcorners->data.fl[3*2+1] = 1;
newcorners->data.fl[3*2+2] = 1;
CvContour hdr;
CvSeqBlock blk;
CvRect trianglebound = cvBoundingRect( cvPointSeqFromMat(CV_SEQ_KIND_CURVE+CV_SEQ_FLAG_CLOSED, curpoints, &hdr, &blk), 1 );
printf("Bounding box: %d,%d,%d,%d\n",trianglebound.x,trianglebound.y,trianglebound.width,trianglebound.height);
for(int y = trianglebound.y; (y < (trianglebound.y + trianglebound.height)) && ( y < image1->height); y++) {
for(int x = trianglebound.x; (x < (trianglebound.x + trianglebound.width)) && (x < image1->width); x++) {
// check to see if we're inside this triangle
/*
CvPoint v0 = cvPoint( curtri.points[2].x - curtri.points[0].x, curtri.points[2].y - curtri.points[0].y );
CvPoint v1 = cvPoint( curtri.points[1].x - curtri.points[0].x, curtri.points[1].y - curtri.points[0].y );
CvPoint v2 = cvPoint( x - curtri.points[0].x, y - curtri.points[0].y );
int dot00 = v0.x * v0.x + v0.y * v0. y;
int dot01 = v0.x * v1.x + v0.y * v1. y;
int dot02 = v0.x * v2.x + v0.y * v2. y;
int dot11 = v1.x * v1.x + v1.y * v1. y;
int dot12 = v1.x * v2.x + v1.y * v2. y;
double invDenom = 1.0 / (double)(dot00 * dot11 - dot01 * dot01);
double u = (double)(dot11 * dot02 - dot01 * dot12) * invDenom;
double v = (double)(dot00 * dot12 - dot01 * dot02) * invDenom;
*/
CvMat * curp = cvCreateMat(3, 1, CV_32FC1);
CvMat * result = cvCreateMat(3, 1, CV_32FC1);
curp->data.fl[0] = x;
curp->data.fl[1] = y;
curp->data.fl[2] = 1;
cvMatMul( baryinvvec[i], curp, result );
// double u = result->data.fl[0]/result->data.fl[2];
// double v = result->data.fl[1]/result->data.fl[2];
/*
if((i == 3019) && (y == 1329) && (x > 2505) && (x < 2584)) {
printf("Range %d: %f, %f, %f\t%f, %f, %f\n",x,result->data.fl[0],result->data.fl[1],result->data.fl[2],
sourcepoint->data.fl[0],sourcepoint->data.fl[1],sourcepoint->data.fl[2]);
}
*/
if( (result->data.fl[0] > MIN_VAL) && (result->data.fl[1] > MIN_VAL) && (result->data.fl[2] > MIN_VAL) && (fabs(1.0 - (result->data.fl[0]+result->data.fl[1]+result->data.fl[2])) <= 0.01) ) {
// if((u > 0) || (v > 0) /*&& ((u +v) < 1)*/ )
// printf("Barycentric: %f %f %f\n", result->data.fl[0], result->data.fl[1], result->data.fl[2]);
// this point is inside this triangle
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
CvMat * sourcepoint = cvCreateMat(3, 1, CV_32FC1);
cvMatMul( newcorners, result, sourcepoint );
double sourcex = sourcepoint->data.fl[0]/*/sourcepoint->data.fl[2]*/;
double sourcey = sourcepoint->data.fl[1]/*/sourcepoint->data.fl[2]*/;
if((sourcex >= 0) && (sourcey >= 0) && (sourcex < (image1->width)) && (sourcey < (image1->height))) {
// printf("%d,%d %d,%d\n",x,y,(int)sourcex,(int)sourcey);
cvSet2D( image1, y, x, cvGet2D( clean_nonthresh, (int)sourcey, (int)sourcex ) );
}
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
cvReleaseMat( &sourcepoint );
}
cvReleaseMat( &result );
cvReleaseMat( &curp );
}
}
for(int k = 0; k < verts; k++) {
double x = clean_texture->data.fl[2*k+0];
double y = clean_texture->data.fl[2*k+1];
// check to see if we're inside this triangle
CvMat * curp = cvCreateMat(3, 1, CV_32FC1);
CvMat * result = cvCreateMat(3, 1, CV_32FC1);
curp->data.fl[0] = x;
curp->data.fl[1] = y;
curp->data.fl[2] = 1;
cvMatMul( baryinvvec[i], curp, result );
if( (result->data.fl[0] > MIN_VAL) && (result->data.fl[1] > MIN_VAL) && (result->data.fl[2] > MIN_VAL) && (fabs(1.0 - (result->data.fl[0]+result->data.fl[1]+result->data.fl[2])) <= 0.01) ) {
CvMat * sourcepoint = cvCreateMat(3, 1, CV_32FC1);
cvMatMul( newcorners, result, sourcepoint );
double sourcex = sourcepoint->data.fl[0]/*/sourcepoint->data.fl[2]*/;
double sourcey = sourcepoint->data.fl[1]/*/sourcepoint->data.fl[2]*/;
if((sourcex >= 0) && (sourcey >= 0) && (sourcex < (image1->width)) && (sourcey < (image1->height))) {
clean_texture->data.fl[2*k+0] = sourcex;
clean_texture->data.fl[2*k+1] = sourcey;
// cvSet2D( image1, y, x, cvGet2D( clean_nonthresh, (int)sourcey, (int)sourcex ) );
}
cvReleaseMat( &sourcepoint );
}
cvReleaseMat( &result );
cvReleaseMat( &curp );
}
cvReleaseMat( &newcorners );
}
cvReleaseMat( &curpoints );
}
cvReleaseImage( &clean_nonthresh );
printf("done.\n");
cvSaveImage("fullwarp.jpg", image1);
printf("Drawing subdivisions on warped image...");
draw_subdiv( image1, delaunay, NULL, NULL, 0, NULL );
// draw_subdiv( image1, delaunay, delaunay_points, source_points, count, status );
printf("done.\n");
cvSaveImage("edgeswarp.jpg", image1);
cvReleaseImage(&image2);
image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_COLOR);
// cvCreateImage( cvGetSize(image2), IPL_DEPTH_8U, 3 );
// cvCalcSubdivVoronoi2D( delaunay );
printf("Drawing subdivisions on unwarped image...");
// draw_subdiv( image2, delaunay, delaunay_points, dest_points, count, status );
// draw_subdiv( image2, delaunay, NULL, NULL, 0, NULL );
printf("done.\n");
cvSaveImage("edges.jpg",image2);
cvReleaseImage(&image1);
cvFree(&source_points);
cvFree(&dest_points);
cvFree(&status);
cvReleaseMemStorage(&storage);
cvFree(&delaunay_points);
cvReleaseImage(&image2);
return 0;
}
/* main code */
int main(int argc, char** argv ){
CvCapture *capture;
int i, key;
//*struct tm *newtime;
time_t second,milsecond;
char rightfilename[100], leftfilename[100];
// ブーストされた分類器のカスケードを読み込む
// cascade = (CvHaarClassifierCascade *) cvLoad (cascade_name, 0, 0, 0);
righteye_cascade = (CvHaarClassifierCascade *) cvLoad (righteye_cascade_name, 0, 0, 0);
lefteye_cascade = (CvHaarClassifierCascade *) cvLoad (lefteye_cascade_name, 0, 0, 0);
/* initialize camera */
capture = cvCaptureFromCAM( 0 );
/* always check */
if( !capture ) return 1;
/* get video properties, needed by template image */
frame = cvQueryFrame( capture );
if ( !frame ) return 1;
/* create template image */
tpl = cvCreateImage( cvSize( TPL_WIDTH, TPL_HEIGHT ),
frame->depth, frame->nChannels );
/* create image for template matching result */
tm = cvCreateImage( cvSize( WINDOW_WIDTH - TPL_WIDTH + 1,
WINDOW_HEIGHT - TPL_HEIGHT + 1 ),
IPL_DEPTH_32F, 1 );
//eyezone
eyezone1 = cvCreateImage(cvSize(50,50), IPL_DEPTH_8U, 1);
minieyezone1 = cvCreateImage(cvSize(16,16), IPL_DEPTH_8U, 1);
output1 = cvCreateImage(cvSize(512,512), IPL_DEPTH_8U, 1);
eyezone2 = cvCreateImage(cvSize(50,50), IPL_DEPTH_8U, 1);
minieyezone2 = cvCreateImage(cvSize(16,16), IPL_DEPTH_8U, 1);
output2 = cvCreateImage(cvSize(512,512), IPL_DEPTH_8U, 1);
/* create a window and install mouse handler */
cvNamedWindow( "video", CV_WINDOW_AUTOSIZE );
cvSetMouseCallback( "video", mouseHandler, NULL );
cvNamedWindow("output1", CV_WINDOW_AUTOSIZE);
cvNamedWindow("output2", CV_WINDOW_AUTOSIZE);
gray = cvCreateImage (cvGetSize (frame), IPL_DEPTH_8U, 1);
righteye_storage = cvCreateMemStorage (0);
lefteye_storage = cvCreateMemStorage (0);
CvPoint righteye_center, lefteye_center;
// eye candidate
CvRect righteye_cand1, righteye_cand2, lefteye_cand1, lefteye_cand2, right, left;
int eye_candidate_num = 0;
while( key != 'q' ) {
eye_candidate_num = 0;
/* get a frame */
frame = cvQueryFrame( capture );
/* always check */
if( !frame ) break;
/* 'fix' frame */
/* cvFlip( frame, frame, -1 ); */
frame->origin = 0;
/* perform tracking if template is available */
if( is_tracking ) trackObject();
cvClearMemStorage (righteye_storage);
cvClearMemStorage (lefteye_storage);
cvCvtColor (frame, gray, CV_BGR2GRAY);
cvEqualizeHist (gray, gray);
righteye = cvHaarDetectObjects (gray, righteye_cascade, righteye_storage, 1.11, 4, 0, cvSize (40, 40), cvSize(40,40));
lefteye = cvHaarDetectObjects (gray, lefteye_cascade, lefteye_storage, 1.11, 4, 0, cvSize (40, 40), cvSize(40,40));
//右目を円で描画
for (i = 0; i < (righteye ? righteye->total : 0); i++) {
CvRect *r = (CvRect *) cvGetSeqElem (righteye, i);
CvPoint center;
int radius;
center.x = cvRound (r->x + r->width * 0.5);
center.y = cvRound (r->y + r->height * 0.5);
radius = cvRound ((r->width + r->height) * 0.25);
cvCircle (frame, center, radius, colors[i % 8], 3, 8, 0);
//右目候補
if(i == 0){
righteye_cand1 = *r;
}
if(i == 1){
righteye_cand2 = *r;
}
}
//左目を死角で描画
for (i = 0; i < (lefteye ? lefteye->total : 0); i++) {
CvRect *r = (CvRect *) cvGetSeqElem (lefteye, i);
CvPoint apex1, apex2;
apex1 = cvPoint(r->x, r->y);
apex2.x = cvRound(r->x + r->width);
apex2.y = cvRound(r->y + r->height);
cvRectangle (frame,apex1, apex2, colors[i % 8], 3, 8, 0);
//左目候補
if(i == 0){
lefteye_cand1 = *r;
}
if(i == 1){
lefteye_cand2 = *r;
}
}
//候補しぼり
if(righteye->total >= 1){
if(righteye->total >= 2){
if(righteye_cand1.x <= righteye_cand2.x){
right = righteye_cand1;
righteye_center.x = cvRound(right.x + right.width*0.5);
righteye_center.y = cvRound(right.y + right.height*0.5);
}
else{
right = righteye_cand2;
righteye_center.x = cvRound(right.x + right.width*0.5);
righteye_center.y = cvRound(right.y + right.height*0.5);
}
}
else{
right = righteye_cand1;
righteye_center.x = cvRound(right.x + right.width*0.5);
righteye_center.y = cvRound(right.y + right.height*0.5);
}
eyezone1 = cvCreateImage(cvSize(right.width, right.height), IPL_DEPTH_8U, 1);
cvGetRectSubPix(gray, eyezone1, cvPointTo32f(righteye_center));
cvEqualizeHist(eyezone1, eyezone1);
cvResize(eyezone1, minieyezone1, CV_INTER_LINEAR);
cvResize(minieyezone1, output1, CV_INTER_NN);
}
if(lefteye->total >= 1){
if(lefteye->total >= 2){
if(lefteye_cand1.x >= lefteye_cand2.x){
left = lefteye_cand1;
lefteye_center.x = cvRound(left.x + left.width*0.5);
lefteye_center.y = cvRound(left.y + left.height*0.5);
}
else{
left = lefteye_cand2;
lefteye_center.x = cvRound(left.x + left.width*0.5);
lefteye_center.y = cvRound(right.y + left.height*0.5);
}
}
else{
left = lefteye_cand1;
lefteye_center.x = cvRound(left.x + left.width*0.5);
lefteye_center.y = cvRound(left.y + left.height*0.5);
}
eyezone2 = cvCreateImage(cvSize(left.width, left.height), IPL_DEPTH_8U, 1);
cvGetRectSubPix(gray, eyezone2, cvPointTo32f(lefteye_center));
cvEqualizeHist(eyezone2, eyezone2);
cvResize(eyezone2, minieyezone2, CV_INTER_LINEAR);
cvResize(minieyezone2, output2, CV_INTER_NN);
}
printf("righteye width = %d, height = %d\n", right.width, right.height);
printf("lefteye width = %d, height = %d\n", left.width, left.height);
// printf("righteye x = %d\n", right.x);
// printf("lefteye x = %d\n", left.x);
/* display frame */
cvShowImage( "video", frame);
//cvShowImage( "eyezone1", eyezone1);
//cvShowImage( "eyezone2", eyezone2);
cvShowImage( "output1", output1);
cvShowImage( "output2", output2);
//ファイル出力,時間計測
time(&second);
milsecond = clock();
// printf("時間[sec] = %ld\n", second);
printf("経過時間[usec] = %ld\n", milsecond);
//sprintf(filename, "%ld.bmp",second);
//printf("sprintf = %s\n", filename);
//cvSaveImage(filename, frame,0);
if(key == 'n'){
sprintf(rightfilename, "n_right%ld.bmp", milsecond);
sprintf(leftfilename, "n_left%ld.bmp", milsecond);
printf("fileoutput %s, %s\n", rightfilename, leftfilename);
cvSaveImage(rightfilename, minieyezone1, 0);
cvSaveImage(leftfilename, minieyezone2, 0);
}
if(key == 'h'){
sprintf(rightfilename, "h_right%ld.bmp", milsecond);
sprintf(leftfilename, "h_left%ld.bmp", milsecond);
printf("fileoutput %s, %s\n", rightfilename, leftfilename);
cvSaveImage(rightfilename, minieyezone1, 0);
cvSaveImage(leftfilename, minieyezone2, 0);
}
if(key == 'j'){
sprintf(rightfilename, "j_right%ld.bmp", milsecond);
sprintf(leftfilename, "j_left%ld.bmp", milsecond);
printf("fileoutput %s, %s\n", rightfilename, leftfilename);
cvSaveImage(rightfilename, minieyezone1, 0);
cvSaveImage(leftfilename, minieyezone2, 0);
}
if(key == 'k'){
sprintf(rightfilename, "k_right%ld.bmp", milsecond);
sprintf(leftfilename, "k_left%ld.bmp", milsecond);
printf("fileoutput %s, %s\n", rightfilename, leftfilename);
cvSaveImage(rightfilename, minieyezone1, 0);
cvSaveImage(leftfilename, minieyezone2, 0);
}
if(key == 'l'){
sprintf(rightfilename, "l_right%ld.bmp", milsecond);
sprintf(leftfilename, "l_left%ld.bmp", milsecond);
printf("fileoutput %s, %s\n", rightfilename, leftfilename);
cvSaveImage(rightfilename, minieyezone1, 0);
cvSaveImage(leftfilename, minieyezone2, 0);
}
/* exit if user press 'q' */
key = cvWaitKey( 1 );
}
/* free memory */
cvDestroyWindow( "video" );
cvDestroyWindow( "output1");
cvDestroyWindow( "output2");
cvReleaseCapture( &capture );
cvReleaseImage( &tpl );
cvReleaseImage( &tm );
cvReleaseImage( &gray);
cvReleaseImage( &eyezone1);
cvReleaseImage( &eyezone2);
cvReleaseImage( &minieyezone1);
cvReleaseImage( &minieyezone2);
cvReleaseImage( &output1);
cvReleaseImage( &output2);
return 0;
}
int main(int argc, char * argv[])
{
if(argc < 2) {
fprintf(stderr, "%s image1 image2\n", argv[0]);
return 1;
}
char * im1fname = argv[1];
char * im2fname = argv[2];
IplImage * image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_GRAYSCALE);
IplImage * eigenvalues = cvCreateImage(cvGetSize(image1), 32, 1);
IplImage * temp = cvCreateImage(cvGetSize(image1), 32, 1);
int count = MAX_COUNT;
double quality = 0.5;
// double min_distance = 2;
double min_distance = 50;
int block_size = 7;
int use_harris = 0;
int win_size = 10;
int flags = 0;
CvPoint2D32f * source_points = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(CvPoint2D32f));
CvPoint2D32f * dest_points = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(CvPoint2D32f));
CvPoint2D32f * delaunay_points = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(CvPoint2D32f));
cvGoodFeaturesToTrack( image1, eigenvalues, temp, source_points, &count,
quality, min_distance, 0, block_size, use_harris, 0.04 );
printf("%d features\n",count);
setbuf(stdout, NULL);
printf("Finding corner subpix...");
cvFindCornerSubPix( image1, source_points, count,
cvSize(win_size,win_size), cvSize(-1,-1),
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03));
printf("done.\n");
cvReleaseImage(&eigenvalues);
cvReleaseImage(&temp);
IplImage * image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_GRAYSCALE);
char * status = (char*)cvAlloc(sizeof(char)*MAX_COUNT);
IplImage * pyramid = cvCreateImage( cvGetSize(image1), IPL_DEPTH_8U, 1 );
IplImage * second_pyramid = cvCreateImage( cvGetSize(image2), IPL_DEPTH_8U, 1 );
printf("Computing optical flow...");
cvCalcOpticalFlowPyrLK(image1, image2, pyramid, second_pyramid, source_points,
dest_points, count, cvSize(win_size,win_size), 4, status, 0,
cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03),
flags);
printf("done.\n");
int num_matches = 0;
int num_out_matches = 0;
int max_dist = 30;
int offset = 200;
CvMemStorage * storage = cvCreateMemStorage(0);
CvSubdiv2D * delaunay = cvCreateSubdivDelaunay2D( cvRect(0,0,image1->width,image1->height), storage);
cvReleaseImage(&image1);
cvReleaseImage(&image2);
image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_COLOR);
image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_COLOR);
cvSet( image1, cvScalarAll(255) );
std::map<CvPoint, CvPoint> point_lookup_map;
std::vector<std::pair<CvPoint, CvPoint> > point_lookup;
// put corners in the point lookup as going to themselves
point_lookup_map[cvPoint(0,0)] = cvPoint(0,0);
point_lookup_map[cvPoint(0,image1->height-1)] = cvPoint(0,image1->height-1);
point_lookup_map[cvPoint(image1->width-1,0)] = cvPoint(image1->width-1,0);
point_lookup_map[cvPoint(image1->width-1,image1->height-1)] = cvPoint(image1->width-1,image1->height-1);
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(0,0), cvPoint(0,0)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(0,image1->height-1), cvPoint(0,image1->height-1)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(image1->width-1,0), cvPoint(image1->width-1,0)));
point_lookup.push_back(std::pair<CvPoint,CvPoint>(cvPoint(image1->width-1,image1->height-1), cvPoint(image1->width-1,image1->height-1)));
printf("Inserting corners...");
// put corners in the Delaunay subdivision
for(unsigned int i = 0; i < point_lookup.size(); i++) {
cvSubdivDelaunay2DInsert( delaunay, cvPointTo32f(point_lookup[i].first) );
}
printf("done.\n");
CvSubdiv2DEdge proxy_edge;
for(int i = 0; i < count; i++) {
if(status[i]) {
CvPoint source = cvPointFrom32f(source_points[i]);
CvPoint dest = cvPointFrom32f(dest_points[i]);
if((((int)fabs((double)(source.x - dest.x))) > max_dist) ||
(((int)fabs((double)(source.y - dest.y))) > max_dist)) {
num_out_matches++;
}
else if((dest.x >= 0) && (dest.y >= 0) && (dest.x < (image1->width)) && (dest.y < (image1->height))) {
if(point_lookup_map.find(source) == point_lookup_map.end()) {
num_matches++;
point_lookup_map[source] = dest;
point_lookup.push_back(std::pair<CvPoint,CvPoint>(source,dest));
delaunay_points[i] = (cvSubdivDelaunay2DInsert( delaunay, cvPointTo32f(source) ))->pt;
cvSetImageROI( image1, cvRect(source.x-8,source.y-8,8*2,8*2) );
cvResetImageROI( image2 );
cvGetRectSubPix( image2, image1, dest_points[i] );
}
/*
cvSet2D( image1, source.y, source.x, cvGet2D( image2, dest.y, dest.x ) );
cvSet2D( image1, source.y, source.x+1, cvGet2D( image2, dest.y, dest.x+1 ) );
cvSet2D( image1, source.y, source.x-1, cvGet2D( image2, dest.y, dest.x-1 ) );
cvSet2D( image1, source.y+1, source.x, cvGet2D( image2, dest.y+1, dest.x ) );
cvSet2D( image1, source.y-1, source.x, cvGet2D( image2, dest.y-1, dest.x ) );
cvSet2D( image1, source.y+1, source.x+1, cvGet2D( image2, dest.y+1, dest.x+1 ) );
cvSet2D( image1, source.y-1, source.x-1, cvGet2D( image2, dest.y-1, dest.x-1 ) );
cvSet2D( image1, source.y+1, source.x-1, cvGet2D( image2, dest.y+1, dest.x-1 ) );
cvSet2D( image1, source.y-1, source.x+1, cvGet2D( image2, dest.y-1, dest.x+1 ) );
*/
// cvCircle( image1, source, 4, CV_RGB(255,0,0), 2, CV_AA );
// cvCircle( image2, dest, 4, CV_RGB(255,0,0), 2, CV_AA );
}
/*
cvSetImageROI( image1, cvRect(source.x-offset,source.y-offset,offset*2,offset*2) );
cvSetImageROI( image2, cvRect(dest.x-offset,dest.y-offset,offset*2,offset*2) );
cvNamedWindow("image1",0);
cvNamedWindow("image2",0);
cvShowImage("image1",image1);
cvShowImage("image2",image2);
printf("%d,%d -> %d,%d\n",source.x,source.y,dest.x,dest.y);
cvWaitKey(0);
cvDestroyAllWindows();
*/
}
}
printf("%d %d\n",num_matches,num_out_matches);
printf("%d lookups\n",point_lookup_map.size());
cvResetImageROI( image1 );
cvSaveImage("sparse.jpg", image1);
cvReleaseImage(&image1);
image1 = cvLoadImage(im1fname, CV_LOAD_IMAGE_COLOR);
cvSet( image1, cvScalarAll(255) );
printf("Warping image...");
CvSeqReader reader;
int total = delaunay->edges->total;
int elem_size = delaunay->edges->elem_size;
cvStartReadSeq( (CvSeq*)(delaunay->edges), &reader, 0 );
std::vector<Triangle> trivec;
std::vector<CvMat *> baryinvvec;
for( int i = 0; i < total; i++ ) {
CvQuadEdge2D* edge = (CvQuadEdge2D*)(reader.ptr);
if( CV_IS_SET_ELEM( edge )) {
CvSubdiv2DEdge curedge = (CvSubdiv2DEdge)edge;
CvSubdiv2DEdge t = curedge;
Triangle temptri;
int count = 0;
// construct a triangle from this edge
do {
CvSubdiv2DPoint* pt = cvSubdiv2DEdgeOrg( t );
if(count < 3) {
pt->pt.x = pt->pt.x >= image1->width ? image1->width-1 : pt->pt.x;
pt->pt.y = pt->pt.y >= image1->height ? image1->height-1 : pt->pt.y;
pt->pt.x = pt->pt.x < 0 ? 0 : pt->pt.x;
pt->pt.y = pt->pt.y < 0 ? 0 : pt->pt.y;
temptri.points[count] = cvPointFrom32f( pt->pt );
}
else {
printf("More than 3 edges\n");
}
count++;
t = cvSubdiv2DGetEdge( t, CV_NEXT_AROUND_LEFT );
} while( t != curedge );
// check that triangle is not already in
if( std::find(trivec.begin(), trivec.end(), temptri) == trivec.end() ) {
// push triangle in and draw
trivec.push_back(temptri);
cvLine( image1, temptri.points[0], temptri.points[1], CV_RGB(255,0,0), 1, CV_AA, 0 );
cvLine( image1, temptri.points[1], temptri.points[2], CV_RGB(255,0,0), 1, CV_AA, 0 );
cvLine( image1, temptri.points[2], temptri.points[0], CV_RGB(255,0,0), 1, CV_AA, 0 );
// compute barycentric computation vector for this triangle
CvMat * barycen = cvCreateMat( 3, 3, CV_32FC1 );
CvMat * baryceninv = cvCreateMat( 3, 3, CV_32FC1 );
barycen->data.fl[3*0+0] = temptri.points[0].x;
barycen->data.fl[3*0+1] = temptri.points[1].x;
barycen->data.fl[3*0+2] = temptri.points[2].x;
barycen->data.fl[3*1+0] = temptri.points[0].y;
barycen->data.fl[3*1+1] = temptri.points[1].y;
barycen->data.fl[3*1+2] = temptri.points[2].y;
barycen->data.fl[3*2+0] = 1;
barycen->data.fl[3*2+1] = 1;
barycen->data.fl[3*2+2] = 1;
cvInvert( barycen, baryceninv, CV_LU );
baryinvvec.push_back(baryceninv);
cvReleaseMat( &barycen );
}
}
CV_NEXT_SEQ_ELEM( elem_size, reader );
}
printf("%d triangles...", trivec.size());
cvSaveImage("triangles.jpg", image1);
cvSet( image1, cvScalarAll(255) );
IplImage * clean_nonthresh = cvLoadImage( "conhull-clean.jpg", CV_LOAD_IMAGE_COLOR );
// for each triangle
for(unsigned int i = 0; i < trivec.size(); i++) {
Triangle curtri = trivec[i];
CvMat * curpoints = cvCreateMat( 1, 3, CV_32SC2 );
Triangle target;
std::map<CvPoint,CvPoint>::iterator piter[3];
printf("Triangle %d / %d\n",i,trivec.size());
bool is_corner = false;
for(int j = 0; j < 3; j++) {
/*
curpoints->data.i[2*j+0] = curtri.points[j].x;
curpoints->data.i[2*j+1] = curtri.points[j].y;
*/
CV_MAT_ELEM( *curpoints, CvPoint, 0, j ) = curtri.points[j];
printf("%d,%d\n",curtri.points[j].x,curtri.points[j].y);
/*
if((curtri.points[j] == cvPoint(0,0)) || (curtri.points[j] == cvPoint(0,image1->height)) ||(curtri.points[j] == cvPoint(image1->width,0)) ||(curtri.points[j] == cvPoint(image1->width,image1->height))) {
is_corner = true;
break;
}
*/
for(unsigned int k = 0; k < point_lookup.size(); k++) {
std::pair<CvPoint,CvPoint> thispair = point_lookup[k];
if(thispair.first == curtri.points[j]) {
target.points[j] = thispair.second;
break;
}
}
/*
piter[j] = point_lookup_map.find(curtri.points[j]);
if(piter[j] != point_lookup_map.end() ) {
target.points[j] = piter[j]->second;
}
*/
}
// if((piter[0] != point_lookup_map.end()) && (piter[1] != point_lookup_map.end()) && (piter[2] != point_lookup_map.end())) {
if(!is_corner) {
CvMat * newcorners = cvCreateMat( 3, 3, CV_32FC1 );
newcorners->data.fl[3*0+0] = target.points[0].x;
newcorners->data.fl[3*0+1] = target.points[1].x;
newcorners->data.fl[3*0+2] = target.points[2].x;
newcorners->data.fl[3*1+0] = target.points[0].y;
newcorners->data.fl[3*1+1] = target.points[1].y;
newcorners->data.fl[3*1+2] = target.points[2].y;
newcorners->data.fl[3*2+0] = 1;
newcorners->data.fl[3*2+1] = 1;
newcorners->data.fl[3*2+2] = 1;
CvContour hdr;
CvSeqBlock blk;
CvRect trianglebound = cvBoundingRect( cvPointSeqFromMat(CV_SEQ_KIND_CURVE+CV_SEQ_FLAG_CLOSED, curpoints, &hdr, &blk), 1 );
printf("Bounding box: %d,%d,%d,%d\n",trianglebound.x,trianglebound.y,trianglebound.width,trianglebound.height);
for(int y = trianglebound.y; (y < (trianglebound.y + trianglebound.height)) && ( y < image1->height); y++) {
for(int x = trianglebound.x; (x < (trianglebound.x + trianglebound.width)) && (x < image1->width); x++) {
// check to see if we're inside this triangle
/*
CvPoint v0 = cvPoint( curtri.points[2].x - curtri.points[0].x, curtri.points[2].y - curtri.points[0].y );
CvPoint v1 = cvPoint( curtri.points[1].x - curtri.points[0].x, curtri.points[1].y - curtri.points[0].y );
CvPoint v2 = cvPoint( x - curtri.points[0].x, y - curtri.points[0].y );
int dot00 = v0.x * v0.x + v0.y * v0. y;
int dot01 = v0.x * v1.x + v0.y * v1. y;
int dot02 = v0.x * v2.x + v0.y * v2. y;
int dot11 = v1.x * v1.x + v1.y * v1. y;
int dot12 = v1.x * v2.x + v1.y * v2. y;
double invDenom = 1.0 / (double)(dot00 * dot11 - dot01 * dot01);
double u = (double)(dot11 * dot02 - dot01 * dot12) * invDenom;
double v = (double)(dot00 * dot12 - dot01 * dot02) * invDenom;
*/
CvMat * curp = cvCreateMat(3, 1, CV_32FC1);
CvMat * result = cvCreateMat(3, 1, CV_32FC1);
curp->data.fl[0] = x;
curp->data.fl[1] = y;
curp->data.fl[2] = 1;
cvMatMul( baryinvvec[i], curp, result );
// double u = result->data.fl[0]/result->data.fl[2];
// double v = result->data.fl[1]/result->data.fl[2];
if( (result->data.fl[0] > 0) && (result->data.fl[1] > 0) && (fabs(1.0 - (result->data.fl[0]+result->data.fl[1]+result->data.fl[2])) <= 0.01) ) {
// if((u > 0) || (v > 0) /*&& ((u +v) < 1)*/ ) {
// printf("Barycentric: %f %f %f\n", result->data.fl[0], result->data.fl[1], result->data.fl[2]);
// this point is inside this triangle
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
CvMat * sourcepoint = cvCreateMat(3, 1, CV_32FC1);
cvMatMul( newcorners, result, sourcepoint );
double sourcex = sourcepoint->data.fl[0]/*/sourcepoint->data.fl[2]*/;
double sourcey = sourcepoint->data.fl[1]/*/sourcepoint->data.fl[2]*/;
if((sourcex >= 0) && (sourcey >= 0) && (sourcex < (image1->width)) && (sourcey < (image1->height))) {
// printf("%d,%d %d,%d\n",x,y,(int)sourcex,(int)sourcey);
cvSet2D( image1, y, x, cvGet2D( clean_nonthresh, (int)sourcey, (int)sourcex ) );
}
/*
if((i == 143) && (y == 3577) && (x > 2055) && (x < 2087)) {
printf("%d: %f, %f, %f\t%f, %f, %f\n",x,result->data.fl[0],result->data.fl[1],result->data.fl[2],
sourcepoint->data.fl[0],sourcepoint->data.fl[1],sourcepoint->data.fl[2]);
}
*/
cvReleaseMat( &sourcepoint );
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
}
cvReleaseMat( &result );
cvReleaseMat( &curp );
}
}
cvReleaseMat( &newcorners );
}
cvReleaseMat( &curpoints );
}
/*
for(int y = 0; y < image1->height; y++) {
for(int x = 0; x < image1->width; x++) {
CvMat * curp = cvCreateMat(3, 1, CV_32FC1);
CvMat * result = cvCreateMat(3, 1, CV_32FC1);
curp->data.fl[0] = x;
curp->data.fl[1] = y;
curp->data.fl[2] = 1;
for(unsigned int i = 0; i < baryinvvec.size(); i++) {
cvMatMul( baryinvvec[i], curp, result );
double u = result->data.fl[0]/result->data.fl[2];
double v = result->data.fl[1]/result->data.fl[2];
if((u > 0) && (v > 0) && (u + v < 1)) {
// printf("Point %d,%d inside %d,%d %d,%d %d,%d\n",x,y,trivec[i].points[0].x,trivec[i].points[0].y,
// trivec[i].points[1].x,trivec[i].points[1].y,trivec[i].points[2].x,trivec[i].points[2].y);
break;
}
}
cvReleaseMat( &result );
cvReleaseMat( &curp );
}
}
*/
cvReleaseImage( &clean_nonthresh );
#ifdef OLD_BUSTED
for(int y = 0; y < image1->height; y++) {
for(int x = 0; x < image1->width; x++) {
CvSubdiv2DPointLocation locate_result;
CvSubdiv2DEdge on_edge;
CvSubdiv2DPoint * on_vertex;
CvPoint curpoint = cvPoint( x, y );
locate_result = cvSubdiv2DLocate( delaunay, cvPointTo32f( curpoint ),
&on_edge, &on_vertex );
if( (locate_result != CV_PTLOC_OUTSIDE_RECT) && (locate_result != CV_PTLOC_ERROR) ) {
if( locate_result == CV_PTLOC_VERTEX ) { // this point is on a vertex
for(int i = 0; i < count; i++) {
if(((on_vertex->pt).x == delaunay_points[i].x) && ((on_vertex->pt).y == delaunay_points[i].y)) {
cvSet2D( image1, y, x, cvGet2D( image2, cvPointFrom32f(dest_points[i]).y, cvPointFrom32f(dest_points[i]).x ) );
break;
}
}
}
else if( locate_result == CV_PTLOC_ON_EDGE ) { // this point is on an edge
CvSubdiv2DPoint* org_pt;
CvSubdiv2DPoint* dst_pt;
CvPoint org_pt_warp;
CvPoint dst_pt_warp;
org_pt = cvSubdiv2DEdgeOrg(on_edge);
dst_pt = cvSubdiv2DEdgeDst(on_edge);
for(int i = 0; i < count; i++) {
if(((org_pt->pt).x == delaunay_points[i].x) && ((org_pt->pt).y == delaunay_points[i].y)) {
org_pt_warp = cvPointFrom32f(dest_points[i]);
}
if(((dst_pt->pt).x == delaunay_points[i].x) && ((dst_pt->pt).y == delaunay_points[i].y)) {
dst_pt_warp = cvPointFrom32f(dest_points[i]);
}
}
// compute vector length of original edge and current point
double original_length;
double cur_length;
if( (int)((org_pt->pt).x) == curpoint.x ) { // vertical line
original_length = fabs((org_pt->pt).y - (dst_pt->pt).y);
cur_length = fabs((org_pt->pt).y - curpoint.y);
}
else if( (int)((org_pt->pt).y) == curpoint.y ) { // horizontal line
original_length = fabs((org_pt->pt).x - (dst_pt->pt).x);
cur_length = fabs((org_pt->pt).x - curpoint.x);
}
else { // sloped line
original_length = sqrt(pow((org_pt->pt).x - (dst_pt->pt).x, 2.0) + pow((org_pt->pt).y - (dst_pt->pt).y, 2.0));
cur_length = sqrt(pow((org_pt->pt).x - curpoint.x, 2.0) + pow((org_pt->pt).y - curpoint.y, 2.0));
}
// compute ratio of this point on the edge
double ratio = cur_length / original_length;
// copy this point from the destination edge
CvPoint point_in_original;
int warped_x = (int)(org_pt_warp.x - dst_pt_warp.x);
int warped_y = (int)(org_pt_warp.y - dst_pt_warp.y);
if( org_pt_warp.x == curpoint.x ) { // vertical line
point_in_original.y = (int)(org_pt_warp.y + (ratio * (org_pt_warp.y - dst_pt_warp.y)));
point_in_original.x = org_pt_warp.x;
}
else if(org_pt_warp.y == curpoint.y) { // horizontal line
point_in_original.x = (int)(org_pt_warp.x + (ratio * (org_pt_warp.x - dst_pt_warp.x)));
point_in_original.y = org_pt_warp.y;
}
else { // sloped line
double destination_length = sqrt(pow((org_pt_warp).x - (dst_pt_warp).x, 2.0) + pow((org_pt_warp).y - (dst_pt_warp).y, 2.0));
double scaled_length = ratio * destination_length;
double dest_angle = atan(fabs( (double)warped_y / (double)warped_x ));
double xdist = scaled_length * cos(dest_angle);
double ydist = scaled_length * sin(dest_angle);
xdist = warped_x > 0 ? xdist : xdist * -1;
ydist = warped_y > 0 ? ydist : ydist * -1;
point_in_original.x = (int)( org_pt_warp.x + xdist);
point_in_original.y = (int)( org_pt_warp.y + ydist);
}
if((point_in_original.x >= 0) && (point_in_original.y >= 0) && (point_in_original.x < (image1->width)) && (point_in_original.y < (image1->height))) {
cvSet2D( image1, y, x, cvGet2D( image2, point_in_original.y, point_in_original.x ) );
}
else {
printf("Edge point outside image\n");
}
// cvSet2D( image1, y, x, cvGet2D( image2, (int)(org_pt_warp.x + (ratio * (org_pt_warp.x - dst_pt_warp.x))),
// (int)(org_pt_warp.y + (ratio * (org_pt_warp.y - dst_pt_warp.y))) ) );
}
else if( locate_result == CV_PTLOC_INSIDE ) { // this point is inside a facet (triangle)
/*
printf("Point inside facet: %d, %d\n",curpoint.x,curpoint.y);
int count = 0;
CvPoint * origins = (CvPoint*)malloc(sizeof(CvPoint)*3);
CvSubdiv2DEdge t = on_edge;
// count number of edges
do {
CvSubdiv2DPoint* pt = cvSubdiv2DEdgeOrg( t );
if(count < 3) {
origins[count] = cvPoint( cvRound(pt->pt.x), cvRound(pt->pt.y));
printf("%d,%d\t",origins[count].x,origins[count].y);
}
count++;
t = cvSubdiv2DGetEdge( t, CV_NEXT_AROUND_LEFT );
} while(t != on_edge);
printf("\n");
free(origins);
*/
}
}
}
}
#endif // OLD_BUSTED
printf("done.\n");
cvSaveImage("fullwarp.jpg", image1);
printf("Drawing subdivisions on warped image...");
draw_subdiv( image1, delaunay, NULL, NULL, 0, NULL );
// draw_subdiv( image1, delaunay, delaunay_points, source_points, count, status );
printf("done.\n");
cvSaveImage("edgeswarp.jpg", image1);
cvReleaseImage(&image2);
image2 = cvLoadImage(im2fname, CV_LOAD_IMAGE_COLOR);
// cvCreateImage( cvGetSize(image2), IPL_DEPTH_8U, 3 );
// cvCalcSubdivVoronoi2D( delaunay );
printf("Drawing subdivisions on unwarped image...");
draw_subdiv( image2, delaunay, delaunay_points, dest_points, count, status );
// draw_subdiv( image2, delaunay, NULL, NULL, 0, NULL );
printf("done.\n");
cvSaveImage("edges.jpg",image2);
cvReleaseImage(&image1);
cvFree(&source_points);
cvFree(&dest_points);
cvFree(&status);
cvReleaseMemStorage(&storage);
cvFree(&delaunay_points);
cvReleaseImage(&image2);
return 0;
}
