Exemplo n.º 1
0
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;
}
Exemplo n.º 2
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;
}