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;
}
int
main( int /* argc */, char** /* argv */ )
{
TriclopsContext triclops;
TriclopsImage disparityImage;
TriclopsImage refImage;
TriclopsInput triclopsInput;
TriclopsROI* pRois;
int nMaxRois;
TriclopsError te;
FlyCaptureContext flycapture;
FlyCaptureImage flycaptureImage;
FlyCaptureInfoEx pInfo;
FlyCapturePixelFormat pixelFormat;
FlyCaptureError fe;
int iMaxCols = 0;
int iMaxRows = 0;
// Create the camera context
fe = flycaptureCreateContext( &flycapture );
_HANDLE_FLYCAPTURE_ERROR( "flycaptureCreateContext()", fe );
// Initialize the camera
fe = flycaptureInitialize( flycapture, 0 );
_HANDLE_FLYCAPTURE_ERROR( "flycaptureInitialize()", fe );
// Get the camera configuration
char* szCalFile;
fe = flycaptureGetCalibrationFileFromCamera( flycapture, &szCalFile );
_HANDLE_FLYCAPTURE_ERROR( "flycaptureGetCalibrationFileFromCamera()", fe );
// Create a Triclops context from the cameras calibration file
te = triclopsGetDefaultContextFromFile( &triclops, szCalFile );
_HANDLE_TRICLOPS_ERROR( "triclopsGetDefaultContextFromFile()", te );
// Get camera information
fe = flycaptureGetCameraInfo( flycapture, &pInfo );
_HANDLE_FLYCAPTURE_ERROR( "flycatpureGetCameraInfo()", fe );
if (pInfo.CameraType == FLYCAPTURE_COLOR)
{
pixelFormat = FLYCAPTURE_RAW16;
}
else
{
pixelFormat = FLYCAPTURE_MONO16;
}
switch (pInfo.CameraModel)
{
case FLYCAPTURE_BUMBLEBEE2:
{
unsigned long ulValue;
flycaptureGetCameraRegister( flycapture, 0x1F28, &ulValue );
if ( ( ulValue & 0x2 ) == 0 )
{
// Hi-res BB2
iMaxCols = 1024;
iMaxRows = 768;
}
else
{
// Low-res BB2
iMaxCols = 640;
iMaxRows = 480;
}
}
break;
case FLYCAPTURE_BUMBLEBEEXB3:
iMaxCols = 1280;
iMaxRows = 960;
break;
default:
te = TriclopsErrorInvalidCamera;
_HANDLE_TRICLOPS_ERROR( "triclopsCheckCameraModel()", te );
break;
}
// Start grabbing
fe = flycaptureStartCustomImage(
flycapture, 3, 0, 0, iMaxCols, iMaxRows, 100, pixelFormat);
_HANDLE_FLYCAPTURE_ERROR( "flycaptureStart()", fe );
// Grab an image from the camera
fe = flycaptureGrabImage2( flycapture, &flycaptureImage );
_HANDLE_FLYCAPTURE_ERROR( "flycaptureGrabImage()", fe );
// Extract information from the FlycaptureImage
int imageCols = flycaptureImage.iCols;
int imageRows = flycaptureImage.iRows;
int imageRowInc = flycaptureImage.iRowInc;
int iSideBySideImages = flycaptureImage.iNumImages;
unsigned long timeStampSeconds = flycaptureImage.timeStamp.ulSeconds;
unsigned long timeStampMicroSeconds = flycaptureImage.timeStamp.ulMicroSeconds;
// Create buffers for holding the mono images
unsigned char* rowIntColor =
new unsigned char[ imageCols * imageRows * iSideBySideImages * 4];
unsigned char* rowIntMono =
new unsigned char[ imageCols * imageRows * iSideBySideImages ];
// Create a temporary FlyCaptureImage for preparing the stereo image
FlyCaptureImage tempColorImage;
FlyCaptureImage tempMonoImage;
tempColorImage.pData = rowIntColor;
tempMonoImage.pData = rowIntMono;
// Convert the pixel interleaved raw data to row interleaved format
fe = flycapturePrepareStereoImage(
flycapture, flycaptureImage, &tempMonoImage, &tempColorImage );
_HANDLE_FLYCAPTURE_ERROR( "flycapturePrepareStereoImage()", fe );
// Pointers to positions in the mono buffer that correspond to the beginning
// of the red, green and blue sections
unsigned char* redMono = NULL;
unsigned char* greenMono = NULL;
unsigned char* blueMono = NULL;
redMono = rowIntMono;
if (flycaptureImage.iNumImages == 2)
{
greenMono = redMono + imageCols;
blueMono = redMono + imageCols;
}
if (flycaptureImage.iNumImages == 3)
{
greenMono = redMono + imageCols;
blueMono = redMono + ( 2 * imageCols );
}
// Use the row interleaved images to build up an RGB TriclopsInput.
// An RGB triclops input will contain the 3 raw images (1 from each camera).
te = triclopsBuildRGBTriclopsInput(
imageCols,
imageRows,
imageRowInc,
timeStampSeconds,
timeStampMicroSeconds,
redMono,
greenMono,
blueMono,
&triclopsInput);
_HANDLE_TRICLOPS_ERROR( "triclopsBuildRGBTriclopsInput()", te );
// Set up some stereo parameters:
// Set to 640x480 output images
te = triclopsSetResolution( triclops, 480, 640 );
_HANDLE_TRICLOPS_ERROR( "triclopsSetResolution()", te );
// Set disparity range to be quite wide
te = triclopsSetDisparity( triclops, 0, 200 );
_HANDLE_TRICLOPS_ERROR( "triclopsSetDisparity()", te );
// Set subpixel interpolation off - so we know we don't need to use
// TriclopsImage16 structures when we access and save the disparity image
te = triclopsSetSubpixelInterpolation( triclops, 0 );
_HANDLE_TRICLOPS_ERROR( "triclopsSetSubpixelInterpolation()", te );
// Get the pointer to the regions of interest array
te = triclopsGetROIs( triclops, &pRois, &nMaxRois );
_HANDLE_TRICLOPS_ERROR( "triclopsGetROIs()", te );
if( nMaxRois >= 4 )
{
// Set up four regions of interest:
// Entire upper left quadrant of image
pRois[0].row = 0;
pRois[0].col = 0;
pRois[0].nrows = 240;
pRois[0].ncols = 320;
// Part of the lower right
pRois[1].row = 240;
pRois[1].col = 320;
pRois[1].nrows = 180;
pRois[1].ncols = 240;
// Centered in upper right quadrant
pRois[2].row = 60;
pRois[2].col = 400;
pRois[2].nrows = 120;
pRois[2].ncols = 160;
// Small section of lower left
pRois[3].row = 300;
pRois[3].col = 30;
pRois[3].nrows = 80;
pRois[3].ncols = 80;
// Tell the TriclopsContext how many ROIs we want to process
te = triclopsSetNumberOfROIs( triclops, 4 );
_HANDLE_TRICLOPS_ERROR( "triclopsSetNumberOfROIs()", te );
}
else
{
printf( "Only %d ROIs available in the TriclopsContext "
"- this should never happen!\n"
"Aborting!\n",
nMaxRois );
// Destroy the Triclops context
triclopsDestroyContext( triclops ) ;
// Close the camera and destroy the context
flycaptureStop( flycapture );
flycaptureDestroyContext( flycapture );
return 1;
}
// Rectify the images
te = triclopsRectify( triclops, &triclopsInput );
_HANDLE_TRICLOPS_ERROR( "triclopsRectify()", te );
// Do stereo processing
te = triclopsStereo( triclops );
_HANDLE_TRICLOPS_ERROR( "triclopsStereo()", te );
// Retrieve the disparity image from the Triclops context
te = triclopsGetImage( triclops,
TriImg_DISPARITY,
TriCam_REFERENCE,
&disparityImage );
_HANDLE_TRICLOPS_ERROR( "triclopsGetImage()", te );
// Retrieve the rectified image from the Triclops context
te = triclopsGetImage( triclops,
TriImg_RECTIFIED,
TriCam_REFERENCE,
&refImage );
_HANDLE_TRICLOPS_ERROR( "triclopsGetImage()", te );
// Save the disparity image
te = triclopsSaveImage( &disparityImage, "disparity.pgm" );
_HANDLE_TRICLOPS_ERROR( "triclopsSaveImage()", te );
// Save the rectified image
te = triclopsSaveImage( &refImage, "rectified.pgm" );
_HANDLE_TRICLOPS_ERROR( "triclopsSaveImage()", te );
// Delete the image buffer, it is not needed once the TriclopsInput
// has been built
delete [] rowIntMono;
redMono = NULL;
greenMono = NULL;
blueMono = NULL;
// Destroy the Triclops context
triclopsDestroyContext( triclops ) ;
// Close the camera and destroy the Flycapture context
flycaptureStop( flycapture );
flycaptureDestroyContext( flycapture );
return 0;
}
int
main( int /* argc */, char* /* * argv */ )
{
TriclopsContext triclops;
TriclopsError error;
TriclopsInput triclopsInput;
char* szInputFile = "input.ppm";
char* szCalFile = "input.cal";
char* szDisparityBase = "disparity";
// Get the camera calibration data
error = triclopsGetDefaultContextFromFile( &triclops, szCalFile );
_HANDLE_TRICLOPS_ERROR( "triclopsGetDefaultContextFromFile(): "
"Can't open calibration file",
error );
// Load images from file
if ( !ppmReadToTriclopsInput( szInputFile, &triclopsInput ) )
{
printf( "ppmReadToTriclopsInput() failed. Can't read '%s'\n", szInputFile );
return 1;
}
doRectifyStereoSave( triclops, &triclopsInput, szDisparityBase, "default" );
// try doing stereo with a big stereo mask
triclopsSetStereoMask( triclops, 21 );
doRectifyStereoSave( triclops, &triclopsInput, szDisparityBase, "bigmask" );
// try doing stereo with a small stereo mask
triclopsSetStereoMask( triclops, 5 );
doRectifyStereoSave( triclops, &triclopsInput, szDisparityBase, "smallmask" );
// set mask to a neutral value
triclopsSetStereoMask( triclops, 11 );
// try doing stereo without any validation
triclopsSetSurfaceValidation( triclops, 0 );
triclopsSetUniquenessValidation( triclops, 0 );
triclopsSetTextureValidation( triclops, 0 );
doRectifyStereoSave( triclops, &triclopsInput, szDisparityBase, "novalidation" );
// try doing stereo with only texture and surface
triclopsSetSurfaceValidation( triclops, 1 );
triclopsSetTextureValidation( triclops, 1 );
doRectifyStereoSave( triclops, &triclopsInput, szDisparityBase, "surf-tex-val" );
// try doing stereo in 2 camera mode with back and forth validation
triclopsSetCameraConfiguration( triclops, TriCfg_2CAM_HORIZONTAL );
triclopsSetBackForthValidation( triclops, 1 );
doRectifyStereoSave( triclops, &triclopsInput, szDisparityBase, "backforth" );
// try doing stereo in subpixel mode to compare the results of subpixel with
// whole pixel - try running "convertimage16" example to convert this image to
// one that is more easily viewed
triclopsSetSubpixelInterpolation( triclops, 1 );
doRectifyStereoSave( triclops, &triclopsInput, szDisparityBase, "subpixel" );
// clean up memory allocated in context
freeInput( &triclopsInput );
error = triclopsDestroyContext( triclops );
_HANDLE_TRICLOPS_ERROR( "triclopsDestroyContext()", error );
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;
}
int
main( int /* argc */, char** /* argv */ )
{
TriclopsError te;
FlyCaptureError fe;
TriclopsContext triclops;
FlyCaptureContext flycapture;
float baseline;
int nrows;
int ncols;
float focalLength;
TriclopsCameraConfiguration triclopsConfig;
// Create the Flycapture context
fe = flycaptureCreateContext( &flycapture );
_HANDLE_FLYCAPTURE_ERROR( "flycaptureCreateContext()", fe );
// Initialize the camera
fe = flycaptureInitialize( flycapture, 0 );
_HANDLE_FLYCAPTURE_ERROR( "flycaptureInitialize()", fe );
// Get the camera configuration
char* szCalFile;
fe = flycaptureGetCalibrationFileFromCamera( flycapture, &szCalFile );
_HANDLE_FLYCAPTURE_ERROR( "flycaptureGetCalibrationFileFromCamera()", fe );
// Create a Triclops context from the cameras calibration file
te = triclopsGetDefaultContextFromFile( &triclops, szCalFile );
_HANDLE_TRICLOPS_ERROR( "triclopsGetDefaultContextFromFile()", te );
// Destroy the Flycapture context - this program just displays information contained
// in the TriclopsContext
flycaptureDestroyContext( flycapture );
printf( "Triclops Version : %s\n", triclopsVersion() );
// get the camera configuration
te = triclopsGetCameraConfiguration( triclops, &triclopsConfig );
_HANDLE_TRICLOPS_ERROR( "triclopsGetCameraConfiguration()", te );
switch( triclopsConfig )
{
case TriCfg_L:
printf( "Configuration : 3 Camera\n" );
break;
case TriCfg_2CAM_HORIZONTAL:
printf( "Configuration : 2 Camera horizontal\n" );
break;
case TriCfg_2CAM_VERTICAL:
printf( "Configuration : 2 Camera vertical\n" );
break;
default:
printf( "Unrecognized configuration: %d\n", triclopsConfig );
}
// Get the baseline
te = triclopsGetBaseline( triclops, &baseline );
_HANDLE_TRICLOPS_ERROR( "triclopsGetBaseline()", te );
printf( "Baseline : %f cm\n", baseline*100.0 );
// Get the focal length
te = triclopsGetFocalLength( triclops, &focalLength );
_HANDLE_TRICLOPS_ERROR( "triclopsGetFocalLength()", te );
te = triclopsGetResolution( triclops, &nrows, &ncols );
_HANDLE_TRICLOPS_ERROR( "triclopsGetResolution()", te );
printf( "Focal Length : %f pixels for a %d x %d image\n",
focalLength,
ncols,
nrows ) ;
int nRows, nCols;
te = triclopsGetResolution( triclops, &nRows, &nCols );
_HANDLE_TRICLOPS_ERROR( "triclopsGetResolution()", te );
float fCenterRow, fCenterCol;
te = triclopsGetImageCenter( triclops, &fCenterRow, &fCenterCol );
_HANDLE_TRICLOPS_ERROR( "triclopsGetImageCenter()", te );
printf( "The default image resolution for stereo processing is %d x %d\n",
nCols, nRows );
printf( "For this resolution, the 'image center' or 'principal point' "
"is:\n" );
printf( "Center Row = %f\n", fCenterRow );
printf( "Center Col = %f\n", fCenterCol );
// Destroy the Triclops context
triclopsDestroyContext( triclops ) ;
return 0;
}
int
main( int /* argc */, char** /* argv */ )
{
TriclopsContext context;
TriclopsImage depthImage;
TriclopsInput inputData;
TriclopsError error;
// get the camera module configuration
error = triclopsGetDefaultContextFromFile( &context, "config" );
_HANDLE_TRICLOPS_ERROR( "triclopsGetDefaultContextFromFile()", error );
if ( error != TriclopsErrorOk )
{
printf( "Can't open calibration file 'config'\n" );
exit( 1 );
}
// Load images from file
TriclopsBool bErr = ppmReadToTriclopsInput( "input.ppm", &inputData );
if( !bErr )
{
printf( "ppmReadToTriclopsInput() failed. Can't find input.ppm?\n" );
exit( 1 );
}
// set up some stereo parameters:
// set to 320x240 output images
error = triclopsSetResolution( context, 240, 320 );
_HANDLE_TRICLOPS_ERROR( "triclopsSetResolution()", error );
// set disparity range
error = triclopsSetDisparity( context, 5, 60 );
_HANDLE_TRICLOPS_ERROR( "triclopsSetDisparity()", error );
// set the display mapping
// note: disparity mapping corrupts the disparity values so that making
// distance measurements is more difficult and less accurate.
// Do not use it when you intend to actually use disparity values for
// purposes other than display
error = triclopsSetDisparityMapping( context, 128, 255 );
_HANDLE_TRICLOPS_ERROR( "triclopsSetDisparityMapping()", error );
error = triclopsSetDisparityMappingOn( context, 1 );
_HANDLE_TRICLOPS_ERROR( "triclopsSetDisparityMappingOn()", error );
// set the validation mappings to 0 (black)
error = triclopsSetUniquenessValidationMapping( context, 0 );
_HANDLE_TRICLOPS_ERROR( "triclopsSetUniquenessValidationMapping()", error );
error = triclopsSetTextureValidationMapping( context, 0 );
_HANDLE_TRICLOPS_ERROR( "triclopsSetTextureValidationMapping()", error );
// Preprocessing the images
error = triclopsPreprocess( context, &inputData );
_HANDLE_TRICLOPS_ERROR( "triclopsPreprocess()", error );
// stereo processing
error = triclopsStereo( context );
_HANDLE_TRICLOPS_ERROR( "triclopsStereo()", error );
// retrieve the depth image from the context
error = triclopsGetImage( context, TriImg_DISPARITY, TriCam_REFERENCE, &depthImage );
_HANDLE_TRICLOPS_ERROR( "triclopsGetImage()", error );
// save the depth image
error = triclopsSaveImage( &depthImage, "depth.pgm" );
_HANDLE_TRICLOPS_ERROR( "triclopsSaveImage()", error );
// clean up memory allocated in context
freeInput( &inputData );
error = triclopsDestroyContext( context );
return 0;
}
int do3dPoints( FC2::Image const & grabbedImage,
TriclopsContext const & triclops,
TriclopsImage16 const & disparityImage16,
TriclopsInput const & colorData,
PointCloud & returnedPoints )
{
TriclopsImage monoImage = {0};
TriclopsColorImage colorImage = {0};
TriclopsError te;
float x, y, z;
int nPoints = 0;
int pixelinc ;
int i, j, k;
unsigned short * row;
unsigned short disparity;
PointT point3d;
// Rectify the color image if applicable
bool isColor = false;
if ( grabbedImage.GetPixelFormat() == FC2::PIXEL_FORMAT_RAW16 )
{
isColor = true;
te = triclopsRectifyColorImage( triclops,
TriCam_REFERENCE,
const_cast<TriclopsInput *>( &colorData ),
&colorImage );
_HANDLE_TRICLOPS_ERROR( "triclopsRectifyColorImage()", te );
}
else
{
te = triclopsGetImage( triclops,
TriImg_RECTIFIED,
TriCam_REFERENCE,
&monoImage );
_HANDLE_TRICLOPS_ERROR( "triclopsGetImage()", te );
}
// The format for the output file is:
// <x> <y> <z> <red> <grn> <blu> <row> <col>
// <x> <y> <z> <red> <grn> <blu> <row> <col>
// ...
// Determine the number of pixels spacing per row
pixelinc = disparityImage16.rowinc / 2;
//ROS_INFO("DisparityData x,y: %d,%d",disparityImage16.nrows, disparityImage16.ncols);
for ( i = 0, k = 0; i < disparityImage16.nrows; i++ )
{
row = disparityImage16.data + i * pixelinc;
for ( j = 0; j < disparityImage16.ncols; j++, k++ )
{
disparity = row[j];
// do not save invalid points
if ( disparity < 0xFF00 )
{
// convert the 16 bit disparity value to floating point x,y,z
triclopsRCD16ToXYZ( triclops, i, j, disparity, &x, &y, &z );
// look at points within a range
if ( z < 5.0 )
{
point3d.x = z;
point3d.y = -x;
point3d.z = -y;
if ( isColor )
{
point3d.r = ( int )colorImage.red[k];
point3d.g = ( int )colorImage.green[k];
point3d.b = ( int )colorImage.blue[k];
}
else
{
// For mono cameras, we just assign the same value to RGB
point3d.r = ( int )monoImage.data[k];
point3d.g = ( int )monoImage.data[k];
point3d.b = ( int )monoImage.data[k];
}
returnedPoints.push_back( point3d );
// fprintf( pPointFile, "%f %f %f %d %d %d %d %d\n", x, y, z, r, g, b, i, j );
nPoints++;
}
}
}
}
//ROS_INFO( "Points in file: %d\n", nPoints );
return 0;
}
int generateTriclopsInput( FC2::Image const & grabbedImage,
ImageContainer & imageContainer,
TriclopsInput & triclopsColorInput,
TriclopsInput & triclopsMonoInput )
{
FC2::Error fc2Error;
FC2T::ErrorType fc2TriclopsError;
TriclopsError te;
FC2::Image * unprocessedImage = imageContainer.unprocessed;
fc2TriclopsError = FC2T::unpackUnprocessedRawOrMono16Image(
grabbedImage,
true /*assume little endian*/,
unprocessedImage[RIGHT],
unprocessedImage[LEFT] );
if ( fc2TriclopsError != FC2T::ERRORTYPE_OK )
{
return FC2T::handleFc2TriclopsError( fc2TriclopsError,
"unpackUnprocessedRawOrMono16Image" );
}
FC2::Image * monoImage = imageContainer.mono;
//ROS_INFO( "UnrpocessedImage cols,rows %d,%d", unprocessedImage[RIGHT].GetCols(), unprocessedImage[RIGHT].GetRows() );
// check if the unprocessed image is color
if ( unprocessedImage[RIGHT].GetBayerTileFormat() != FC2::NONE )
{
FC2::Image * bgruImage = imageContainer.bgru;
for ( int i = 0; i < 2; ++i )
{
if ( convertToBGRU( unprocessedImage[i], bgruImage[i] ) )
{
return 1;
}
}
FC2::Image & packedColorImage = imageContainer.packed;
// pack BGRU right and left image into an image
fc2TriclopsError = FC2T::packTwoSideBySideRgbImage( bgruImage[RIGHT],
bgruImage[LEFT],
packedColorImage );
if ( fc2TriclopsError != FC2T::ERRORTYPE_OK )
{
return handleFc2TriclopsError( fc2TriclopsError,
"packTwoSideBySideRgbImage" );
}
// Use the row interleaved images to build up a packed TriclopsInput.
// A packed triclops input will contain a single image with 32 bpp.
te = triclopsBuildPackedTriclopsInput( grabbedImage.GetCols(),
grabbedImage.GetRows(),
packedColorImage.GetStride(),
( unsigned long )grabbedImage.GetTimeStamp().seconds,
( unsigned long )grabbedImage.GetTimeStamp().microSeconds,
packedColorImage.GetData(),
&triclopsColorInput );
_HANDLE_TRICLOPS_ERROR( "triclopsBuildPackedTriclopsInput()", te );
// the following does not change the size of the image
// and therefore it PRESERVES the internal buffer!
packedColorImage.SetDimensions( packedColorImage.GetRows(),
packedColorImage.GetCols(),
packedColorImage.GetStride(),
packedColorImage.GetPixelFormat(),
FC2::NONE );
for ( int i = 0; i < 2; ++i )
{
fc2Error = bgruImage[i].Convert( FlyCapture2::PIXEL_FORMAT_MONO8, &monoImage[i] );
if ( fc2Error != FlyCapture2::PGRERROR_OK )
{
return Fc2Triclops::handleFc2Error( fc2Error );
}
}
}
else
{
monoImage[RIGHT] = unprocessedImage[RIGHT];
monoImage[LEFT] = unprocessedImage[LEFT];
}
// Use the row interleaved images to build up an RGB TriclopsInput.
// An RGB triclops input will contain the 3 raw images (1 from each camera).
te = triclopsBuildRGBTriclopsInput( grabbedImage.GetCols(),
grabbedImage.GetRows(),
grabbedImage.GetCols(),
( unsigned long )grabbedImage.GetTimeStamp().seconds,
( unsigned long )grabbedImage.GetTimeStamp().microSeconds,
monoImage[RIGHT].GetData(),
monoImage[LEFT].GetData(),
monoImage[LEFT].GetData(),
&triclopsMonoInput );
_HANDLE_TRICLOPS_ERROR( "triclopsBuildRGBTriclopsInput()", te );
return 0;
}
