int doStereo( TriclopsContext const & triclops, TriclopsInput const & stereoData, TriclopsImage16 & depthImage ) { TriclopsError te; // Set subpixel interpolation on to use // TriclopsImage16 structures when we access and save the disparity image te = triclopsSetSubpixelInterpolation( triclops, 1 ); _HANDLE_TRICLOPS_ERROR( "triclopsSetSubpixelInterpolation()", te ); te = triclopsSetDisparity( triclops, 0, 70 ); _HANDLE_TRICLOPS_ERROR( "triclopsSetSubpixelInterpolation()", te ); //ROS_INFO("stereoData x,y: %d,%d",stereoData.nrows, stereoData.ncols); te = triclopsSetResolution( triclops, stereoData.nrows, stereoData.ncols ); _HANDLE_TRICLOPS_ERROR( "triclopsSetResolution()", te ); // Rectify the images te = triclopsRectify( triclops, const_cast<TriclopsInput *>( &stereoData ) ); _HANDLE_TRICLOPS_ERROR( "triclopsRectify()", te ); // Do stereo processing te = triclopsStereo( triclops ); _HANDLE_TRICLOPS_ERROR( "triclopsStereo()", te ); // Retrieve the interpolated depth image from the context te = triclopsGetImage16( triclops, TriImg16_DISPARITY, TriCam_REFERENCE, &depthImage ); _HANDLE_TRICLOPS_ERROR( "triclopsGetImage()", te ); return 0; }
void doRectifyStereoSave( TriclopsContext triclops, TriclopsInput* pTriclopsInput, const char* szDisparityBase, const char* szNameModifier ) { TriclopsError error; // Rectify the images error = triclopsRectify( triclops, pTriclopsInput ); _HANDLE_TRICLOPS_ERROR( "triclopsRectify()", error ); // Do stereo processing error = triclopsStereo( triclops ); _HANDLE_TRICLOPS_ERROR( "triclopsStereo()", error ); // test whether we are doing subpixel stereo TriclopsBool bSubpixel; error = triclopsGetSubpixelInterpolation( triclops, &bSubpixel ); _HANDLE_TRICLOPS_ERROR( "triclopsGetSubpixelInterpolation()", error ); char szDisparityFile[1024]; if ( bSubpixel ) { // Retrieve the disparity image from the context TriclopsImage16 triclopsImage16; error = triclopsGetImage16( triclops, TriImg16_DISPARITY, TriCam_REFERENCE, &triclopsImage16 ); _HANDLE_TRICLOPS_ERROR( "triclopsGetImage16(): Failed to get disparity image", error ); // create the disparity name sprintf( szDisparityFile, "%s-%s.pgm", szDisparityBase, szNameModifier ); // Save the disparity 16-bit image error = triclopsSaveImage16( &triclopsImage16, szDisparityFile ); _HANDLE_TRICLOPS_ERROR( "triclopsSaveImage16(): Failed to save disparity image", error ); printf( "Wrote 16-bit disparity image to '%s'\n", szDisparityFile ); // scale it into an 8 bit image and save that as well TriclopsImage triclopsImage; // need to allocate the 8-bit TriclopsImage to scale this one into triclopsImage.nrows = triclopsImage16.nrows; triclopsImage.ncols = triclopsImage16.ncols; triclopsImage.rowinc = triclopsImage16.ncols; triclopsImage.data = (unsigned char*) malloc( triclopsImage.nrows*triclopsImage.ncols ); scaleImage16ToImage8( &triclopsImage16, &triclopsImage, 90, 255 ); // create the disparity name sprintf( szDisparityFile, "%s-%s-scaled.pgm", szDisparityBase, szNameModifier ); triclopsSaveImage( &triclopsImage, szDisparityFile ); printf( "Wrote disparity image to '%s'\n", szDisparityFile ); free( triclopsImage.data ); } else { // Retrieve the disparity image from the context TriclopsImage triclopsImage; error = triclopsGetImage( triclops, TriImg_DISPARITY, TriCam_REFERENCE, &triclopsImage ); _HANDLE_TRICLOPS_ERROR( "triclopsGetImage(): Failed to get disparity image", error ); // scale the image so it is easier to see scaleImage( &triclopsImage, 90, 255 ); // create the disparity name sprintf( szDisparityFile, "%s-%s.pgm", szDisparityBase, szNameModifier ); // Save the disparity image error = triclopsSaveImage( &triclopsImage, szDisparityFile ); _HANDLE_TRICLOPS_ERROR( "triclopsSaveImage(): Failed to save disparity image", error ); printf( "Wrote disparity image to '%s'\n", szDisparityFile ); } return; }
/* version that takes the DPPU pan and tilt, and the robot pose */ void stereoCam::doStereoFrame( #ifdef OPENCV IplImage *frame, IplImage *disparity, IplImage *depth, FILE *out, double pan, double tilt, double rx, double ry, double rth #else FILE *out #endif ) { if ( inRoutine() ) return; // protected routine for (int i=0; i<10; i++) { pucRightRGB = NULL; pucLeftRGB = NULL; pucCenterRGB = NULL; extractImagesColor( &stereoCamera, DC1394_BAYER_METHOD_NEAREST, pucDeInterlacedBuffer, pucRGBBuffer, pucGreenBuffer, &pucRightRGB, &pucLeftRGB, &pucCenterRGB, &input ); } #if 0 // write out the raw images from the camera if ( !writePpm( "yright.ppm", pucRightRGB, stereoCamera.nCols, stereoCamera.nRows ) ) printf( "wrote right.ppm\n" ); if ( !writePpm( "yleft.ppm", pucLeftRGB, stereoCamera.nCols, stereoCamera.nRows ) ) printf( "wrote left.ppm\n" ); #endif e = triclopsRectify( triclops, &input ); TRIERR(e); // e = triclopsPreprocess(triclops, &input); // TRIERR(e); /************************************/ e = triclopsStereo( triclops ); TRIERR(e) /************************************/ #if 0 // get and save the rectified and disparity images e=triclopsGetImage( triclops, TriImg_RECTIFIED, TriCam_REFERENCE, &image ); TRIERR(e); e=triclopsSaveImage( &image, "yrectified.pgm" ); TRIERR(e); printf( "wrote 'rectified.pgm'\n" ); #endif e=triclopsGetImage16( triclops, TriImg16_DISPARITY, TriCam_REFERENCE, &image16 ); TRIERR(e); #if 0 // save disparity image -- warning its 16bit not 8bit! if (gWriteImages) { e=triclopsSaveImage16( &image16, "ydisparity.pgm" ); TRIERR(e); printf( "wrote 'disparity.pgm'\n" ); } #endif outRoutine(); // allow other access to camera now int nPoints=0; int iPixelInc = image16.rowinc/2; int rPixelInc = 3*stereoCamera.nCols; for ( int iRow = 0; iRow < image16.nrows; iRow++ ) { unsigned short* pusRow = image16.data + iRow * iPixelInc; unsigned char * rgbRow = pucRightRGB + 3 * iRow * rPixelInc; for ( int iCol= 0; iCol < image16.ncols; iCol++ ) { unsigned short usDisp = pusRow[ iCol ]; float x,y,z; double posBB[3], posW[3]; // x,y,z in bumblebee and in world frames #ifdef OPENCV if (frame!=0) cvSet2D(frame,iRow,iCol, // populate RGB image cvScalar(rgbRow[ 3*iCol*3+2], rgbRow[ 3*iCol*3+1], rgbRow[ 3*iCol*3 ],0)); #endif if ( usDisp < 0xFF00 ) { // valid disparity only // convert the 16 bit disparity value to floating point x,y,z triclopsRCD16ToXYZ( triclops, iRow, iCol, usDisp, &x, &y, &z // x,y,z, in bumblebee frarame ); nPoints++; posBB[0]=x; posBB[1]=y; posBB[2]=z; if (z>0.1 && z< 5.0) {// near and far distance (meters) filter transformBumbleBee2World(posBB, posW, pan, tilt, rx, ry, rth); if (out!=0) fprintf(out, "%f, %f, %f, %d, %d, %d, %d, %d, %d\n", posW[0], posW[1], posW[2], // write out world frame locations iRow, iCol, usDisp, rgbRow[ 3*iCol*3], rgbRow[ 3*iCol*3+1], rgbRow[ 3*iCol*3+2]); #ifdef OPENCV if (depth!=0) cvSet2D(depth,iRow,iCol, // populate depth image with local frame info //cvScalar(double(x), double(y), double(z), 0.0)); cvScalar(posW[0], posW[1], posW[2], 0)); #endif }// write out result #ifdef OPENCV if (disparity!=0) cvSet2D(disparity,iRow,iCol, // populate disparity image cvScalar(usDisp/255,0,0,0)); #endif }// if valid disparity }// for iCol }// for iRow return; }