Example #1
0
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;
}
Example #2
0
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();
}
Example #4
0
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;}
}