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);
}
TDV_NAMESPACE_BEGIN
StereoCorrespondenceCV::StereoCorrespondenceCV(
MatchingMode mode, int maxDisparity, int maxIterations)
: m_limg8U(CV_8U), m_rimg8U(CV_8U)
{
if ( mode == LocalMatching )
{
m_bmState = cvCreateStereoBMState(CV_STEREO_BM_BASIC, maxDisparity);
workName("OpenCV_BlockMatcher");
}
else
{
m_gcState = cvCreateStereoGCState(maxDisparity, maxIterations);
workName("OpenCV_GraphCut");
}
m_mode = mode;
}
void AdjustBMParam::tryParam(){
int preFilterSz = this->ui->preFilterSz->value();
int preFilterCap = this->ui->preFilterCap->value();
int sadWndSz = this->ui->SADWndSz->value();
int minDisp = this->ui->minDisp->value();
int numDisp = 16*(this->ui->numDisp->value());
int texture = this->ui->textureth->value();
int uniquness = this->ui->uniq->value();
int spkWndSz = this->ui->speckleWndSz->value();
int spkRange = this->ui->specklerange->value();
if(sadWndSz%2==0)
sadWndSz++;
if(sadWndSz<5)
sadWndSz = 5;
if(preFilterSz%2==0)
preFilterSz++;
if(preFilterSz<5)
preFilterSz = 5;
CvStereoBMState *BMState = cvCreateStereoBMState();
assert(BMState != 0);
BMState->preFilterSize=preFilterSz;
BMState->preFilterCap=preFilterCap;
BMState->SADWindowSize=sadWndSz;
BMState->minDisparity= minDisp;
BMState->numberOfDisparities=numDisp;
BMState->textureThreshold=texture; //reduce noise
BMState->uniquenessRatio=uniquness;
BMState->speckleWindowSize = spkWndSz;
BMState->speckleRange = spkRange;
stereoCalibration->setBMState(BMState);
stereoCalibration->computeDisparity(leftImage, rightImage);
CvMat* pair = cvCreateMat(leftImage->height, leftImage->width*2, CV_8UC3 );
CvMat part;
cvGetCols( pair, &part, 0, leftImage->width );
cvCvtColor( stereoCalibration->leftImageRectified, &part, CV_GRAY2BGR );
cvGetCols( pair, &part, leftImage->width,leftImage->width*2 );
cvCvtColor( stereoCalibration->rightImageRectified, &part, CV_GRAY2BGR);
for(int j = 0; j < leftImage->height; j += 25 )
cvLine( pair, cvPoint(0,j),cvPoint(leftImage->width*2,j),CV_RGB(255,0,0));
cvReleaseImage(&dispartiyImageColor);
dispartiyImageColor = cvCreateImage (cvGetSize(leftImage),IPL_DEPTH_8U, 3);
if (dispartiyImageColor == NULL)
{
fprintf(stderr,"ERROR: Could not create pseudocolor image.\n");
exit(2);
}
IplImage *img, img_header;
img = cvGetImage(stereoCalibration->imageDisparityNormalized, &img_header);
disparityMapColor->g2cd_gray_to_chromadepth(img, this->dispartiyImageColor, this->g2cd_data);
cvShowImage("disparity coloured", dispartiyImageColor);
cvNamedWindow( "rectified", 1 );
cvShowImage( "rectified", pair );
// cvReleaseStereoBMState(&BMState);
//cvReleaseImage(&img);
}
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;}
}
