void ImgProducer::calcFLOWMASK()
{
/* Calcul du flot */
cvCalcOpticalFlowHS(imgFGRAY(),imgFGRAYPrec(),0,img[idGIMGX],img[idGIMGY],lambda,criteria);
/* Calcul du masque */
int i,j;
unsigned char *mask;
float * fx, * fy;
fx=(float *)img[idGIMGX]->imageData;
fy=(float *)img[idGIMGY]->imageData;
mask=(unsigned char *)img[idFLOWMASK]->imageData;
for(j=0;j<height;j++)
{
for(i=0;i<width;i++)
{
if((fabs(*fx)>0.5)||(fabs(*fy)>0.5)) *mask=255;
else *mask=0;
fx++;
fy++;
mask++;
}
fx+=xremainstep;
fy+=yremainstep;
mask+=mremainstep;
}
imgOK[idGIMGX] = 1;
imgOK[idGIMGY] = 1;
imgOK[idFLOWMASK] = 1;
}
int main(int argc, char** argv)
{
// Initialize, load two images from the file system, and
// allocate the images and other structures we will need for
// results.
// exit if no input images
if (argc < 3)
return -1;
IplImage* imgA = cvLoadImage(argv[1],0);
IplImage* imgB = cvLoadImage(argv[2],0);
IplImage* velx = cvCreateImage(cvGetSize(imgA),IPL_DEPTH_32F,1);
IplImage* vely = cvCreateImage(cvGetSize(imgA),IPL_DEPTH_32F,1);
IplImage* imgC = cvCreateImage(cvGetSize(imgA),IPL_DEPTH_8U,1);
cvNamedWindow( "A" );
cvNamedWindow( "B" );
cvNamedWindow( "C" );
cvShowImage( "A",imgA );
cvShowImage( "B",imgB );
// Call the actual Horn and Schunck algorithm
//
cvCalcOpticalFlowHS(
imgA,
imgB,
0,
velx,
vely,
.10,
cvTermCriteria(
CV_TERMCRIT_ITER | CV_TERMCRIT_EPS,
imgA->width,
1e-6
)
);
// Now make some image of what we are looking at:
//
cvZero( imgC );
int step = 4;
for( int y=0; y<imgC->height; y += step ) {
float* px = (float*) ( velx->imageData + y * velx->widthStep );
float* py = (float*) ( vely->imageData + y * vely->widthStep );
for( int x=0; x<imgC->width; x += step ) {
if( px[x]>1 && py[x]>1 ) {
cvCircle(
imgC,
cvPoint( x, y ),
2,
CVX_GRAY50,
-1
);
cvLine(
imgC,
cvPoint( x, y ),
cvPoint( x+px[x]/2, y+py[x]/2 ),
CVX_WHITE,
1,
0
);
}
}
}
// show tracking
cvShowImage( "C",imgC );
cvWaitKey(0);
// destroy windows
cvDestroyWindow( "A" );
cvDestroyWindow( "B" );
cvDestroyWindow( "C" );
// release memory
cvReleaseImage( &imgA );
cvReleaseImage( &imgB );
cvReleaseImage( &imgC );
return 0;
}
void Flow::calculate_flow_hs() {
cvCalcOpticalFlowHS(ampl_img_2, ampl_img_1, 1, velx, vely, 1, cvTermCriteria(1, 10, 0.5));
}
void CVisionPipeline::TrackMotion (CIplImage &image, float &xVel, float &yVel)
{
CvTermCriteria term;
CvRect box;
static TAnalisysMatrix velXMatrix, velYMatrix, velModulusMatrix;
crvColorToGray (image.ptr(), m_imgCurr.ptr());
// Prepare ROI's
m_imgPrev.PushROI ();
m_imgCurr.PushROI ();
m_imgVelX.PushROI ();
m_imgVelY.PushROI ();
m_imgCurrProc.PushROI ();
m_trackArea.GetBoxImg (&image, box);
m_imgPrev.SetROI (box);
m_imgCurr.SetROI (box);
m_imgPrevProc.SetROI (box);
// Mutex is not needed
m_imgCurrProc.SetROI (box);
m_imgVelX.SetROI (box);
m_imgVelY.SetROI (box);
// Pre-processing
PreprocessImage ();
// Compute optical flow
term.type= CV_TERMCRIT_ITER;
term.max_iter= 6;
cvCalcOpticalFlowHS (m_imgPrevProc.ptr(), m_imgCurrProc.ptr(), 0,
m_imgVelX.ptr(), m_imgVelY.ptr(), 0.001, term);
MatrixMeanImageCells (&m_imgVelX, velXMatrix);
MatrixMeanImageCells (&m_imgVelY, velYMatrix);
int x, y;
float velModulusMax= 0;
// Compute modulus for every motion cell
for (x= 0; x< COMP_MATRIX_WIDTH; ++x) {
for (y= 0; y< COMP_MATRIX_HEIGHT; ++y) {
velModulusMatrix[x][y]=
(velXMatrix[x][y] * velXMatrix[x][y] + velYMatrix[x][y] * velYMatrix[x][y]);
if (velModulusMax< velModulusMatrix[x][y]) velModulusMax= velModulusMatrix[x][y];
}
}
// Select valid cells (i.e. those with enough motion)
int validCells= 0;
xVel= yVel= 0;
for (x= 0; x< COMP_MATRIX_WIDTH; ++x) {
for (y= 0; y< COMP_MATRIX_HEIGHT; ++y) {
if (velModulusMatrix[x][y]> (0.05 * velModulusMax) ) {
++validCells;
xVel+= velXMatrix[x][y];
yVel+= velYMatrix[x][y];
}
}
}
// Ensure minimal area to avoid extremely high values
int cellArea= (box.width * box.height) / (COMP_MATRIX_WIDTH * COMP_MATRIX_HEIGHT);
if (cellArea== 0) cellArea= 1;
int minValidCells= (3000 / cellArea);
if (validCells< minValidCells) validCells= minValidCells;
// Compute speed
xVel= - (xVel / (float) validCells) * 40;
yVel= (yVel / (float) validCells) * 80;
// Restore ROI's
m_imgCurrProc.PopROI ();
m_imgPrev.PopROI ();
m_imgCurr.PopROI ();
m_imgVelX.PopROI ();
m_imgVelY.PopROI ();
}
