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; }