int StereoVision::stereoProcess(CvArr* imageSrcLeft,CvArr* imageSrcRight) {
if(!calibrationDone) return RESULT_FAIL;
if(!imagesRectified[0]) imagesRectified[0] = cvCreateMat( imageSize.height,imageSize.width, CV_8U );
if(!imagesRectified[1]) imagesRectified[1] = cvCreateMat( imageSize.height,imageSize.width, CV_8U );
if(!imageDepth) imageDepth = cvCreateMat( imageSize.height,imageSize.width, CV_16S );
if(!imageDepthNormalized) imageDepthNormalized = cvCreateMat( imageSize.height,imageSize.width, CV_8U );
// 2013.4.29
// CZT
//
if(!imageDepthNormalized_c1) imageDepthNormalized_c1 = cvCreateMat( 512, 512, CV_8U );
/*if(!imagesRectified[0]) imagesRectified[0] = cvCreateMat( 512, 512, CV_8U );
if(!imagesRectified[1]) imagesRectified[1] = cvCreateMat( 512, 512, CV_8U );
if(!imageDepth) imageDepth = cvCreateMat( 512, 512, CV_16S );
if(!imageDepthNormalized) imageDepthNormalized = cvCreateMat( 512, 512, CV_8U );*/
//rectify images
cvRemap( imageSrcLeft, imagesRectified[0] , mx1, my1 );
cvRemap( imageSrcRight, imagesRectified[1] , mx2, my2 );
CvStereoBMState *BMState = cvCreateStereoBMState();
BMState->preFilterSize=41;
BMState->preFilterCap=31;
BMState->SADWindowSize=41;
BMState->minDisparity=-64;
BMState->numberOfDisparities=128;
BMState->textureThreshold=10;
BMState->uniquenessRatio=15;
cvFindStereoCorrespondenceBM( imagesRectified[0], imagesRectified[1], imageDepth, BMState);
cvNormalize( imageDepth, imageDepthNormalized, 0, 255, CV_MINMAX );
cvReleaseStereoBMState(&BMState);
// 2013.4.29
// CZT
//
cvResize(imageDepthNormalized, imageDepthNormalized_c1);
cvFlip(imageDepthNormalized_c1, imageDepthNormalized_c1, 1); // Flip, y-axis
return RESULT_OK;
}
void remapAndDisparity(CvMat* l_remap0, CvMat* l_remap1, CvMat* r_remap0, CvMat* r_remap1)
{
/* Variable initialization */
CvArr *l_frame_remap = NULL;
CvArr *r_frame_remap = NULL;
CvArr *disparity_map = NULL;
IplImage *l_frame = NULL;
IplImage *r_frame = NULL;
/*
* Open the goddamned webcams using dumbass C-API.
* This is cv2.VideoCapture(id) in python
*/
CvCapture *l_cap = cvCaptureFromCAM(1);
CvCapture *r_cap = cvCaptureFromCAM(2);
/* Time to remap the frames!!!! */
while (1) {
/* This is cap.read() in python */
l_frame = cvQueryFrame(l_cap);
r_frame = cvQueryFrame(r_cap);
/* This is cv2.remap in python */
cvRemap(l_frame, l_frame_remap, l_remap0, l_remap1, CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
cvRemap(r_frame, r_frame_remap, r_remap0, r_remap1, CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
/* This is cv2.stereoBM in python */
CvStereoBMState *SBMS = cvCreateStereoBMState(CV_STEREO_BM_BASIC, 16);
/* This is cv2.stereoBM.compute in python */
cvFindStereoCorrespondenceBM(l_frame_remap, r_frame_remap, disparity_map, SBMS);
/*
* Show the disparity map and waitkey.
*/
cvShowImage("Image", disparity_map);
key = cvWaitKey(10)&0xFF;
if (key == 27 || key == 113 /* 113 is key 'q' */) {
printf("Bye.\n");
break;
}
}
cvReleaseImage(&l_frame);
cvReleaseImage(&r_frame);
cvReleaseImage(&disparity_map);
}
bool StereoCorrespondenceCV::update()
{
WriteGuard<ReadWritePipe<FloatImage> > wguard(m_wpipe);
FloatImage limg, rimg;
if ( m_lrpipe->read(&limg) && m_rrpipe->read(&rimg) )
{
CvMat *limg_c = limg.cpuMem();
CvMat *rimg_c = rimg.cpuMem();
CvMat *limg8u_c = m_limg8U.getImage(cvGetSize(limg_c));
CvMat *rimg8u_c = m_rimg8U.getImage(cvGetSize(rimg_c));
cvConvertScale(limg_c, limg8u_c, 255.0);
cvConvertScale(rimg_c, rimg8u_c, 255.0);
FloatImage output = FloatImage::CreateCPU(
Dim::minDim(limg.dim(), rimg.dim()));
CvMat *out_c = output.cpuMem();
CudaBenchmarker bMarker;
bMarker.begin();
if ( m_mode == LocalMatching )
cvFindStereoCorrespondenceBM(limg8u_c, rimg8u_c, out_c, m_bmState);
else
cvFindStereoCorrespondenceGC(limg8u_c, rimg8u_c, out_c, NULL,
m_gcState);
bMarker.end();
m_mark.addProbe(bMarker.elapsedTime());
wguard.write(output);
FloatImageSinkPol::sink(limg);
FloatImageSinkPol::sink(rimg);
}
return wguard.wasWrite();
}
CvMat * createDisparityMapBM(CvArr * img1, CvArr * img2,
int numberOfDisparities, int maxIters)
{
CvSize imageSize = cvGetSize(img1);
CvMat* disp = cvCreateMat( imageSize.height,
imageSize.width, CV_16S );
//Setup for finding stereo corrrespondences
CvStereoBMState *BMState = cvCreateStereoBMState();
assert(BMState != 0);
BMState->preFilterSize=41;
BMState->preFilterCap=31;
// window size
BMState->SADWindowSize=41;
// depth range
// BMState->minDisparity=-64;
// BMState->numberOfDisparities=128;
BMState->minDisparity=-16;
BMState->numberOfDisparities=32;
BMState->textureThreshold=10;
BMState->uniquenessRatio=15;
if( img1 && img2 )
{
// CvMat part;
// void cvRemap( const CvArr* src, CvArr* dst,
// const CvArr* mapx, const CvArr* mapy)
// FUNCTION: dst(x,y)<-src(mapx(x,y),mapy(x,y))
// cvRemap( img1, img1r, mx1, my1 );
// cvRemap( img2, img2r, mx2, my2 );
// if( !isVerticalStereo || useUncalibrated != 0 )
{
// When the stereo camera is oriented vertically,
// useUncalibrated==0 does not transpose the
// image, so the epipolar lines in the rectified
// images are vertical. Stereo correspondence
// function does not support such a case.
// cvFindStereoCorrespondenceBM( img1r, img2r, disp,
// BMState);
cvFindStereoCorrespondenceBM( img1, img2, disp,
BMState);
// cvNormalize( disp, vdisp, 0, 256, CV_MINMAX );
}
cvReleaseStereoBMState(&BMState);
CvMat * disparity_left_visual =
cvCreateMat( imageSize.height, imageSize.width, CV_8U );
// because the values in the left disparity images are
// usually negative
cvConvertScale( disp, disparity_left_visual, 1 );
// cvSave( "disparity.pgm", disparity_left_visual );
return disparity_left_visual;
//cvSaveImage( ".out/disparity.pgm", disparity_left_visual );
}else {return 0;}
}