diff options
Diffstat (limited to 'src/pix_opencv_lk.cc')
| -rw-r--r-- | src/pix_opencv_lk.cc | 1170 |
1 files changed, 1170 insertions, 0 deletions
diff --git a/src/pix_opencv_lk.cc b/src/pix_opencv_lk.cc new file mode 100644 index 0000000..8a9fe2e --- /dev/null +++ b/src/pix_opencv_lk.cc @@ -0,0 +1,1170 @@ + +// +// GEM - Graphics Environment for Multimedia +// +// zmoelnig@iem.kug.ac.at +// +// Implementation file +// +// Copyright (c) 1997-2000 Mark Danks. +// Copyright (c) Günther Geiger. +// Copyright (c) 2001-2002 IOhannes m zmoelnig. forum::für::umläute. IEM +// Copyright (c) 2002 James Tittle & Chris Clepper +// For information on usage and redistribution, and for a DISCLAIMER OF ALL +// WARRANTIES, see the file, "GEM.LICENSE.TERMS" in this distribution. +// +///////////////////////////////////////////////////////// + +#include "pix_opencv_lk.h" +#include <stdio.h> + +CPPEXTERN_NEW(pix_opencv_lk) + +///////////////////////////////////////////////////////// +// +// pix_opencv_lk +// +///////////////////////////////////////////////////////// +// Constructor +// +///////////////////////////////////////////////////////// + +pix_opencv_lk :: pix_opencv_lk() +{ + int i; + + comp_xsize=320; + comp_ysize=240; + + inlet_new(this->x_obj, &this->x_obj->ob_pd, gensym("float"), gensym("winsize")); + + m_dataout = outlet_new(this->x_obj, &s_anything); + win_size = 10; + + points[0] = 0; + points[1] = 0; + status = 0; + count = 0; + need_to_init = 1; + night_mode = 0; + flags = 0; + add_remove_pt = 0; + quality = 0.1; + min_distance = 10; + maxmove = 20; + markall = 0; + ftolerance = 5; + delaunay = -1; + threshold = -1; + + for ( i=0; i<MAX_MARKERS; i++ ) + { + x_xmark[i] = -1; + x_ymark[i] = -1; + } + + // initialize font + cvInitFont( &font, CV_FONT_HERSHEY_PLAIN, 1.0, 1.0, 0, 1, 8 ); + + rgba = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 4 ); + orgb = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 3 ); + rgb = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 3 ); + gray = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + ogray = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + prev_gray = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + pyramid = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + prev_pyramid = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0])); + points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0])); + status = (char*)cvAlloc(MAX_COUNT); + +} + +///////////////////////////////////////////////////////// +// Destructor +// +///////////////////////////////////////////////////////// +pix_opencv_lk :: ~pix_opencv_lk() +{ + // Destroy cv_images + cvReleaseImage( &rgba ); + cvReleaseImage( &orgb ); + cvReleaseImage( &rgb ); + cvReleaseImage( &gray ); + cvReleaseImage( &ogray ); + cvReleaseImage( &prev_gray ); + cvReleaseImage( &pyramid ); + cvReleaseImage( &prev_pyramid ); +} + +///////////////////////////////////////////////////////// +// processImage +// +///////////////////////////////////////////////////////// +void pix_opencv_lk :: processRGBAImage(imageStruct &image) +{ + int i, k; + int im, oi; + int marked; + float dist, odist; + + if ((this->comp_xsize!=image.xsize)&&(this->comp_ysize!=image.ysize)) + { + + this->comp_xsize=image.xsize; + this->comp_ysize=image.ysize; + + cvReleaseImage( &rgba ); + cvReleaseImage( &orgb ); + cvReleaseImage( &rgb ); + cvReleaseImage( &gray ); + cvReleaseImage( &ogray ); + cvReleaseImage( &prev_gray ); + cvReleaseImage( &pyramid ); + cvReleaseImage( &prev_pyramid ); + + rgba = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 4 ); + orgb = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 3 ); + rgb = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 3 ); + gray = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + ogray = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + prev_gray = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + pyramid = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + prev_pyramid = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0])); + points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0])); + status = (char*)cvAlloc(MAX_COUNT); + + } + + memcpy( rgba->imageData, image.data, image.xsize*image.ysize*4 ); + cvCvtColor(rgba, rgb, CV_BGRA2RGB); + cvCvtColor(rgba, orgb, CV_BGRA2RGB); + cvCvtColor(rgba, gray, CV_BGRA2GRAY); + + if( night_mode ) + cvZero( rgb ); + + for ( im=0; im<MAX_MARKERS; im++ ) + { + x_found[im]--; + } + + if ( delaunay >= 0 ) + { + // init data structures for the delaunay + x_fullrect.x = -comp_xsize/2; + x_fullrect.y = -comp_ysize/2; + x_fullrect.width = 2*comp_xsize; + x_fullrect.height = 2*comp_ysize; + + x_storage = cvCreateMemStorage(0); + x_subdiv = cvCreateSubdiv2D( CV_SEQ_KIND_SUBDIV2D, sizeof(*x_subdiv), + sizeof(CvSubdiv2DPoint), + sizeof(CvQuadEdge2D), + x_storage ); + cvInitSubdivDelaunay2D( x_subdiv, x_fullrect ); + } + + if( need_to_init ) + { + /* automatic initialization */ + IplImage* eig = cvCreateImage( cvSize(gray->width,gray->height), 32, 1 ); + IplImage* temp = cvCreateImage( cvSize(gray->width,gray->height), 32, 1 ); + + count = MAX_COUNT; + cvGoodFeaturesToTrack( gray, eig, temp, points[1], &count, + quality, min_distance, 0, 3, 0, 0.04 ); + // cvFindCornerSubPix( gray, points[1], count, + // cvSize(win_size,win_size), cvSize(-1,-1), + // cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)); + cvReleaseImage( &eig ); + cvReleaseImage( &temp ); + + add_remove_pt = 0; + } + else if( count > 0 ) + { + cvCalcOpticalFlowPyrLK( prev_gray, gray, prev_pyramid, pyramid, + points[0], points[1], count, cvSize(win_size,win_size), 3, status, 0, + cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flags ); + flags |= CV_LKFLOW_PYR_A_READY; + for( i = k = 0; i < count; i++ ) + { + if( add_remove_pt ) + { + double dx = pt.x - points[1][i].x; + double dy = pt.y - points[1][i].y; + + if( dx*dx + dy*dy <= 25 ) + { + add_remove_pt = 0; + continue; + } + } + + if( !status[i] ) + continue; + + points[1][k++] = points[1][i]; + if ( delaunay == 0 ) // add all the points + { + cvSubdivDelaunay2DInsert( x_subdiv, points[1][i] ); + cvCalcSubdivVoronoi2D( x_subdiv ); + } + // only add points included in (color-threshold)<p<(color+treshold) + if ( ( delaunay > 0 ) && ( x_xmark[delaunay-1] != -1 ) ) + { + int px = cvPointFrom32f(points[1][i]).x; + int py = cvPointFrom32f(points[1][i]).y; + int ppx, ppy; + + // eight connected pixels + for ( ppx=px-1; ppx<=px+1; ppx++ ) + { + for ( ppy=py-1; ppy<=py+1; ppy++ ) + { + if ( ( ppx < 0 ) || ( ppx >= comp_xsize ) ) continue; + if ( ( ppy < 0 ) || ( ppy >= comp_ysize ) ) continue; + + uchar red = ((uchar*)(orgb->imageData + orgb->widthStep*ppx))[ppy*3]; + uchar green = ((uchar*)(orgb->imageData + orgb->widthStep*ppx))[ppy*3+1]; + uchar blue = ((uchar*)(orgb->imageData + orgb->widthStep*ppx))[ppy*3+2]; + + uchar pred = ((uchar*)(orgb->imageData + orgb->widthStep*x_xmark[delaunay-1]))[x_ymark[delaunay-1]*3]; + uchar pgreen = ((uchar*)(orgb->imageData + orgb->widthStep*x_xmark[delaunay-1]))[x_ymark[delaunay-1]*3+1]; + uchar pblue = ((uchar*)(orgb->imageData + orgb->widthStep*x_xmark[delaunay-1]))[x_ymark[delaunay-1]*3+2]; + + int diff = abs(red-pred) + abs(green-pgreen) + abs(blue-pblue); + + // post( "pix_opencv_lk : point (%d,%d,%d) : diff : %d threshold : %d", blue, green, red, diff, threshold ); + + if ( diff < threshold ) + { + cvSubdivDelaunay2DInsert( x_subdiv, points[1][i] ); + cvCalcSubdivVoronoi2D( x_subdiv ); + } + } + } + } + + cvCircle( rgb, cvPointFrom32f(points[1][i]), 3, CV_RGB(0,255,0), -1, 8,0); + + marked=0; + oi=-1; + dist=(comp_xsize>comp_ysize)?comp_xsize:comp_ysize; + + for ( im=0; im<MAX_MARKERS; im++ ) + { + + if ( x_xmark[im] == -1 ) continue; // i don't see the point + + odist=sqrt( pow( points[1][i].x - x_xmark[im], 2 ) + pow( points[1][i].y - x_ymark[im], 2 ) ); + + // search for the closest point + if ( odist <= maxmove ) + { + if ( odist < dist ) + { + dist = odist; + marked=1; + oi=im; + } + } + } + + if ( oi != -1 ) + { + char tindex[4]; + sprintf( tindex, "%d", oi ); + cvPutText( rgb, tindex, cvPointFrom32f(points[1][i]), &font, CV_RGB(255,255,255)); + x_xmark[oi]=(int)points[1][i].x; + x_ymark[oi]=(int)points[1][i].y; + x_found[oi]=ftolerance; + SETFLOAT(&x_list[0], oi); + SETFLOAT(&x_list[1], x_xmark[oi]); + SETFLOAT(&x_list[2], x_ymark[oi]); + outlet_list( m_dataout, 0, 3, x_list ); + } + + if ( markall && !marked ) + { + for ( im=0; im<MAX_MARKERS; im++) + { + if ( x_xmark[im] == -1 ) + { + x_xmark[im]=points[1][i].x; + x_ymark[im]=points[1][i].y; + x_found[im]=ftolerance; + break; + } + } + } + } + count = k; + } + + for ( im=0; im<MAX_MARKERS; im++ ) + { + if ( (x_xmark[im] != -1.0 ) && !x_found[im] ) + { + x_xmark[im]=-1.0; + x_ymark[im]=-1.0; + SETFLOAT(&x_list[0], im+1); + SETFLOAT(&x_list[1], x_xmark[im]); + SETFLOAT(&x_list[2], x_ymark[im]); + // send a lost point message to the patch + outlet_list( m_dataout, 0, 3, x_list ); + post( "pix_opencv_lk : lost point %d", im+1 ); + } + } + + if( add_remove_pt && count < MAX_COUNT ) + { + points[1][count++] = cvPointTo32f(pt); + cvFindCornerSubPix( gray, points[1] + count - 1, 1, + cvSize(win_size,win_size), cvSize(-1,-1), + cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)); + add_remove_pt = 0; + } + + // draw the delaunay + if ( delaunay >= 0 ) + { + CvSeqReader reader; + int i, total = x_subdiv->edges->total; + int elem_size = x_subdiv->edges->elem_size; + + cvStartReadSeq( (CvSeq*)(x_subdiv->edges), &reader, 0 ); + + for( i = 0; i < total; i++ ) + { + CvQuadEdge2D* edge = (CvQuadEdge2D*)(reader.ptr); + CvSubdiv2DPoint* org_pt; + CvSubdiv2DPoint* dst_pt; + CvPoint2D32f org; + CvPoint2D32f dst; + CvPoint iorg, idst; + + if( CV_IS_SET_ELEM( edge )) + { + org_pt = cvSubdiv2DEdgeOrg((CvSubdiv2DEdge)edge); + dst_pt = cvSubdiv2DEdgeDst((CvSubdiv2DEdge)edge); + + if( org_pt && dst_pt ) + { + org = org_pt->pt; + dst = dst_pt->pt; + + iorg = cvPoint( cvRound( org.x ), cvRound( org.y )); + idst = cvPoint( cvRound( dst.x ), cvRound( dst.y )); + + if ( ( org.x > 0 ) && ( org.x < comp_xsize ) && + ( dst.x > 0 ) && ( dst.x < comp_xsize ) && + ( org.y > 0 ) && ( org.y < comp_ysize ) && + ( dst.y > 0 ) && ( dst.y < comp_ysize ) ) + cvLine( rgb, iorg, idst, CV_RGB(255,0,0), 1, CV_AA, 0 ); + } + } + + CV_NEXT_SEQ_ELEM( elem_size, reader ); + } + } + + CV_SWAP( prev_gray, gray, swap_temp ); + CV_SWAP( prev_pyramid, pyramid, swap_temp ); + CV_SWAP( points[0], points[1], swap_points ); + need_to_init = 0; + + cvCvtColor(rgb, rgba, CV_BGR2BGRA); + memcpy( image.data, rgba->imageData, image.xsize*image.ysize*4 ); +} + +void pix_opencv_lk :: processRGBImage(imageStruct &image) +{ + int i, k; + int im, oi; + int marked; + float dist, odist; + + if ((this->comp_xsize!=image.xsize)&&(this->comp_ysize!=image.ysize)) + { + + this->comp_xsize=image.xsize; + this->comp_ysize=image.ysize; + + cvReleaseImage( &rgba ); + cvReleaseImage( &orgb ); + cvReleaseImage( &rgb ); + cvReleaseImage( &gray ); + cvReleaseImage( &ogray ); + cvReleaseImage( &prev_gray ); + cvReleaseImage( &pyramid ); + cvReleaseImage( &prev_pyramid ); + + rgba = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 4 ); + orgb = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 3 ); + rgb = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 3 ); + gray = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + ogray = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + prev_gray = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + pyramid = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + prev_pyramid = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0])); + points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0])); + status = (char*)cvAlloc(MAX_COUNT); + + } + + memcpy( rgb->imageData, image.data, image.xsize*image.ysize*3 ); + memcpy( orgb->imageData, image.data, image.xsize*image.ysize*3 ); + cvCvtColor(rgb, gray, CV_BGRA2GRAY); + + if( night_mode ) + cvZero( rgb ); + + for ( im=0; im<MAX_MARKERS; im++ ) + { + x_found[im]--; + } + + if ( delaunay >= 0 ) + { + // init data structures for the delaunay + x_fullrect.x = -comp_xsize/2; + x_fullrect.y = -comp_ysize/2; + x_fullrect.width = 2*comp_xsize; + x_fullrect.height = 2*comp_ysize; + + x_storage = cvCreateMemStorage(0); + x_subdiv = cvCreateSubdiv2D( CV_SEQ_KIND_SUBDIV2D, sizeof(*x_subdiv), + sizeof(CvSubdiv2DPoint), + sizeof(CvQuadEdge2D), + x_storage ); + cvInitSubdivDelaunay2D( x_subdiv, x_fullrect ); + } + + if( need_to_init ) + { + /* automatic initialization */ + IplImage* eig = cvCreateImage( cvSize(gray->width,gray->height), 32, 1 ); + IplImage* temp = cvCreateImage( cvSize(gray->width,gray->height), 32, 1 ); + + count = MAX_COUNT; + cvGoodFeaturesToTrack( gray, eig, temp, points[1], &count, + quality, min_distance, 0, 3, 0, 0.04 ); + // cvFindCornerSubPix( gray, points[1], count, + // cvSize(win_size,win_size), cvSize(-1,-1), + // cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)); + cvReleaseImage( &eig ); + cvReleaseImage( &temp ); + + add_remove_pt = 0; + } + else if( count > 0 ) + { + cvCalcOpticalFlowPyrLK( prev_gray, gray, prev_pyramid, pyramid, + points[0], points[1], count, cvSize(win_size,win_size), 3, status, 0, + cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flags ); + flags |= CV_LKFLOW_PYR_A_READY; + for( i = k = 0; i < count; i++ ) + { + if( add_remove_pt ) + { + double dx = pt.x - points[1][i].x; + double dy = pt.y - points[1][i].y; + + if( dx*dx + dy*dy <= 25 ) + { + add_remove_pt = 0; + continue; + } + } + + if( !status[i] ) + continue; + + points[1][k++] = points[1][i]; + if ( delaunay == 0 ) // add all the points + { + cvSubdivDelaunay2DInsert( x_subdiv, points[1][i] ); + cvCalcSubdivVoronoi2D( x_subdiv ); + } + // only add points included in (color-threshold)<p<(color+treshold) + if ( ( delaunay > 0 ) && ( x_xmark[delaunay-1] != -1 ) ) + { + int px = cvPointFrom32f(points[1][i]).x; + int py = cvPointFrom32f(points[1][i]).y; + int ppx, ppy; + + // eight connected pixels + for ( ppx=px-1; ppx<=px+1; ppx++ ) + { + for ( ppy=py-1; ppy<=py+1; ppy++ ) + { + if ( ( ppx < 0 ) || ( ppx >= comp_xsize ) ) continue; + if ( ( ppy < 0 ) || ( ppy >= comp_ysize ) ) continue; + + uchar red = ((uchar*)(orgb->imageData + orgb->widthStep*ppx))[ppy*3]; + uchar green = ((uchar*)(orgb->imageData + orgb->widthStep*ppx))[ppy*3+1]; + uchar blue = ((uchar*)(orgb->imageData + orgb->widthStep*ppx))[ppy*3+2]; + + uchar pred = ((uchar*)(orgb->imageData + orgb->widthStep*x_xmark[delaunay-1]))[x_ymark[delaunay-1]*3]; + uchar pgreen = ((uchar*)(orgb->imageData + orgb->widthStep*x_xmark[delaunay-1]))[x_ymark[delaunay-1]*3+1]; + uchar pblue = ((uchar*)(orgb->imageData + orgb->widthStep*x_xmark[delaunay-1]))[x_ymark[delaunay-1]*3+2]; + + int diff = abs(red-pred) + abs(green-pgreen) + abs(blue-pblue); + + // post( "pix_opencv_lk : point (%d,%d,%d) : diff : %d", blue, green, red, diff ); + + if ( diff < threshold ) + { + cvSubdivDelaunay2DInsert( x_subdiv, points[1][i] ); + cvCalcSubdivVoronoi2D( x_subdiv ); + } + } + } + } + + cvCircle( rgb, cvPointFrom32f(points[1][i]), 3, CV_RGB(0,255,0), -1, 8,0); + + marked=0; + oi=-1; + dist=(comp_xsize>comp_ysize)?comp_xsize:comp_ysize; + + for ( im=0; im<MAX_MARKERS; im++ ) + { + if ( x_xmark[im] == -1 ) continue; // i don't see the point + + odist=sqrt( pow( points[1][i].x - x_xmark[im], 2 ) + pow( points[1][i].y - x_ymark[im], 2 ) ); + + // search for this point + if ( odist <= maxmove ) + { + if ( odist < dist ) + { + dist = odist; + marked=1; + oi=im; + } + } + } + + if ( oi !=-1 ) + { + char tindex[4]; + sprintf( tindex, "%d", oi ); + cvPutText( rgb, tindex, cvPointFrom32f(points[1][i]), &font, CV_RGB(255,255,255)); + x_xmark[oi]=points[1][i].x; + x_ymark[oi]=points[1][i].y; + x_found[oi]=ftolerance; + SETFLOAT(&x_list[0], oi); + SETFLOAT(&x_list[1], x_xmark[oi]); + SETFLOAT(&x_list[2], x_ymark[oi]); + outlet_list( m_dataout, 0, 3, x_list ); + } + + if ( markall && !marked ) + { + for ( im=0; im<MAX_MARKERS; im++) + { + if ( x_xmark[im] == -1 ) + { + x_xmark[im]=points[1][i].x; + x_ymark[im]=points[1][i].y; + x_found[im]=ftolerance; + break; + } + } + } + } + count = k; + } + + for ( im=0; im<MAX_MARKERS; im++ ) + { + if ( (x_xmark[im] != -1.0 ) && !x_found[im] ) + { + x_xmark[im]=-1.0; + x_ymark[im]=-1.0; + SETFLOAT(&x_list[0], im+1); + SETFLOAT(&x_list[1], x_xmark[im]); + SETFLOAT(&x_list[2], x_ymark[im]); + // send a lost point message to the patch + outlet_list( m_dataout, 0, 3, x_list ); + post( "pix_opencv_lk : lost point %d", im+1 ); + } + } + + if( add_remove_pt && count < MAX_COUNT ) + { + points[1][count++] = cvPointTo32f(pt); + cvFindCornerSubPix( gray, points[1] + count - 1, 1, + cvSize(win_size,win_size), cvSize(-1,-1), + cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)); + add_remove_pt = 0; + } + + // draw the delaunay + if ( delaunay >= 0 ) + { + CvSeqReader reader; + int i, total = x_subdiv->edges->total; + int elem_size = x_subdiv->edges->elem_size; + + cvStartReadSeq( (CvSeq*)(x_subdiv->edges), &reader, 0 ); + + for( i = 0; i < total; i++ ) + { + CvQuadEdge2D* edge = (CvQuadEdge2D*)(reader.ptr); + CvSubdiv2DPoint* org_pt; + CvSubdiv2DPoint* dst_pt; + CvPoint2D32f org; + CvPoint2D32f dst; + CvPoint iorg, idst; + + if( CV_IS_SET_ELEM( edge )) + { + org_pt = cvSubdiv2DEdgeOrg((CvSubdiv2DEdge)edge); + dst_pt = cvSubdiv2DEdgeDst((CvSubdiv2DEdge)edge); + + if( org_pt && dst_pt ) + { + org = org_pt->pt; + dst = dst_pt->pt; + + iorg = cvPoint( cvRound( org.x ), cvRound( org.y )); + idst = cvPoint( cvRound( dst.x ), cvRound( dst.y )); + + if ( ( org.x > 0 ) && ( org.x < comp_xsize ) && + ( dst.x > 0 ) && ( dst.x < comp_xsize ) && + ( org.y > 0 ) && ( org.y < comp_ysize ) && + ( dst.y > 0 ) && ( dst.y < comp_ysize ) ) + cvLine( rgb, iorg, idst, CV_RGB(255,0,0), 1, CV_AA, 0 ); + } + } + + CV_NEXT_SEQ_ELEM( elem_size, reader ); + } + } + + CV_SWAP( prev_gray, gray, swap_temp ); + CV_SWAP( prev_pyramid, pyramid, swap_temp ); + CV_SWAP( points[0], points[1], swap_points ); + need_to_init = 0; + + memcpy( image.data, rgb->imageData, image.xsize*image.ysize*3 ); +} + +void pix_opencv_lk :: processYUVImage(imageStruct &image) +{ + post( "pix_opencv_lk : yuv format not supported" ); +} + +void pix_opencv_lk :: processGrayImage(imageStruct &image) +{ + int i, k; + int im, oi; + int marked; + float dist, odist; + + if ((this->comp_xsize!=image.xsize)&&(this->comp_ysize!=image.ysize)) + { + + this->comp_xsize=image.xsize; + this->comp_ysize=image.ysize; + + cvReleaseImage( &rgba ); + cvReleaseImage( &orgb ); + cvReleaseImage( &rgb ); + cvReleaseImage( &ogray ); + cvReleaseImage( &gray ); + cvReleaseImage( &prev_gray ); + cvReleaseImage( &pyramid ); + cvReleaseImage( &prev_pyramid ); + + rgba = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 4 ); + orgb = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 3 ); + rgb = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 3 ); + gray = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + prev_gray = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + pyramid = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + prev_pyramid = cvCreateImage( cvSize(comp_xsize, comp_ysize), 8, 1 ); + points[0] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0])); + points[1] = (CvPoint2D32f*)cvAlloc(MAX_COUNT*sizeof(points[0][0])); + status = (char*)cvAlloc(MAX_COUNT); + + } + + memcpy( gray->imageData, image.data, image.xsize*image.ysize ); + memcpy( ogray->imageData, image.data, image.xsize*image.ysize ); + + if( night_mode ) + cvZero( gray ); + + for ( im=0; im<MAX_MARKERS; im++ ) + { + x_found[im]--; + } + + if ( delaunay >= 0 ) + { + // init data structures for the delaunay + x_fullrect.x = -comp_xsize/2; + x_fullrect.y = -comp_ysize/2; + x_fullrect.width = 2*comp_xsize; + x_fullrect.height = 2*comp_ysize; + + x_storage = cvCreateMemStorage(0); + x_subdiv = cvCreateSubdiv2D( CV_SEQ_KIND_SUBDIV2D, sizeof(*x_subdiv), + sizeof(CvSubdiv2DPoint), + sizeof(CvQuadEdge2D), + x_storage ); + cvInitSubdivDelaunay2D( x_subdiv, x_fullrect ); + } + + if( need_to_init ) + { + /* automatic initialization */ + IplImage* eig = cvCreateImage( cvSize(gray->width,gray->height), 32, 1 ); + IplImage* temp = cvCreateImage( cvSize(gray->width,gray->height), 32, 1 ); + + count = MAX_COUNT; + cvGoodFeaturesToTrack( gray, eig, temp, points[1], &count, + quality, min_distance, 0, 3, 0, 0.04 ); + // cvFindCornerSubPix( gray, points[1], count, + // cvSize(win_size,win_size), cvSize(-1,-1), + // cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)); + cvReleaseImage( &eig ); + cvReleaseImage( &temp ); + + add_remove_pt = 0; + } + else if( count > 0 ) + { + cvCalcOpticalFlowPyrLK( prev_gray, gray, prev_pyramid, pyramid, + points[0], points[1], count, cvSize(win_size,win_size), 3, status, 0, + cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03), flags ); + flags |= CV_LKFLOW_PYR_A_READY; + for( i = k = 0; i < count; i++ ) + { + if( add_remove_pt ) + { + double dx = pt.x - points[1][i].x; + double dy = pt.y - points[1][i].y; + + if( dx*dx + dy*dy <= 25 ) + { + add_remove_pt = 0; + continue; + } + } + + if( !status[i] ) + continue; + + points[1][k++] = points[1][i]; + if ( delaunay == 0 ) // add all the points + { + cvSubdivDelaunay2DInsert( x_subdiv, points[1][i] ); + cvCalcSubdivVoronoi2D( x_subdiv ); + } + // only add points included in (color-threshold)<p<(color+treshold) + if ( ( delaunay > 0 ) && ( x_xmark[delaunay-1] != -1 ) ) + { + int px = cvPointFrom32f(points[1][i]).x; + int py = cvPointFrom32f(points[1][i]).y; + int ppx, ppy; + + // eight connected pixels + for ( ppx=px-1; ppx<=px+1; ppx++ ) + { + for ( ppy=py-1; ppy<=py+1; ppy++ ) + { + if ( ( ppx < 0 ) || ( ppx >= comp_xsize ) ) continue; + if ( ( ppy < 0 ) || ( ppy >= comp_ysize ) ) continue; + + uchar lum = ((uchar*)(ogray->imageData + ogray->widthStep*ppx))[ppy]; + uchar plum = ((uchar*)(ogray->imageData + ogray->widthStep*x_xmark[delaunay-1]))[x_ymark[delaunay-1]]; + + int diff = abs(plum-lum); + + // post( "pix_opencv_lk : point (%d,%d,%d) : diff : %d", blue, green, red, diff ); + + if ( diff < threshold ) + { + cvSubdivDelaunay2DInsert( x_subdiv, points[1][i] ); + cvCalcSubdivVoronoi2D( x_subdiv ); + } + } + } + } + + cvCircle( gray, cvPointFrom32f(points[1][i]), 3, CV_RGB(0,255,0), -1, 8,0); + + marked=0; + oi=-1; + dist=(comp_xsize>comp_ysize)?comp_xsize:comp_ysize; + + for ( im=0; im<MAX_MARKERS; im++ ) + { + if ( x_xmark[im] == -1 ) continue; // i don't see the point + + odist=sqrt( pow( points[1][i].x - x_xmark[im], 2 ) + pow( points[1][i].y - x_ymark[im], 2 ) ); + + // search for the closest point + if ( odist <= maxmove ) + { + if ( odist < dist ) + { + dist = odist; + marked=1; + oi=im; + } + } + } + + if ( oi !=-1 ) + { + char tindex[4]; + sprintf( tindex, "%d", oi ); + cvPutText( gray, tindex, cvPointFrom32f(points[1][i]), &font, CV_RGB(255,255,255)); + x_xmark[oi]=points[1][i].x; + x_ymark[oi]=points[1][i].y; + x_found[oi]=ftolerance; + SETFLOAT(&x_list[0], oi); + SETFLOAT(&x_list[1], x_xmark[oi]); + SETFLOAT(&x_list[2], x_ymark[oi]); + outlet_list( m_dataout, 0, 3, x_list ); + } + + if ( markall && !marked ) + { + for ( im=0; im<MAX_MARKERS; im++) + { + if ( x_xmark[im] == -1 ) + { + x_xmark[im]=points[1][i].x; + x_ymark[im]=points[1][i].y; + x_found[im]=ftolerance; + break; + } + } + } + } + count = k; + } + + for ( im=0; im<MAX_MARKERS; im++ ) + { + if ( (x_xmark[im] != -1.0 ) && !x_found[im] ) + { + x_xmark[im]=-1.0; + x_ymark[im]=-1.0; + SETFLOAT(&x_list[0], im+1); + SETFLOAT(&x_list[1], x_xmark[im]); + SETFLOAT(&x_list[2], x_ymark[im]); + // send a lost point message to the patch + outlet_list( m_dataout, 0, 3, x_list ); + post( "pix_opencv_lk : lost point %d", im+1 ); + } + } + + if( add_remove_pt && count < MAX_COUNT ) + { + points[1][count++] = cvPointTo32f(pt); + cvFindCornerSubPix( gray, points[1] + count - 1, 1, + cvSize(win_size,win_size), cvSize(-1,-1), + cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03)); + add_remove_pt = 0; + } + + // draw the delaunay + if ( delaunay >= 0 ) + { + CvSeqReader reader; + int i, total = x_subdiv->edges->total; + int elem_size = x_subdiv->edges->elem_size; + + cvStartReadSeq( (CvSeq*)(x_subdiv->edges), &reader, 0 ); + + for( i = 0; i < total; i++ ) + { + CvQuadEdge2D* edge = (CvQuadEdge2D*)(reader.ptr); + CvSubdiv2DPoint* org_pt; + CvSubdiv2DPoint* dst_pt; + CvPoint2D32f org; + CvPoint2D32f dst; + CvPoint iorg, idst; + + if( CV_IS_SET_ELEM( edge )) + { + org_pt = cvSubdiv2DEdgeOrg((CvSubdiv2DEdge)edge); + dst_pt = cvSubdiv2DEdgeDst((CvSubdiv2DEdge)edge); + + if( org_pt && dst_pt ) + { + org = org_pt->pt; + dst = dst_pt->pt; + + iorg = cvPoint( cvRound( org.x ), cvRound( org.y )); + idst = cvPoint( cvRound( dst.x ), cvRound( dst.y )); + + if ( ( org.x > 0 ) && ( org.x < comp_xsize ) && + ( dst.x > 0 ) && ( dst.x < comp_xsize ) && + ( org.y > 0 ) && ( org.y < comp_ysize ) && + ( dst.y > 0 ) && ( dst.y < comp_ysize ) ) + cvLine( gray, iorg, idst, CV_RGB(255,0,0), 1, CV_AA, 0 ); + } + } + + CV_NEXT_SEQ_ELEM( elem_size, reader ); + } + } + + CV_SWAP( prev_gray, gray, swap_temp ); + CV_SWAP( prev_pyramid, pyramid, swap_temp ); + CV_SWAP( points[0], points[1], swap_points ); + need_to_init = 0; + + memcpy( image.data, gray->imageData, image.xsize*image.ysize ); +} + +///////////////////////////////////////////////////////// +// static member function +// +///////////////////////////////////////////////////////// + +void pix_opencv_lk :: obj_setupCallback(t_class *classPtr) +{ + class_addmethod(classPtr, (t_method)&pix_opencv_lk::winSizeMessCallback, + gensym("winsize"), A_FLOAT, A_NULL); + class_addmethod(classPtr, (t_method)&pix_opencv_lk::nightModeMessCallback, + gensym("nightmode"), A_FLOAT, A_NULL); + class_addmethod(classPtr, (t_method)&pix_opencv_lk::qualityMessCallback, + gensym("quality"), A_FLOAT, A_NULL); + class_addmethod(classPtr, (t_method)&pix_opencv_lk::initMessCallback, + gensym("init"), A_NULL); + class_addmethod(classPtr, (t_method)&pix_opencv_lk::markMessCallback, + gensym("mark"), A_GIMME, A_NULL); + class_addmethod(classPtr, (t_method)&pix_opencv_lk::deleteMessCallback, + gensym("delete"), A_FLOAT, A_NULL); + class_addmethod(classPtr, (t_method)&pix_opencv_lk::clearMessCallback, + gensym("clear"), A_NULL); + class_addmethod(classPtr, (t_method)&pix_opencv_lk::minDistanceMessCallback, + gensym("mindistance"), A_FLOAT, A_NULL); + class_addmethod(classPtr, (t_method)&pix_opencv_lk::maxMoveMessCallback, + gensym("maxmove"), A_FLOAT, A_NULL); + class_addmethod(classPtr, (t_method)&pix_opencv_lk::ftoleranceMessCallback, + gensym("ftolerance"), A_FLOAT, A_NULL); + class_addmethod(classPtr, (t_method)&pix_opencv_lk::delaunayMessCallback, + gensym("delaunay"), A_SYMBOL, A_NULL); + class_addmethod(classPtr, (t_method)&pix_opencv_lk::pdelaunayMessCallback, + gensym("pdelaunay"), A_FLOAT, A_FLOAT, A_NULL); +} + +void pix_opencv_lk :: winSizeMessCallback(void *data, t_floatarg winsize) +{ + GetMyClass(data)->winSizeMess((float)winsize); +} + +void pix_opencv_lk :: nightModeMessCallback(void *data, t_floatarg nightmode) +{ + GetMyClass(data)->nightModeMess((float)nightmode); +} + +void pix_opencv_lk :: qualityMessCallback(void *data, t_floatarg quality) +{ + GetMyClass(data)->qualityMess((float)quality); +} + +void pix_opencv_lk :: initMessCallback(void *data) +{ + GetMyClass(data)->initMess(); +} + +void pix_opencv_lk :: markMessCallback(void *data, t_symbol *s, int argc, t_atom *argv) +{ + GetMyClass(data)->markMess(argc, argv); +} + +void pix_opencv_lk :: deleteMessCallback(void *data, t_floatarg index) +{ + GetMyClass(data)->deleteMess((float)index); +} + +void pix_opencv_lk :: clearMessCallback(void *data) +{ + GetMyClass(data)->clearMess(); +} + +void pix_opencv_lk :: minDistanceMessCallback(void *data, t_floatarg mindistance) +{ + GetMyClass(data)->minDistanceMess((float)mindistance); +} + +void pix_opencv_lk :: maxMoveMessCallback(void *data, t_floatarg maxmove) +{ + GetMyClass(data)->maxMoveMess((float)maxmove); +} + +void pix_opencv_lk :: ftoleranceMessCallback(void *data, t_floatarg ftolerance) +{ + GetMyClass(data)->ftoleranceMess((float)ftolerance); +} + +void pix_opencv_lk :: delaunayMessCallback(void *data, t_symbol *s) +{ + GetMyClass(data)->delaunayMess(s); +} + +void pix_opencv_lk :: pdelaunayMessCallback(void *data, t_floatarg fpoint, t_floatarg fthreshold) +{ + GetMyClass(data)->pdelaunayMess((float)fpoint, (float)fthreshold); +} + +void pix_opencv_lk :: winSizeMess(float fwinsize) +{ + if (fwinsize>1.0) win_size = (int)fwinsize; +} + +void pix_opencv_lk :: nightModeMess(float fnightmode) +{ + if ((fnightmode==0.0)||(fnightmode==1.0)) night_mode = (int)fnightmode; +} + +void pix_opencv_lk :: qualityMess(float fquality) +{ + if (fquality>0.0) quality = fquality; +} + +void pix_opencv_lk :: initMess(void) +{ + need_to_init = 1; +} + +void pix_opencv_lk :: markMess(int argc, t_atom *argv) +{ + int i; + int inserted; + + if ( argc == 1 ) // mark all or none + { + if ( argv[0].a_type != A_SYMBOL ) + { + error( "pix_opencv_lk : wrong argument (should be 'all')" ); + return; + } + if ( !strcmp( argv[0].a_w.w_symbol->s_name, "all" ) ) + { + markall = 1; + return; + } + if ( !strcmp( argv[0].a_w.w_symbol->s_name, "none" ) ) + { + markall = 0; + clearMess(); + return; + } + } + else + { + if ( ( argv[0].a_type != A_FLOAT ) || ( argv[1].a_type != A_FLOAT ) ) + { + error( "pix_opencv_lk : wrong argument (should be mark px py)" ); + return; + } + else + { + float fpx = argv[0].a_w.w_float; + float fpy = argv[1].a_w.w_float; + int px, py; + + if ( ( fpx < 0.0 ) || ( fpx > comp_xsize ) || ( fpy < 0.0 ) || ( fpy > comp_ysize ) ) + { + return; + } + + px = (int)fpx; + py = (int)fpy; + inserted = 0; + for ( i=0; i<MAX_MARKERS; i++) + { + if ( x_xmark[i] == -1 ) + { + x_xmark[i] = px; + x_ymark[i] = py; + x_found[i] = ftolerance; + inserted = 1; + // post( "pix_opencv_lk : inserted point (%d,%d)", px, py ); + break; + } + } + if ( !inserted ) + { + post( "pix_opencv_lk : max markers reached" ); + } + } + } +} + +void pix_opencv_lk :: deleteMess(float index) +{ + int i; + + if ( ( index < 0.0 ) || ( index >= MAX_MARKERS ) ) + { + return; + } + + x_xmark[(int)index] = -1; + x_ymark[(int)index] = -1; + +} + +void pix_opencv_lk :: clearMess(void) +{ + int i; + + for ( i=0; i<MAX_MARKERS; i++) + { + x_xmark[i] = -1; + x_ymark[i] = -1; + } + +} + +void pix_opencv_lk :: minDistanceMess(float fmindistance) +{ + if (fmindistance>1.0) min_distance = (int)fmindistance; +} + +void pix_opencv_lk :: maxMoveMess(float fmaxmove) +{ + // has to be more than the size of a point + if (fmaxmove>=3.0) maxmove = (int)fmaxmove; +} + +void pix_opencv_lk :: ftoleranceMess(float ftolerance) +{ + if (ftolerance>=0.0) ftolerance = (int)ftolerance; +} + +void pix_opencv_lk :: delaunayMess(t_symbol *s) +{ + if (s == gensym("on")) + delaunay = 0; + if (s == gensym("off")) + delaunay = -1; +} + +void pix_opencv_lk :: pdelaunayMess(float fpoint, float fthreshold) +{ + if (((int)fpoint>0) && ((int)fpoint<MAX_MARKERS)) + { + delaunay = (int)fpoint; + threshold = (int)fthreshold; + // post( "pix_opencv_lk : setting threshold to : %d", threshold ); + } +} + |