bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef RTABMAP_FLYCAPTURE2
if(camera_)
{
// Close the camera
camera_->StopCapture();
camera_->Disconnect();
}
if(triclopsCtx_)
{
triclopsDestroyContext(triclopsCtx_);
triclopsCtx_ = 0;
}
// connect camera
FlyCapture2::Error fc2Error = camera_->Connect();
if(fc2Error != FlyCapture2::PGRERROR_OK)
{
UERROR("Failed to connect the camera.");
return false;
}
// configure camera
Fc2Triclops::StereoCameraMode mode = Fc2Triclops::TWO_CAMERA_NARROW;
if(Fc2Triclops::setStereoMode(*camera_, mode ))
{
UERROR("Failed to set stereo mode.");
return false;
}
// generate the Triclops context
FlyCapture2::CameraInfo camInfo;
if(camera_->GetCameraInfo(&camInfo) != FlyCapture2::PGRERROR_OK)
{
UERROR("Failed to get camera info.");
return false;
}
float dummy;
unsigned packetSz;
FlyCapture2::Format7ImageSettings imageSettings;
int maxWidth = 640;
int maxHeight = 480;
if(camera_->GetFormat7Configuration(&imageSettings, &packetSz, &dummy) == FlyCapture2::PGRERROR_OK)
{
maxHeight = imageSettings.height;
maxWidth = imageSettings.width;
}
// Get calibration from th camera
if(Fc2Triclops::getContextFromCamera(camInfo.serialNumber, &triclopsCtx_))
{
UERROR("Failed to get calibration from the camera.");
return false;
}
triclopsSetResolution(triclopsCtx_, maxHeight, maxWidth);
if (triclopsPrepareRectificationData(triclopsCtx_,
maxHeight,
maxWidth,
maxHeight,
maxWidth))
{
UERROR("Failed to prepare rectification matrices.");
return false;
}
triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW);
float fx, cx, cy, baseline;
triclopsGetFocalLength(triclopsCtx_, &fx);
triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
cx *= maxWidth;
cy *= maxHeight;
triclopsGetBaseline(triclopsCtx_, &baseline);
UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f %dx%d", fx, cx, cy, baseline, maxWidth, maxHeight);
if(camera_->StartCapture() != FlyCapture2::PGRERROR_OK)
{
UERROR("Failed to start capture.");
return false;
}
return true;
#else
UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
#endif
return false;
}
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;
}