static int CheckImage(IplImage* image, char* file, char* /*funcname*/)
{
//printf("loading %s\n", file );
IplImage* read = cvLoadImage( file, 1 );
if( !read )
{
trsWrite( ATS_CON | ATS_LST, "can't read image\n" );
return 1;
}
int err = 0;
#if 0
{
IplImage* temp = cvCloneImage( read );
cvAbsDiff( image, read, temp );
cvThreshold( temp, temp, 0, 255, CV_THRESH_BINARY );
cvvNamedWindow( "Original", 0 );
cvvNamedWindow( "Diff", 0 );
cvvShowImage( "Original", read );
cvvShowImage( "Diff", temp );
cvvWaitKey(0);
cvvDestroyWindow( "Original" );
cvvDestroyWindow( "Diff" );
}
#endif
cvAbsDiff( image, read, read );
cvThreshold( read, read, 0, 1, CV_THRESH_BINARY );
err = cvRound( cvNorm( read, 0, CV_L1 ))/3;
cvReleaseImage( &read );
return err;
}
void CHmiDsi::WarpToRightView (float *left0, IplImage *disp0, IplImage *occmask0, float *warped0, IplImage *occmask0_right_view)
{
int scaled_imgW = disp0->width;
int scaled_imgH = disp0->height;
IplImage *disp0_right = cvCloneImage (disp0);
cvZero (disp0_right);
cvZero (occmask0_right_view);
// warp to derive disparity map of second view
for (int y = 0; y < scaled_imgH; y++)
for (int x = 0; x < scaled_imgW; x++)
{
int d = (int) (uchar) disp0->imageData[y * disp0->widthStep + x];
if (x - d < 0 || (occmask0->imageData [y * occmask0->widthStep + x]) == 0) continue;
int curdisp_right = (int) (uchar) disp0_right->imageData[y * disp0_right->widthStep + x - d];
if (curdisp_right < d)
{
disp0_right->imageData[y * disp0_right->widthStep + x - d] = (uchar) d;
occmask0_right_view->imageData[y * occmask0_right_view->widthStep + x - d] = (uchar) 255;
}
}
// use disparity map of second view to generate warped right view
float *dst = warped0;
float *src;
for (int y = 0; y < scaled_imgH; y++)
for (int x = 0; x < scaled_imgW; x++)
{
if (occmask0_right_view->imageData [y * occmask0_right_view->widthStep + x] != 0)
{
int d = (int) (uchar) disp0_right->imageData[y * disp0_right->widthStep + x];
src = left0 + (y * disp0_right->width + x + d) * 3;
*dst = *src; dst++; src++;
*dst = *src; dst++; src++;
*dst = *src; dst++;
}
else
{
*dst = 0.f; dst++;
*dst = 0.f; dst++;
*dst = 0.f; dst++;
}
}
#ifdef SHOW_HMI_STEPS
cvConvertScale(disp0_right, disp0_right, 16);
cvvNamedWindow ("disp_right", CV_WINDOW_AUTOSIZE);
cvMoveWindow ("disp_right", disp0_right->width, 0);
cvShowImage ("disp_right", disp0_right);
cvvNamedWindow ("occmask0_right", CV_WINDOW_AUTOSIZE);
cvMoveWindow ("occmask0_right", 0, 0);
cvShowImage ("occmask0_right", occmask0_right_view);
#endif
cvReleaseImage (&disp0_right);
}
void showfloatarray (float *data, int imgW, int imgH, int numchannels, float scale, CvPoint pos, char *txt, float addvalue = 0.0)
{
IplImage *show;
if (numchannels == 1)
show = cvCreateImage (cvSize(imgW, imgH), IPL_DEPTH_8U, 1);
else if (numchannels == 3)
show = cvCreateImage (cvSize(imgW, imgH), IPL_DEPTH_8U, 3);
for (int y = 0; y < imgH; y++)
for (int x = 0; x < imgW; x++)
if (numchannels == 1)
{
int val = (int) ((data[y * imgW + x] + addvalue) * scale) + 0.5;
if (val < 0) val *= -1;
if (val > 255) val = 255;
show->imageData [y * show->widthStep + x] = val;
}
else if (numchannels == 3)
for (int c = 0; c < 3; c++)
{
int val = (int) ((data[(y * imgW + x) * 3 + 2 - c] + addvalue) * scale) + 0.5;
if (val < 0) val *= -1;
if (val > 255) val = 255;
show->imageData [y * show->widthStep + 3 * x + c] = val;
}
cvvNamedWindow (txt, CV_WINDOW_AUTOSIZE);
cvMoveWindow (txt, pos.x, pos.y);
cvShowImage (txt, show);
cvReleaseImage (&show);
}
void show_points( IplImage* gray, CvPoint2D32f* u, int u_cnt, CvPoint2D32f* v, int v_cnt,
CvSize etalon_size, int was_found )
{
CvSize size;
int i;
cvGetImageRawData( gray, 0, 0, &size );
IplImage* rgb = cvCreateImage( size, 8, 3 );
cvMerge( gray, gray, gray, 0, rgb );
if( v )
{
for( i = 0; i < v_cnt; i++ )
{
cvCircle( rgb, cvPoint(cvRound(v[i].x), cvRound(v[i].y)), 3, CV_RGB(255,0,0), CV_FILLED);
}
}
if( u )
{
for( i = 0; i < u_cnt; i++ )
{
cvCircle( rgb, cvPoint(cvRound(u[i].x), cvRound(u[i].y)), 3, CV_RGB(0,255,0), CV_FILLED);
}
}
cvDrawChessboardCorners( rgb, etalon_size, v, v_cnt, was_found );
cvvNamedWindow( "test", 0 );
cvvShowImage( "test", rgb );
cvvWaitKey(0);
}
// plots disparity map with occlusions in red
void ShowOcclusionGraph (kolmogorov::qpbo::QPBO<REAL> *q, Proposal *proposal1, Proposal *proposal2, float scale, char *fn)
{
int imgW = proposal1->imgW;
int imgH = proposal1->imgH;
IplImage *disp = cvCreateImage(cvSize(imgW, imgH), IPL_DEPTH_8U, 3);
IplImage *occ = cvCreateImage(cvSize(imgW, imgH), IPL_DEPTH_8U, 3);
for (int y = 0; y < imgH; y++)
for (int x = 0; x < imgW; x++)
{
int disp_node = q->GetLabel(y * imgW + x);
Proposal *solution = 0;
if (disp_node == 0)
solution = proposal1;
else if (disp_node == 1)
solution = proposal2;
else
{
disp->imageData[y * disp->widthStep + 3 * x + 0] = 0;
disp->imageData[y * disp->widthStep + 3 * x + 1] = 255;
disp->imageData[y * disp->widthStep + 3 * x + 2] = 0;
occ->imageData[y * disp->widthStep + 3 * x + 0] = 0;
occ->imageData[y * disp->widthStep + 3 * x + 1] = 255;
occ->imageData[y * disp->widthStep + 3 * x + 2] = 0;
continue;
}
float d = solution->surfacemodels[y * imgW + x]->PointDisp(cvPoint(x, y), scale);
d *= scale;
int int_d = min ((int) (d + 0.5f), 255);
for (int c = 0; c < 3; c++)
disp->imageData[y * disp->widthStep + 3 * x + c] = (uchar) int_d;
int occidx;
if (disp_node == 0)
occidx = y * imgW + x + imgW * imgH;
else
occidx = y * imgW + x + 2 * imgW * imgH;
int occluded = q->GetLabel(occidx);
if (hasSwappedMeaning(occidx))
occluded = (!occluded);
if (occluded)
{
occ->imageData[y * disp->widthStep + 3 * x + 0] = 0;
occ->imageData[y * disp->widthStep + 3 * x + 1] = 0;
occ->imageData[y * disp->widthStep + 3 * x + 2] = 255;
}
else
{
for (int c = 0; c < 3; c++)
occ->imageData[y * disp->widthStep + 3 * x + c] =
disp->imageData[y * disp->widthStep + 3 * x + c];
}
}
#ifdef SHOWIMAGES
cvvNamedWindow ("graph occ", CV_WINDOW_AUTOSIZE);
cvShowImage ("graph occ", occ);
#endif
cvSaveImage (fn, occ);
cvReleaseImage (&disp);
cvReleaseImage (&occ);
}
//
// Function: FindCorners
// Purpose : This function finds the corner points in the calibration images
// using opencv functions cvFindChessBoardCornerGuesses and
// cvFindCornerSubPix. After image pixel coordinates are found, their
// respective world coordinates are assigned. (z coordinate is always
// zero). When the function cannot find the specified number of
// coordinates in the image, the image is discarded because of the
// added complexity in assigning their world coordinates.
// Output : uveff : coordinates of the chessborder in image plane.
// XYZeff: coordinates of uveff w.r.t World Coordinate System
// m_effective_image_no : number of images processed.
//----------------------------------------------------------------------------
void camcal::FindCorners()
{
IplImage* img = 0;
IplImage* img0 = 0;
IplImage* img1 = 0;
IplImage* greyimg = 0;
CvFont dfont;
cvInitFont (&dfont, CV_FONT_VECTOR0, 0.3, 0.3, 0.0f, 1);
CvPoint onecorner;
int numcorners = m_corner_no;
CvPoint2D64d* uv = new CvPoint2D64d[m_image_number * m_corner_no];
CvPoint3D64d* XYZ = new CvPoint3D64d[m_image_number * m_corner_no];
CvPoint2D32f* corners = new CvPoint2D32f[m_corner_no];
CvMemStorage* storage = 0;
m_effective_image_no=-1;
for( int imgnum=0; imgnum<m_image_number; imgnum++ )
{
numcorners = m_corner_no;
img = m_input_images[imgnum];
imgsize.width = img->width;
imgsize.height= img->height;
img0 = cvCloneImage(img);
img1 = cvCloneImage(img);
greyimg = cvCreateImageHeader(imgsize,IPL_DEPTH_8U,1);
cvCreateImageData(greyimg);
cvCvtColor(img, greyimg, CV_RGB2GRAY);
img0 = cvCloneImage(greyimg);
cvFindChessBoardCornerGuesses(greyimg,
img0,
storage,
cvSize(m_x_height,m_x_width),
corners,
&numcorners);
if( numcorners != m_corner_no ) {
cvReleaseImage( &img0 );
cvReleaseImage( &img1 );
cvReleaseImage( &greyimg );
continue;
}
else
m_effective_image_no++;
// draw a circle at each corner found
for( int t = 0; t < numcorners; t++ )
{
onecorner.x = (int)corners[t].x;
onecorner.y = (int)corners[t].y;
cvCircle(img1, onecorner, 8, CV_RGB(0,255,0),2);
// image, center, radius, color, thickness
}
// Find sub-corners
cvFindCornerSubPix(greyimg,
corners,
numcorners,
cvSize (m_x_height,m_x_width),
cvSize(-1, -1),
cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 10, 0.1));
// correct order
if( m_apply_ordering ){
CvPoint3D32f init = FindRectangleCorner( corners, numcorners );
if( init.z < m_x_height*m_x_width )
SortPoints(corners, numcorners, &init);
else
{
cout<<"Sort Error";
cvReleaseImage( &img0 );
cvReleaseImage( &img1 );
cvReleaseImage( &greyimg );
m_effective_image_no--;
continue;
}
}
//draw a circle and put the corner number at each subcorner found
for( int t = 0; t < numcorners; t++ )
{
onecorner.x = (int)corners[t].x;
onecorner.y = (int)corners[t].y;
cvCircle(img1, onecorner, 3, CV_RGB(255,0,0),1);
char buf[10];
sprintf( buf, "%d", t );
// cvPutText(img1,numbers[t], cvPoint(onecorner.x, onecorner.y + 20), &dfont, CV_RGB(255,0,0));
cvPutText(img1, buf, cvPoint(onecorner.x, onecorner.y + 20), &dfont, CV_RGB(255,0,0));
}
// CAMERA CALIBRATION PART
for( int currPoint=0; currPoint < numcorners; currPoint++ )
{
uv[ m_effective_image_no*numcorners + currPoint].x = corners[currPoint].x;
uv[ m_effective_image_no*numcorners + currPoint].y = corners[currPoint].y;
}
int index;
for( int i = 0; i < m_x_width; i++ )
{
for( int j = 0; j < m_x_height; j++ )
{
index = m_effective_image_no*numcorners + i*m_x_height+j;
XYZ[ index ].x = m_grid_width *(m_x_width -i);
XYZ[ index ].y = m_grid_height*(m_x_height-j);
XYZ[ index ].z = 0;
}
}
if( m_display_corners )
{
cvvNamedWindow( "image", 1 );
cvvShowImage("image",img1);
cvvWaitKey(0);
cvDestroyWindow( "image" );
}
// cvReleaseImage( &img );
cvReleaseImage( &img0 );
cvReleaseImage( &img1 );
cvReleaseImage( &greyimg );
} //loop to next image
free (corners);
m_effective_image_no++;
delete []uveff ; uveff =NULL;
delete []XYZeff; XYZeff = NULL;
if( m_image_number == m_effective_image_no ){
uveff = uv; uv = NULL;
XYZeff = XYZ; XYZ = NULL;
}
else
{
int size = m_effective_image_no * m_corner_no;
uveff = new CvPoint2D64d[size];
XYZeff = new CvPoint3D64d[size];
for(int ph=0; ph<size; ph++)
{
uveff [ph] = uv [ph];
XYZeff[ph] = XYZ[ph];
}
delete []uv; uv = NULL;
delete []XYZ; XYZ = NULL;
}
}
