Version 0.8.9: added FlyCapture2 driver for bumblebee2 (tested only on Windows)

corelib/include/rtabmap/core/CameraModel.h

@@ -30,9 +30,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/opencv.hpp>
 
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
 namespace rtabmap {
 
-class CameraModel
+class RTABMAP_EXP CameraModel
 {
 public:
 	CameraModel();

corelib/include/rtabmap/core/CameraRGBD.h

@@ -65,6 +65,11 @@ class Freenect2Device;
 class SyncMultiFrameListener;
 }
 
+namespace FlyCapture2
+{
+class Camera;
+}
+
 typedef struct _freenect_context freenect_context;
 typedef struct _freenect_device freenect_device;
 
@@ -80,6 +85,7 @@ class RTABMAP_EXP CameraRGBD
 public:
 	virtual ~CameraRGBD();
 	void takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy);
+
 	virtual bool init(const std::string & calibrationFolder = ".") = 0;
 	virtual bool isCalibrated() const = 0;
 	virtual std::string getSerial() const = 0;
@@ -241,7 +247,7 @@ public:
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraFreenect();
 
-	bool init(const std::string & calibrationFolder = ".");
+	virtual bool init(const std::string & calibrationFolder = ".");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
@@ -271,7 +277,7 @@ public:
 					const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraFreenect2();
 
-	bool init(const std::string & calibrationFolder = ".");
+	virtual bool init(const std::string & calibrationFolder = ".");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
@@ -286,7 +292,7 @@ private:
 };
 
 /////////////////////////
-// CameraDC1394
+// CameraStereoDC1394
 /////////////////////////
 class DC1394Device;
 
@@ -300,7 +306,7 @@ public:
 	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoDC1394();
 
-	bool init(const std::string & calibrationFolder = ".");
+	virtual bool init(const std::string & calibrationFolder = ".");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 
@@ -312,4 +318,29 @@ private:
 	StereoCameraModel stereoModel_;
 };
 
+/////////////////////////
+// CameraStereoFlyCapture2
+/////////////////////////
+class RTABMAP_EXP CameraStereoFlyCapture2 :
+	public CameraRGBD
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoFlyCapture2();
+
+	virtual bool init(const std::string & calibrationFolder = ".");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual void captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy);
+
+private:
+	FlyCapture2::Camera * camera_;
+	void * triclopsCtx_; // TriclopsContext
+};
+
 } // namespace rtabmap

corelib/src/CMakeLists.txt

@@ -108,6 +108,18 @@ IF(DC1394_FOUND)
 	)
 ENDIF(DC1394_FOUND)
 
+IF(FlyCapture2_FOUND)
+	ADD_DEFINITIONS("-DWITH_FLYCAPTURE2")
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${FlyCapture2_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${FlyCapture2_LIBRARIES}
+	)
+ENDIF(FlyCapture2_FOUND)
+
 IF(G2O_FOUND)
 	ADD_DEFINITIONS("-DWITH_G2O")
 	SET(INCLUDE_DIRS 

corelib/src/CameraRGBD.cpp

@@ -63,6 +63,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <OpenNI.h>
 #endif
 
+#ifdef WITH_FLYCAPTURE2
+#include <triclops.h>
+#include <fc2triclops.h>
+#endif
+
 namespace rtabmap
 {
 
@@ -639,10 +644,12 @@ bool CameraOpenNI2::isCalibrated() const
 
 std::string CameraOpenNI2::getSerial() const
 {
+#ifdef WITH_OPENNI2
 	if(_device)
 	{
 		return _device->getDeviceInfo().getName();
 	}
+#endif
 	return "";
 }
 
@@ -962,7 +969,10 @@ bool CameraFreenect::init(const std::string & calibrationFolder)
 
 bool CameraFreenect::isCalibrated() const
 {
+#ifdef WITH_FREENECT
 	return freenectDevice_ != 0 && freenectDevice_->getDepthFocal() > 0.0f;
+#endif
+	return false;
 }
 
 std::string CameraFreenect::getSerial() const
@@ -1162,7 +1172,7 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
 }
 
 //
-// CameraDC1394
+// CameraStereoDC1394
 // Inspired from ROS camera1394stereo package
 //
 
@@ -1487,6 +1497,14 @@ bool CameraStereoDC1394::init(const std::string & calibrationFolder)
 					UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
 							device_->guid().c_str(), calibrationFolder.c_str());
 				}
+				else
+				{
+					UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
+							stereoModel_.left().fx(),
+							stereoModel_.left().cx(),
+							stereoModel_.left().cy(),
+							stereoModel_.baseline());
+				}
 			}
 		}
 		return ok;
@@ -1539,4 +1557,276 @@ void CameraStereoDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & f
 #endif
 }
 
+//
+// CameraTriclops
+//
+CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transform & localTransform) :
+		CameraRGBD(imageRate, localTransform),
+		camera_(0),
+		triclopsCtx_(0)
+{
+#ifdef WITH_FLYCAPTURE2
+	camera_ = new FlyCapture2::Camera();
+#endif
+}
+
+CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
+{
+#ifdef WITH_FLYCAPTURE2
+	// Close the camera
+	camera_->StopCapture();
+	camera_->Disconnect();
+
+	// Destroy the Triclops context
+	triclopsDestroyContext( triclopsCtx_ ) ;
+
+	delete camera_;
+#endif
+}
+
+bool CameraStereoFlyCapture2::available()
+{
+#ifdef WITH_FLYCAPTURE2
+	return true;
+#else
+	return false;
+#endif
+}
+
+bool CameraStereoFlyCapture2::init(const std::string & calibrationFolder)
+{
+#ifdef WITH_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;
+	}
+
+	float fx, cx, cy, baseline;
+	triclopsGetFocalLength(triclopsCtx_, &fx);
+	triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
+	triclopsGetBaseline(triclopsCtx_, &baseline);
+	UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f", fx, cx, cy, baseline);
+
+	triclopsSetCameraConfiguration(triclopsCtx_, TriCfg_2CAM_HORIZONTAL_NARROW );
+	UASSERT(triclopsSetResolutionAndPrepare(triclopsCtx_, maxHeight, maxWidth, maxHeight, maxWidth) == Fc2Triclops::ERRORTYPE_OK);
+
+	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;
+}
+
+bool CameraStereoFlyCapture2::isCalibrated() const
+{
+#ifdef WITH_FLYCAPTURE2
+	if(triclopsCtx_)
+	{
+		float fx, cx, cy, baseline;
+		triclopsGetFocalLength(triclopsCtx_, &fx);
+		triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
+		triclopsGetBaseline(triclopsCtx_, &baseline);
+		return fx > 0.0f && cx > 0.0f && cy > 0.0f && baseline > 0.0f;
+	}
+#endif
+	return false;
+}
+
+std::string CameraStereoFlyCapture2::getSerial() const
+{
+#ifdef WITH_FLYCAPTURE2
+	if(camera_ && camera_->IsConnected())
+	{
+		FlyCapture2::CameraInfo camInfo;
+		if(camera_->GetCameraInfo(&camInfo) == FlyCapture2::PGRERROR_OK)
+		{
+			return uNumber2Str(camInfo.serialNumber);
+		}
+	}
+#endif
+	return "";
+}
+
+// struct containing image needed for processing
+#ifdef WITH_FLYCAPTURE2
+struct ImageContainer
+{
+	FlyCapture2::Image tmp[2];
+    FlyCapture2::Image unprocessed[2];
+} ;
+#endif
+
+void CameraStereoFlyCapture2::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy)
+{
+#ifdef WITH_FLYCAPTURE2
+	left = cv::Mat();
+	right = cv::Mat();
+	fx = 0.0f;
+	baseline = 0.0f;
+	cx = 0.0f;
+	cy = 0.0f;
+
+	if(camera_ && triclopsCtx_ && camera_->IsConnected())
+	{
+		// grab image from camera.
+		// this image contains both right and left images
+		FlyCapture2::Image grabbedImage;
+		if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK)
+		{
+			// right and left image extracted from grabbed image
+			ImageContainer imageCont;
+
+			// generate triclops input from grabbed image
+			FlyCapture2::Image imageRawRight;
+			FlyCapture2::Image imageRawLeft;
+			FlyCapture2::Image * unprocessedImage = imageCont.unprocessed;
+
+			// Convert the pixel interleaved raw data to de-interleaved and color processed data
+			if(Fc2Triclops::unpackUnprocessedRawOrMono16Image(
+										   grabbedImage,
+										   true /*assume little endian*/,
+										   imageRawLeft /* right */,
+										   imageRawRight /* left */) == Fc2Triclops::ERRORTYPE_OK)
+			{
+				// convert to color
+				FlyCapture2::Image srcImgRightRef(imageRawRight);
+				FlyCapture2::Image srcImgLeftRef(imageRawLeft);
+
+				bool ok = true;;
+				if ( srcImgRightRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK ||
+				     srcImgLeftRef.SetColorProcessing(FlyCapture2::HQ_LINEAR) != FlyCapture2::PGRERROR_OK)
+				{
+					ok = false;
+				}
+
+				if(ok)
+				{
+					FlyCapture2::Image imageColorRight;
+					FlyCapture2::Image imageColorLeft;
+					if ( srcImgRightRef.Convert(FlyCapture2::PIXEL_FORMAT_MONO8, &imageColorRight) != FlyCapture2::PGRERROR_OK ||
+						 srcImgLeftRef.Convert(FlyCapture2::PIXEL_FORMAT_BGRU, &imageColorLeft) != FlyCapture2::PGRERROR_OK)
+					{
+						ok = false;
+					}
+
+					if(ok)
+					{
+						//RECTIFY RIGHT
+						TriclopsInput triclopsColorInputs;
+						triclopsBuildRGBTriclopsInput(
+							grabbedImage.GetCols(),
+							grabbedImage.GetRows(),
+							imageColorRight.GetStride(),
+							(unsigned long)grabbedImage.GetTimeStamp().seconds,
+							(unsigned long)grabbedImage.GetTimeStamp().microSeconds,
+							imageColorRight.GetData(),
+							imageColorRight.GetData(),
+							imageColorRight.GetData(),
+							&triclopsColorInputs);
+
+						triclopsRectify(triclopsCtx_, const_cast<TriclopsInput *>(&triclopsColorInputs) );
+						// Retrieve the rectified image from the triclops context
+						TriclopsImage rectifiedImage;
+						triclopsGetImage( triclopsCtx_,
+							TriImg_RECTIFIED,
+							TriCam_REFERENCE,
+							&rectifiedImage );
+
+						right = cv::Mat(rectifiedImage.nrows, rectifiedImage.ncols, CV_8UC1, rectifiedImage.data).clone();
+
+						//RECTIFY LEFT COLOR
+						triclopsBuildPackedTriclopsInput(
+							grabbedImage.GetCols(),
+							grabbedImage.GetRows(),
+							imageColorLeft.GetStride(),
+							(unsigned long)grabbedImage.GetTimeStamp().seconds,
+							(unsigned long)grabbedImage.GetTimeStamp().microSeconds,
+							imageColorLeft.GetData(),
+							&triclopsColorInputs );
+
+						cv::Mat pixelsLeftBuffer( grabbedImage.GetRows(), grabbedImage.GetCols(), CV_8UC4);
+						TriclopsPackedColorImage colorImage;
+						triclopsSetPackedColorImageBuffer(
+							triclopsCtx_,
+							TriCam_LEFT,
+							(TriclopsPackedColorPixel*)pixelsLeftBuffer.data );
+
+						triclopsRectifyPackedColorImage(
+							triclopsCtx_,
+							TriCam_LEFT,
+							&triclopsColorInputs,
+							&colorImage );
+
+						cv::cvtColor(pixelsLeftBuffer, left, CV_RGBA2RGB);
+
+						// Set calibration stuff
+						triclopsGetFocalLength(triclopsCtx_, &fx);
+						triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
+						triclopsGetBaseline(triclopsCtx_, &baseline);
+					}
+				}
+			}
+		}
+	}
+
+#else
+	UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
+#endif
+}
+
 } // namespace rtabmap