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

CMakeLists.txt

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 8)
-SET(RTABMAP_PATCH_VERSION 8)
+SET(RTABMAP_PATCH_VERSION 9)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -138,6 +138,7 @@ FIND_PACKAGE(freenect2 QUIET)
 FIND_PACKAGE(OpenNI2)
 FIND_PACKAGE(DC1394)
 FIND_PACKAGE(G2O)
+FIND_PACKAGE(FlyCapture2)
 
 # If Qt is here, the GUI will be built
 IF("${RTABMAP_QT_VERSION}" STREQUAL "4")
@@ -357,6 +358,12 @@ ELSE()
 MESSAGE(STATUS "  With dc1394  = NO (dc1394 not found)")
 ENDIF()
 
+IF(FlyCapture2_FOUND)
+MESSAGE(STATUS "  With FlyCapture2/Triclops  = YES")
+ELSE()
+MESSAGE(STATUS "  With FlyCapture2/Triclops  = NO (Point Grey SDK not found)")
+ENDIF()
+
 IF(G2O_FOUND)
 MESSAGE(STATUS "  With g2o  = YES")
 ELSE()

cmake_modules/FindFlyCapture2.cmake

@@ -0,0 +1,36 @@
+# - Find FlyCapture2 
+# This module finds an installed FlyCapture2+Triclops stereo camera package. (Point Grey SDK)
+#
+# It sets the following variables:
+#  FlyCapture2_FOUND       - Set to false, or undefined, if FlyCapture2 isn't found.
+#  FlyCapture2_INCLUDE_DIRS - The FlyCapture2 include directory.
+#  FlyCapture2_LIBRARIES     - The FlyCapture2 library to link against.
+
+#FlyCapture2 SDK
+find_path(FlyCapture2_INCLUDE_DIR NAMES FlyCapture2.h PATHS $ENV{FlyCapture2_ROOT_DIR}/include)
+find_library(FlyCapture2_LIBRARY NAMES FlyCapture2_v100 FlyCapture2 flycapture2 PATHS $ENV{FlyCapture2_ROOT_DIR}/lib64 $ENV{FlyCapture2_ROOT_DIR}/lib)
+
+# Triclops SDK
+find_path(Triclops_INCLUDE_DIR NAMES triclops.h PATHS $ENV{Triclops_ROOT_DIR}/include)
+find_library(Triclops_LIBRARY NAMES triclops triclops_v100 PATHS $ENV{Triclops_ROOT_DIR}/lib64 $ENV{Triclops_ROOT_DIR}/lib)
+find_library(FlyCaptureBridge_LIBRARY NAMES flycapture2bridge flycapture2bridge_v100 PATHS $ENV{Triclops_ROOT_DIR}/lib64 $ENV{Triclops_ROOT_DIR}/lib)
+find_library(pnmutils_LIBRARY NAMES pnmutils pnmutils_v100 PATHS $ENV{Triclops_ROOT_DIR}/lib64 $ENV{Triclops_ROOT_DIR}/lib)
+
+IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
+   SET(FlyCapture2_FOUND TRUE)
+   SET(FlyCapture2_INCLUDE_DIRS ${FlyCapture2_INCLUDE_DIR} ${Triclops_INCLUDE_DIR})
+   SET(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} ${Triclops_LIBRARY} ${FlyCaptureBridge_LIBRARY} ${pnmutils_LIBRARY})
+ENDIF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
+
+IF (FlyCapture2_FOUND)
+   # show which FlyCapture2 was found only if not quiet
+   IF (NOT FlyCapture2_FIND_QUIETLY)
+      MESSAGE(STATUS "Found FlyCapture2: ${FlyCapture2_LIBRARIES}")
+   ENDIF (NOT FlyCapture2_FIND_QUIETLY)
+ELSE (FlyCapture2_FOUND)
+   # fatal error if FlyCapture2 is required but not found
+   IF (FlyCapture2_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find FlyCapture2 (FlyCapture2 Stereo Vision SDK)")
+   ENDIF (FlyCapture2_FIND_REQUIRED)
+ENDIF (FlyCapture2_FOUND)
+

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

guilib/include/rtabmap/gui/MainWindow.h

@@ -150,6 +150,7 @@ private slots:
 	void selectOpenni2();
 	void selectFreenect2();
 	void selectStereoDC1394();
+	void selectStereoFlyCapture2();
 	void dumpTheMemory();
 	void dumpThePrediction();
 	void downloadAllClouds();

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -89,7 +89,8 @@ public:
 		kSrcOpenNI_CV_ASUS,
 		kSrcOpenNI2,
 		kSrcFreenect2,
-		kSrcStereoDC1394
+		kSrcStereoDC1394,
+		kSrcStereoFlyCapture2
 	};
 
 public:
@@ -242,7 +243,7 @@ private slots:
 	void setupTreeView();
 	void updateBasicParameter();
 	void openDatabaseViewer();
-	void showOpenNI2GroupBox(bool);
+	void updateOpenNI2GroupBoxVisibility();
 	void testOdometry();
 	void testRGBDCamera();
 

guilib/src/AboutDialog.cpp

@@ -55,6 +55,10 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_pcl_version->setText(PCL_VERSION_PRETTY);
 	_ui->label_freenect->setText(CameraFreenect::available()?"Yes":"No");
 	_ui->label_openni2->setText(CameraOpenNI2::available()?"Yes":"No");
+	_ui->label_openni2->setText(CameraFreenect2::available()?"Yes":"No");
+	_ui->label_openni2->setText(CameraStereoDC1394::available()?"Yes":"No");
+	_ui->label_openni2->setText(CameraStereoFlyCapture2::available()?"Yes":"No");
+
 	_ui->label_g2o->setText(graph::G2OOptimizer::available()?"Yes":"No");
 }
 

guilib/src/CalibrationDialog.cpp

@@ -433,7 +433,7 @@ void CalibrationDialog::processImages(const cv::Mat & imageLeft, const cv::Mat &
 	{
 		for(int id=0; id<(stereo_?2:1); ++id)
 		{
-			int step = imageSize_[id].height/12;
+			int step = imageSize_[id].height/16;
 			for(int i=step; i<imageSize_[id].height; i+=step)
 			{
 				cv::line(images[id], cv::Point(0, i), cv::Point(imageSize_[id].width, i), CV_RGB(0,255,0));

guilib/src/MainWindow.cpp

@@ -353,6 +353,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	connect(_ui->actionOpenNI2_sense, SIGNAL(triggered()), this, SLOT(selectOpenni2()));
 	connect(_ui->actionFreenect2, SIGNAL(triggered()), this, SLOT(selectFreenect2()));
 	connect(_ui->actionStereoDC1394, SIGNAL(triggered()), this, SLOT(selectStereoDC1394()));
+	connect(_ui->actionStereoFlyCapture2, SIGNAL(triggered()), this, SLOT(selectStereoFlyCapture2()));
 	_ui->actionFreenect->setEnabled(CameraFreenect::available());
 	_ui->actionOpenNI_CV->setEnabled(CameraOpenNICV::available());
 	_ui->actionOpenNI_CV_ASUS->setEnabled(CameraOpenNICV::available());
@@ -361,6 +362,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->actionOpenNI2_sense->setEnabled(CameraOpenNI2::available());
 	_ui->actionFreenect2->setEnabled(CameraFreenect2::available());
 	_ui->actionStereoDC1394->setEnabled(CameraStereoDC1394::available());
+	_ui->actionStereoFlyCapture2->setEnabled(CameraStereoFlyCapture2::available());
 	this->updateSelectSourceMenu();
 
 	connect(_ui->actionSave_state, SIGNAL(triggered()), this, SLOT(saveFigures()));
@@ -2235,6 +2237,7 @@ void MainWindow::updateSelectSourceMenu()
 	_ui->actionOpenNI2_sense->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcOpenNI2);
 	_ui->actionFreenect2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcFreenect2);
 	_ui->actionStereoDC1394->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcStereoDC1394);
+	_ui->actionStereoFlyCapture2->setChecked(_preferencesDialog->isSourceRGBDUsed() && _preferencesDialog->getSourceRGBD() == PreferencesDialog::kSrcStereoFlyCapture2);
 }
 
 void MainWindow::changeImgRateSetting()
@@ -3575,6 +3578,11 @@ void MainWindow::selectStereoDC1394()
 	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcStereoDC1394);
 }
 
+void MainWindow::selectStereoFlyCapture2()
+{
+	_preferencesDialog->selectSourceRGBD(PreferencesDialog::kSrcStereoFlyCapture2);
+}
+
 
 void MainWindow::dumpTheMemory()
 {

guilib/src/PreferencesDialog.cpp

@@ -120,14 +120,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->label_map_shown->setText(_ui->label_map_shown->text() + " (Disabled, PCL >=1.7.2 required)");
 #endif
 
-#ifdef _WIN32
-	_ui->radioButton_openni2->setChecked(true);
-	_ui->radioButton_opennipcl->setChecked(false);
-	_ui->groupBox_openni2->setVisible(true);
-#else
-	_ui->groupBox_openni2->setVisible(false);
-#endif
-
 	if(RTABMAP_NONFREE == 0)
 	{
 		_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
@@ -166,6 +158,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	{
 		_ui->comboBox_cameraRGBD->setItemData(6, 0, Qt::UserRole - 1);
 	}
+	if(!CameraStereoFlyCapture2::available())
+	{
+		_ui->comboBox_cameraRGBD->setItemData(7, 0, Qt::UserRole - 1);
+	}
 	_ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable());
 	_ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable());
 
@@ -300,6 +296,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	//openni group
 	connect(_ui->groupBox_sourceOpenni, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_cameraRGBD, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOpenNI2GroupBoxVisibility()));
 	connect(_ui->openni2_autoWhiteBalance, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_autoExposure, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->openni2_exposure, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -932,6 +929,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		{
 			_ui->comboBox_cameraRGBD->setCurrentIndex(1); // freenect
 		}
+		else if(CameraOpenNI2::available())
+		{
+			_ui->comboBox_cameraRGBD->setCurrentIndex(4); // openni2
+		}
 		else
 		{
 			_ui->comboBox_cameraRGBD->setCurrentIndex(0); // openni-pcl
@@ -1657,7 +1658,7 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
 		_ui->label_dictionaryPath->setEnabled(false);
 
 		_ui->groupBox_source0->setEnabled(false);
-		_ui->groupBox_odometry2->setEnabled(false);
+		_ui->groupBox_odometry1->setEnabled(false);
 
 		this->setWindowTitle(tr("Preferences [Monitoring mode]"));
 	}
@@ -1672,7 +1673,7 @@ void PreferencesDialog::showEvent ( QShowEvent * event )
 		_ui->label_dictionaryPath->setEnabled(true);
 
 		_ui->groupBox_source0->setEnabled(true);
-		_ui->groupBox_odometry2->setEnabled(true);
+		_ui->groupBox_odometry1->setEnabled(true);
 
 		this->setWindowTitle(tr("Preferences"));
 	}
@@ -2789,9 +2790,9 @@ void PreferencesDialog::changeDictionaryPath()
 	}
 }
 
-void PreferencesDialog::showOpenNI2GroupBox(bool shown)
+void PreferencesDialog::updateOpenNI2GroupBoxVisibility()
 {
-	_ui->groupBox_openni2->setVisible(shown);
+	_ui->groupBox_openni2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcOpenNI_PCL);
 }
 
 /*** GETTERS ***/
@@ -3132,6 +3133,12 @@ CameraRGBD * PreferencesDialog::createCameraRGBD() const
 			this->getGeneralInputRate(),
 			this->getSourceOpenniLocalTransform());
 	}
+	else if(this->getSourceRGBD() == kSrcStereoFlyCapture2)
+	{
+		return new CameraStereoFlyCapture2(
+			this->getGeneralInputRate(),
+			this->getSourceOpenniLocalTransform());
+	}
 	else
 	{
 		UFATAL("RGBD Source type undefined!");

guilib/src/ui/aboutDialog.ui

@@ -82,6 +82,13 @@ p, li { white-space: pre-wrap; }
         </item>
         <item>
          <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
+          <item row="13" column="0">
+           <widget class="QLabel" name="label_16">
+            <property name="text">
+             <string>With stereo dc1394 :</string>
+            </property>
+           </widget>
+          </item>
           <item row="7" column="0">
            <widget class="QLabel" name="label_6">
             <property name="text">
@@ -205,7 +212,7 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="12" column="0">
+          <item row="15" column="0">
            <widget class="QLabel" name="label_14">
             <property name="text">
              <string>With g2o :</string>
@@ -232,7 +239,7 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="12" column="1">
+          <item row="15" column="1">
            <widget class="QLabel" name="label_g2o">
             <property name="text">
              <string/>
@@ -242,6 +249,50 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
+          <item row="12" column="0">
+           <widget class="QLabel" name="label_15">
+            <property name="text">
+             <string>With Freenect2 :</string>
+            </property>
+           </widget>
+          </item>
+          <item row="14" column="0">
+           <widget class="QLabel" name="label_17">
+            <property name="text">
+             <string>With stereo FlyCapture2 :</string>
+            </property>
+           </widget>
+          </item>
+          <item row="12" column="1">
+           <widget class="QLabel" name="label_freenect2">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="13" column="1">
+           <widget class="QLabel" name="label_dc1394">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="14" column="1">
+           <widget class="QLabel" name="label_flycapture2">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+            </property>
+           </widget>
+          </item>
          </layout>
         </item>
         <item>

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>1012</width>
-     <height>25</height>
+     <height>21</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -156,6 +156,7 @@
          <normaloff>:/images/bumblebee2.png</normaloff>:/images/bumblebee2.png</iconset>
        </property>
        <addaction name="actionStereoDC1394"/>
+       <addaction name="actionStereoFlyCapture2"/>
       </widget>
       <addaction name="menuKinect_for_Xbox_360"/>
       <addaction name="menuXtion_PRO_LIVE"/>
@@ -1209,6 +1210,11 @@
     <string>OpenNI2</string>
    </property>
   </action>
+  <action name="actionStereoFlyCapture2">
+   <property name="text">
+    <string>StereoFlyCapture2</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-354</y>
+            <y>0</y>
-            <width>737</width>
+            <width>736</width>
-            <height>982</height>
+            <height>781</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>3</number>
+                <number>0</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1728,6 +1728,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                              <string>StereoDC1394</string>
                             </property>
                            </item>
+                           <item>
+                            <property name="text">
+                             <string>StereoFlyCapture2</string>
+                            </property>
+                           </item>
                           </widget>
                          </item>
                          <item row="1" column="1">
@@ -6264,7 +6269,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                <widget class="QWidget" name="page_12">
                 <layout class="QVBoxLayout" name="verticalLayout_32">
                  <item>
-                  <widget class="QGroupBox" name="groupBox_odometry2">
+                  <widget class="QGroupBox" name="groupBox_odometry1">
                    <property name="title">
                     <string>Odometry</string>
                    </property>
@@ -6280,11 +6285,11 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                      </widget>
                     </item>
                     <item>
-                     <layout class="QGridLayout" name="gridLayout_27" columnstretch="0,0">
+                     <layout class="QGridLayout" name="gridLayout_27" columnstretch="0,1">
                       <item row="2" column="1">
                        <widget class="QLabel" name="label_47">
                         <property name="text">
-                         <string>Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).</string>
+                         <string>Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset). When reset, the odometry starts from the last pose computed.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -6418,6 +6423,13 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
+                      <item row="5" column="0">
+                       <widget class="QPushButton" name="pushButton_testOdometry">
+                        <property name="text">
+                         <string>Test selected odometry</string>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -6787,30 +6799,6 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </layout>
                      </widget>
                     </item>
-                    <item>
-                     <layout class="QHBoxLayout" name="horizontalLayout_5">
-                      <item>
-                       <widget class="QPushButton" name="pushButton_testOdometry">
-                        <property name="text">
-                         <string>Test selected odometry</string>
-                        </property>
-                       </widget>
-                      </item>
-                      <item>
-                       <spacer name="horizontalSpacer_2">
-                        <property name="orientation">
-                         <enum>Qt::Horizontal</enum>
-                        </property>
-                        <property name="sizeHint" stdset="0">
-                         <size>
-                          <width>40</width>
-                          <height>20</height>
-                         </size>
-                        </property>
-                       </spacer>
-                      </item>
-                     </layout>
-                    </item>
                    </layout>
                   </widget>
                  </item>
@@ -6832,7 +6820,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                <widget class="QWidget" name="page_26">
                 <layout class="QVBoxLayout" name="verticalLayout_54">
                  <item>
-                  <widget class="QGroupBox" name="groupBox_odometryBOW3">
+                  <widget class="QGroupBox" name="groupBox_odometryBOW2">
                    <property name="title">
                     <string>BOW</string>
                    </property>
@@ -6957,7 +6945,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                <widget class="QWidget" name="page_28">
                 <layout class="QVBoxLayout" name="verticalLayout_55">
                  <item>
-                  <widget class="QGroupBox" name="groupBox_odometryFlow3">
+                  <widget class="QGroupBox" name="groupBox_odometryFlow2">
                    <property name="title">
                     <string>Optical Flow</string>
                    </property>

tools/Calibration/main.cpp

@@ -46,6 +46,7 @@ void showUsage()
 			"                                       4=OpenNI-CV-ASUS (Xtion PRO Live)\n"
 			"                                       5=Freenect2  (Kinect v2)\n"
 			"                                       6=DC1394     (Bumblebee2)\n"
+			"                                       7=FlyCapture2 (Bumblebee2)\n"
 			"  --device #     Device id\n"
 			"  --debug        Debug log\n"
 			"  --stereo       Stereo\n\n");
@@ -56,6 +57,8 @@ int main(int argc, char * argv[])
 {
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kInfo);
+	ULogger::setPrintTime(false);
+	ULogger::setPrintWhere(false);
 
 	int driver = -1;
 	int device = 0;
@@ -99,6 +102,8 @@ int main(int argc, char * argv[])
 		if(strcmp(argv[i], "--debug") == 0)
 		{
 			ULogger::setLevel(ULogger::kDebug);
+			ULogger::setPrintTime(true);
+			ULogger::setPrintWhere(true);
 			continue;
 		}
 		if(strcmp(argv[i], "--stereo") == 0)
@@ -113,9 +118,9 @@ int main(int argc, char * argv[])
 		printf("Unrecognized option : %s\n", argv[i]);
 		showUsage();
 	}
-	if(driver < -1 || driver > 6)
+	if(driver < -1 || driver > 7)
 	{
-		UERROR("driver should be between -1 and 6.");
+		UERROR("driver should be between -1 and 7.");
 		showUsage();
 	}
 
@@ -187,6 +192,15 @@ int main(int argc, char * argv[])
 		}
 		camera = new rtabmap::CameraStereoDC1394();
 	}
+	else if(driver == 7)
+	{
+		if(!rtabmap::CameraStereoFlyCapture2::available())
+		{
+			UERROR("Not built with FlyCapture2/Triclops support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoFlyCapture2();
+	}
 	else
 	{
 		UFATAL("");
@@ -206,7 +220,7 @@ int main(int argc, char * argv[])
 	}
 	else if(camera)
 	{
-		if(!camera->init())
+		if(!camera->init(""))
 		{
 			printf("Camera init failed!\n");
 			delete camera;

tools/CameraRGBD/main.cpp

@@ -43,7 +43,8 @@ void showUsage()
 			"                                     3=OpenNI-CV  (Kinect)\n"
 			"                                     4=OpenNI-CV-ASUS (Xtion PRO Live)\n"
 			"                                     5=Freenect2  (Kinect v2)\n"
-			"                                     6=DC1394     (Bumblebee2)\n\n");
+			"                                     6=DC1394     (Bumblebee2)\n"
+			"                                     7=FlyCapture2 (Bumblebee2)\n");
 	exit(1);
 }
 
@@ -51,6 +52,8 @@ int main(int argc, char * argv[])
 {
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kInfo);
+	ULogger::setPrintTime(false);
+	ULogger::setPrintWhere(false);
 
 	int driver = 0;
 	if(argc < 2)
@@ -64,7 +67,7 @@ int main(int argc, char * argv[])
 			showUsage();
 		}
 		driver = atoi(argv[argc-1]);
-		if(driver < 0 || driver > 6)
+		if(driver < 0 || driver > 7)
 		{
 			UERROR("driver should be between 0 and 6.");
 			showUsage();
@@ -131,6 +134,15 @@ int main(int argc, char * argv[])
 		}
 		camera = new rtabmap::CameraStereoDC1394();
 	}
+	else if(driver == 7)
+	{
+		if(!rtabmap::CameraStereoFlyCapture2::available())
+		{
+			UERROR("Not built with FlyCapture2/Triclops support...");
+			exit(-1);
+		}
+		camera = new rtabmap::CameraStereoFlyCapture2();
+	}
 	else
 	{
 		UFATAL("");

tools/OdometryViewer/main.cpp

@@ -44,7 +44,7 @@ void showUsage()
 	printf("\nUsage:\n"
 			"odometryViewer [options]\n"
 			"Options:\n"
-			"  -driver #                 Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=dc1394\n"
+			"  -driver #                 Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS, 5=Freenect2, 6=dc1394, 7=FlyCapture2\n"
 			"  -o #                      Odometry type (default 6): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/BRIEF, 7=BRISK\n"
 			"  -nn #                     Nearest neighbor strategy (default 3): kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4\n"
 			"  -nndr #                   Nearest neighbor distance ratio (default 0.7)\n"
@@ -128,7 +128,7 @@ int main (int argc, char * argv[])
 			if(i < argc)
 			{
 				driver = std::atoi(argv[i]);
-				if(driver < 0 || driver > 6)
+				if(driver < 0 || driver > 7)
 				{
 					showUsage();
 				}
@@ -766,6 +766,15 @@ int main (int argc, char * argv[])
 			}
 			camera = new rtabmap::CameraStereoDC1394(rate, t);
 		}
+		else if(driver == 7)
+		{
+			if(!rtabmap::CameraStereoFlyCapture2::available())
+			{
+				UERROR("Not built with FlyCapture2/Triclops support...");
+				exit(-1);
+			}
+			camera = new rtabmap::CameraStereoFlyCapture2(rate, t);
+		}
 		else
 		{
 			UFATAL("Camera driver (%d) not found!", driver);