Camera refactoring (#315)

* Camera Refactoring part 1 (Mac OS X)

* fixed camera*** -> Camera***

* Fixed build for cameras Zed/RealSense/RealSense2

* increased version to 0.17.7

* fixed build for cameras K4W2 and FlyCapture2

corelib/src/CMakeLists.txt

@@ -15,11 +15,25 @@ SET(SRC_FILES
 	
     Camera.cpp
     CameraThread.cpp
-    CameraRGB.cpp
-    CameraRGBD.cpp
-    CameraStereo.cpp
     CameraModel.cpp
     
+    camera/CameraFreenect.cpp
+    camera/CameraFreenect2.cpp
+    camera/CameraImages.cpp
+    camera/CameraK4W2.cpp
+    camera/CameraOpenni.cpp
+    camera/CameraOpenNI2.cpp
+    camera/CameraOpenNICV.cpp
+    camera/CameraRealSense.cpp
+    camera/CameraRealSense2.cpp
+    camera/CameraRGBDImages.cpp
+    camera/CameraStereoDC1394.cpp
+    camera/CameraStereoFlyCapture2.cpp
+    camera/CameraStereoImages.cpp
+    camera/CameraStereoVideo.cpp
+    camera/CameraStereoZed.cpp
+    camera/CameraVideo.cpp
+    
     EpipolarGeometry.cpp
 	VisualWord.cpp
 	VWDictionary.cpp
@@ -49,10 +63,10 @@ SET(SRC_FILES
 	LaserScan.cpp
 	
 	Optimizer.cpp
-	OptimizerTORO.cpp
-	OptimizerG2O.cpp
-	OptimizerGTSAM.cpp
-	OptimizerCVSBA.cpp
+	optimizer/OptimizerTORO.cpp
+	optimizer/OptimizerG2O.cpp
+	optimizer/OptimizerGTSAM.cpp
+	optimizer/OptimizerCVSBA.cpp
 	
 	Registration.cpp
 	RegistrationIcp.cpp
@@ -60,16 +74,16 @@ SET(SRC_FILES
 	
 	Odometry.cpp
 	OdometryThread.cpp
-	OdometryF2M.cpp
-	OdometryMono.cpp
-	OdometryF2F.cpp
-	OdometryFovis.cpp
-	OdometryViso2.cpp
-	OdometryDVO.cpp
-	OdometryOkvis.cpp
-	OdometryORBSLAM2.cpp
-	OdometryLOAM.cpp
-	OdometryMSCKF.cpp
+	odometry/OdometryF2M.cpp
+	odometry/OdometryMono.cpp
+	odometry/OdometryF2F.cpp
+	odometry/OdometryFovis.cpp
+	odometry/OdometryViso2.cpp
+	odometry/OdometryDVO.cpp
+	odometry/OdometryOkvis.cpp
+	odometry/OdometryORBSLAM2.cpp
+	odometry/OdometryLOAM.cpp
+	odometry/OdometryMSCKF.cpp
 	
 	IMUThread.cpp
 	
@@ -248,12 +262,12 @@ ENDIF(FlyCapture2_FOUND)
 IF(WITH_TORO)
 	SET(SRC_FILES
     	${SRC_FILES}
-		toro3d/posegraph3.cpp
-		toro3d/treeoptimizer3_iteration.cpp
-		toro3d/treeoptimizer3.cpp
+		optimizer/toro3d/posegraph3.cpp
+		optimizer/toro3d/treeoptimizer3_iteration.cpp
+		optimizer/toro3d/treeoptimizer3.cpp
 	
-		toro3d/posegraph2.cpp
-		toro3d/treeoptimizer2.cpp
+		optimizer/toro3d/posegraph2.cpp
+		optimizer/toro3d/treeoptimizer2.cpp
 	)
 ENDIF(WITH_TORO)
 
@@ -270,11 +284,11 @@ IF(G2O_FOUND)
 	IF(WITH_VERTIGO)
 		SET(SRC_FILES
     		${SRC_FILES}
-			vertigo/g2o/edge_se2Switchable.cpp
-			vertigo/g2o/edge_se3Switchable.cpp
-			vertigo/g2o/edge_switchPrior.cpp
-			vertigo/g2o/types_g2o_robust.cpp
-			vertigo/g2o/vertex_switchLinear.cpp
+			optimizer/vertigo/g2o/edge_se2Switchable.cpp
+			optimizer/vertigo/g2o/edge_se3Switchable.cpp
+			optimizer/vertigo/g2o/edge_switchPrior.cpp
+			optimizer/vertigo/g2o/types_g2o_robust.cpp
+			optimizer/vertigo/g2o/vertex_switchLinear.cpp
 		)
 	ENDIF(WITH_VERTIGO)
 ENDIF(G2O_FOUND)

corelib/src/Graph.cpp

@@ -43,8 +43,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <queue>
 #include <fstream>
 
-#include <rtabmap/core/OptimizerTORO.h>
-#include <rtabmap/core/OptimizerG2O.h>
+#include <rtabmap/core/optimizer/OptimizerTORO.h>
+#include <rtabmap/core/optimizer/OptimizerG2O.h>
 
 namespace rtabmap {
 

corelib/src/Memory.cpp

@@ -57,7 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Compression.h"
 #include "rtabmap/core/Graph.h"
 #include "rtabmap/core/Stereo.h"
-#include "rtabmap/core/OptimizerG2O.h"
+#include "rtabmap/core/optimizer/OptimizerG2O.h"
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/common.h>
 #include <rtabmap/core/OccupancyGrid.h>

corelib/src/Odometry.cpp

@@ -25,16 +25,16 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include <rtabmap/core/OdometryF2M.h>
 #include "rtabmap/core/Odometry.h"
-#include "rtabmap/core/OdometryF2F.h"
-#include "rtabmap/core/OdometryFovis.h"
-#include "rtabmap/core/OdometryViso2.h"
-#include "rtabmap/core/OdometryDVO.h"
-#include "rtabmap/core/OdometryOkvis.h"
-#include "rtabmap/core/OdometryORBSLAM2.h"
-#include "rtabmap/core/OdometryLOAM.h"
-#include "rtabmap/core/OdometryMSCKF.h"
+#include <rtabmap/core/odometry/OdometryF2M.h>
+#include "rtabmap/core/odometry/OdometryF2F.h"
+#include "rtabmap/core/odometry/OdometryFovis.h"
+#include "rtabmap/core/odometry/OdometryViso2.h"
+#include "rtabmap/core/odometry/OdometryDVO.h"
+#include "rtabmap/core/odometry/OdometryOkvis.h"
+#include "rtabmap/core/odometry/OdometryORBSLAM2.h"
+#include "rtabmap/core/odometry/OdometryLOAM.h"
+#include "rtabmap/core/odometry/OdometryMSCKF.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d.h"
 #include "rtabmap/core/util3d_mapping.h"

corelib/src/OdometryThread.cpp

@@ -25,10 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include <rtabmap/core/OdometryF2M.h>
 #include "rtabmap/core/OdometryThread.h"
 #include "rtabmap/core/Odometry.h"
-#include "rtabmap/core/OdometryMono.h"
+#include "rtabmap/core/odometry/OdometryMono.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/OdometryEvent.h"

corelib/src/Optimizer.cpp

@@ -36,10 +36,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <set>
 #include <queue>
 
-#include <rtabmap/core/OptimizerTORO.h>
-#include <rtabmap/core/OptimizerG2O.h>
-#include <rtabmap/core/OptimizerGTSAM.h>
-#include <rtabmap/core/OptimizerCVSBA.h>
+#include <rtabmap/core/optimizer/OptimizerTORO.h>
+#include <rtabmap/core/optimizer/OptimizerG2O.h>
+#include <rtabmap/core/optimizer/OptimizerGTSAM.h>
+#include <rtabmap/core/optimizer/OptimizerCVSBA.h>
 
 namespace rtabmap {
 

corelib/src/camera/CameraFreenect.cpp

@@ -0,0 +1,477 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraFreenect.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/core/util2d.h>
+
+#ifdef RTABMAP_FREENECT
+#include <libfreenect.h>
+#ifdef FREENECT_DASH_INCLUDES
+#include <libfreenect-registration.h>
+#else
+#include <libfreenect_registration.h>
+#endif
+#endif
+
+namespace rtabmap
+{
+
+#ifdef RTABMAP_FREENECT
+
+class FreenectDevice : public UThread {
+  public:
+	FreenectDevice(freenect_context * ctx, int index, bool color = true, bool registered = true) :
+		index_(index),
+		color_(color),
+		registered_(registered),
+		ctx_(ctx),
+		device_(0),
+		depthFocal_(0.0f)
+	{
+		UASSERT(ctx_ != 0);
+	}
+
+	virtual ~FreenectDevice()
+	{
+		this->join(true);
+		if(device_ && freenect_close_device(device_) < 0){} //FN_WARNING("Device did not shutdown in a clean fashion");
+	}
+
+	const std::string & getSerial() const {return serial_;}
+
+	bool init()
+	{
+		if(device_)
+		{
+			this->join(true);
+			freenect_close_device(device_);
+			device_ = 0;
+		}
+		serial_.clear();
+		std::vector<std::string> deviceSerials;
+		freenect_device_attributes* attr_list;
+		freenect_device_attributes* item;
+		freenect_list_device_attributes(ctx_, &attr_list);
+		for (item = attr_list; item != NULL; item = item->next) {
+			deviceSerials.push_back(std::string(item->camera_serial));
+		}
+		freenect_free_device_attributes(attr_list);
+
+		if(freenect_open_device(ctx_, &device_, index_) < 0)
+		{
+			UERROR("FreenectDevice: Cannot open Kinect");
+			return false;
+		}
+
+		if(index_ >= 0 && index_ < (int)deviceSerials.size())
+		{
+			serial_ = deviceSerials[index_];
+		}
+		else
+		{
+			UERROR("Could not get serial for index %d", index_);
+		}
+
+		UINFO("color=%d registered=%d", color_?1:0, registered_?1:0);
+
+		freenect_set_user(device_, this);
+		freenect_frame_mode videoMode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, color_?FREENECT_VIDEO_RGB:FREENECT_VIDEO_IR_8BIT);
+		freenect_frame_mode depthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, color_ && registered_?FREENECT_DEPTH_REGISTERED:FREENECT_DEPTH_MM);
+		if(!videoMode.is_valid)
+		{
+			UERROR("Freenect: video mode selected not valid!");
+			return false;
+		}
+		if(!depthMode.is_valid)
+		{
+			UERROR("Freenect: depth mode selected not valid!");
+			return false;
+		}
+		UASSERT(videoMode.data_bits_per_pixel == 8 || videoMode.data_bits_per_pixel == 24);
+		UASSERT(depthMode.data_bits_per_pixel == 16);
+		freenect_set_video_mode(device_, videoMode);
+		freenect_set_depth_mode(device_, depthMode);
+		rgbIrBuffer_ = cv::Mat(cv::Size(videoMode.width,videoMode.height), color_?CV_8UC3:CV_8UC1);
+		depthBuffer_ = cv::Mat(cv::Size(depthMode.width,depthMode.height), CV_16UC1);
+		freenect_set_depth_buffer(device_, depthBuffer_.data);
+		freenect_set_video_buffer(device_, rgbIrBuffer_.data);
+		freenect_set_depth_callback(device_, freenect_depth_callback);
+		freenect_set_video_callback(device_, freenect_video_callback);
+
+		float rgb_focal_length_sxga = 1050.0f;
+		float width_sxga = 1280.0f;
+		float width = freenect_get_current_depth_mode(device_).width;
+		float scale = width / width_sxga;
+		if(color_ && registered_)
+		{
+			depthFocal_ =  rgb_focal_length_sxga * scale;
+		}
+		else
+		{
+			freenect_registration reg = freenect_copy_registration(device_);
+			float depth_focal_length_sxga = reg.zero_plane_info.reference_distance / reg.zero_plane_info.reference_pixel_size;
+			freenect_destroy_registration(&reg);
+
+			depthFocal_ =  depth_focal_length_sxga * scale;
+		}
+
+		UINFO("FreenectDevice: Depth focal = %f", depthFocal_);
+		return true;
+	}
+
+	float getDepthFocal() const {return depthFocal_;}
+
+	void getData(cv::Mat & rgb, cv::Mat & depth)
+	{
+		if(this->isRunning())
+		{
+			if(!dataReady_.acquire(1, 5000))
+			{
+				UERROR("Not received any frames since 5 seconds, try to restart the camera again.");
+			}
+			else
+			{
+				UScopeMutex s(dataMutex_);
+				rgb = rgbIrLastFrame_;
+				depth = depthLastFrame_;
+				rgbIrLastFrame_ = cv::Mat();
+				depthLastFrame_= cv::Mat();
+			}
+		}
+	}
+
+private:
+	// Do not call directly even in child
+	void VideoCallback(void* rgb)
+	{
+		UASSERT(rgbIrBuffer_.data == rgb);
+		UScopeMutex s(dataMutex_);
+		bool notify = rgbIrLastFrame_.empty();
+
+		if(color_)
+		{
+			cv::cvtColor(rgbIrBuffer_, rgbIrLastFrame_, CV_RGB2BGR);
+		}
+		else // IrDepth
+		{
+			rgbIrLastFrame_ = rgbIrBuffer_.clone();
+		}
+		if(!depthLastFrame_.empty() && notify)
+		{
+			dataReady_.release();
+		}
+	}
+
+	// Do not call directly even in child
+	void DepthCallback(void* depth)
+	{
+		UASSERT(depthBuffer_.data == depth);
+		UScopeMutex s(dataMutex_);
+		bool notify = depthLastFrame_.empty();
+		depthLastFrame_ = depthBuffer_.clone();
+		if(!rgbIrLastFrame_.empty() && notify)
+		{
+			dataReady_.release();
+		}
+	}
+
+	void startVideo() {
+		if(device_ && freenect_start_video(device_) < 0) UERROR("Cannot start RGB callback");
+	}
+	void stopVideo() {
+		if(device_ && freenect_stop_video(device_) < 0) UERROR("Cannot stop RGB callback");
+	}
+	void startDepth() {
+		if(device_ && freenect_start_depth(device_) < 0) UERROR("Cannot start depth callback");
+	}
+	void stopDepth() {
+		if(device_ && freenect_stop_depth(device_) < 0) UERROR("Cannot stop depth callback");
+	}
+
+	virtual void mainLoopBegin()
+	{
+		this->startDepth();
+		this->startVideo();
+	}
+
+	virtual void mainLoop()
+	{
+		timeval t;
+		t.tv_sec = 0;
+		t.tv_usec = 10000;
+		if(freenect_process_events_timeout(ctx_, &t) < 0)
+		{
+			UERROR("FreenectDevice: Cannot process freenect events");
+			this->kill();
+		}
+	}
+
+	virtual void mainLoopEnd()
+	{
+		this->stopDepth();
+		this->stopVideo();
+		dataReady_.release();
+	}
+
+	static void freenect_depth_callback(freenect_device *dev, void *depth, uint32_t timestamp) {
+		FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
+		device->DepthCallback(depth);
+	}
+	static void freenect_video_callback(freenect_device *dev, void *video, uint32_t timestamp) {
+		FreenectDevice* device = static_cast<FreenectDevice*>(freenect_get_user(dev));
+		device->VideoCallback(video);
+	}
+
+	//noncopyable
+	FreenectDevice( const FreenectDevice& );
+	const FreenectDevice& operator=( const FreenectDevice& );
+
+  private:
+	int index_;
+	bool color_;
+	bool registered_;
+	std::string serial_;
+	freenect_context * ctx_;
+	freenect_device * device_;
+	cv::Mat depthBuffer_;
+	cv::Mat rgbIrBuffer_;
+	UMutex dataMutex_;
+	cv::Mat depthLastFrame_;
+	cv::Mat rgbIrLastFrame_;
+	float depthFocal_;
+	USemaphore dataReady_;
+};
+#endif
+
+//
+// CameraFreenect
+//
+bool CameraFreenect::available()
+{
+#ifdef RTABMAP_FREENECT
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraFreenect::CameraFreenect(int deviceId, Type type, float imageRate, const Transform & localTransform) :
+		Camera(imageRate, localTransform)
+#ifdef RTABMAP_FREENECT
+        ,
+		deviceId_(deviceId),
+		type_(type),
+		ctx_(0),
+		freenectDevice_(0)
+#endif
+{
+#ifdef RTABMAP_FREENECT
+	if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
+	// claim camera
+	freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_CAMERA));
+#endif
+}
+
+CameraFreenect::~CameraFreenect()
+{
+#ifdef RTABMAP_FREENECT
+	if(freenectDevice_)
+	{
+		freenectDevice_->join(true);
+		delete freenectDevice_;
+		freenectDevice_ = 0;
+	}
+	if(ctx_)
+	{
+		if(freenect_shutdown(ctx_) < 0){} //FN_WARNING("Freenect did not shutdown in a clean fashion");
+	}
+#endif
+}
+
+bool CameraFreenect::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_FREENECT
+	if(freenectDevice_)
+	{
+		freenectDevice_->join(true);
+		delete freenectDevice_;
+		freenectDevice_ = 0;
+	}
+
+	if(ctx_ && freenect_num_devices(ctx_) > 0)
+	{
+		// look for calibration files
+		bool hardwareRegistration = true;
+		stereoModel_ = StereoCameraModel();
+		if(!calibrationFolder.empty())
+		{
+			// we need the serial, HACK: init a temp device to get it
+			FreenectDevice dev(ctx_, deviceId_);
+			if(!dev.init())
+			{
+				UERROR("CameraFreenect: Init failed!");
+			}
+			std::string calibrationName = dev.getSerial();
+			if(!cameraName.empty())
+			{
+				calibrationName = cameraName;
+			}
+			stereoModel_.setName(calibrationName, "depth", "rgb");
+			hardwareRegistration = !stereoModel_.load(calibrationFolder, calibrationName, false);
+
+			if(type_ == kTypeIRDepth)
+			{
+				hardwareRegistration = false;
+			}
+
+
+			if((type_ == kTypeIRDepth && !stereoModel_.left().isValidForRectification()) ||
+			   (type_ == kTypeColorDepth && !stereoModel_.right().isValidForRectification()))
+			{
+				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
+						calibrationName.c_str(), calibrationFolder.c_str());
+			}
+			else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && hardwareRegistration)
+			{
+				UWARN("Missing extrinsic calibration file for camera \"%s\" in \"%s\" folder, default registration is used even if rgb is rectified!",
+						calibrationName.c_str(), calibrationFolder.c_str());
+			}
+			else if(type_ == kTypeColorDepth && stereoModel_.right().isValidForRectification() && !hardwareRegistration)
+			{
+				UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
+					  "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
+			}
+		}
+
+		freenectDevice_ = new FreenectDevice(ctx_, deviceId_, type_==kTypeColorDepth, hardwareRegistration);
+		if(freenectDevice_->init())
+		{
+			freenectDevice_->start();
+			uSleep(3000);
+			return true;
+		}
+		else
+		{
+			UERROR("CameraFreenect: Init failed!");
+		}
+		delete freenectDevice_;
+		freenectDevice_ = 0;
+	}
+	else
+	{
+		UERROR("CameraFreenect: No devices connected!");
+	}
+#else
+	UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
+#endif
+	return false;
+}
+
+bool CameraFreenect::isCalibrated() const
+{
+	return true;
+}
+
+std::string CameraFreenect::getSerial() const
+{
+#ifdef RTABMAP_FREENECT
+	if(freenectDevice_)
+	{
+		return freenectDevice_->getSerial();
+	}
+#endif
+	return "";
+}
+
+SensorData CameraFreenect::captureImage(CameraInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_FREENECT
+	if(ctx_ && freenectDevice_)
+	{
+		if(freenectDevice_->isRunning())
+		{
+			cv::Mat depth,rgb;
+			freenectDevice_->getData(rgb, depth);
+			if(!rgb.empty() && !depth.empty())
+			{
+				UASSERT(freenectDevice_->getDepthFocal() != 0.0f);
+
+				// default calibration
+				CameraModel model(
+						freenectDevice_->getDepthFocal(), //fx
+						freenectDevice_->getDepthFocal(), //fy
+						float(rgb.cols/2) - 0.5f,  //cx
+						float(rgb.rows/2) - 0.5f,  //cy
+						this->getLocalTransform(),
+						0,
+						rgb.size());
+
+				if(type_==kTypeIRDepth)
+				{
+					if(stereoModel_.left().isValidForRectification())
+					{
+						rgb = stereoModel_.left().rectifyImage(rgb);
+						depth = stereoModel_.left().rectifyImage(depth, 0);
+						model = stereoModel_.left();
+					}
+				}
+				else
+				{
+					if(stereoModel_.right().isValidForRectification())
+					{
+						rgb = stereoModel_.right().rectifyImage(rgb);
+						model = stereoModel_.right();
+
+						if(stereoModel_.left().isValidForRectification() && !stereoModel_.stereoTransform().isNull())
+						{
+							depth = stereoModel_.left().rectifyImage(depth, 0);
+							depth = util2d::registerDepth(depth, stereoModel_.left().K(), rgb.size(), stereoModel_.right().K(), stereoModel_.stereoTransform());
+						}
+					}
+				}
+				model.setLocalTransform(this->getLocalTransform());
+
+				data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
+			}
+		}
+		else
+		{
+			UERROR("CameraFreenect: Re-initialization needed!");
+			delete freenectDevice_;
+			freenectDevice_ = 0;
+		}
+	}
+#else
+	UERROR("CameraFreenect: RTAB-Map is not built with Freenect support!");
+#endif
+	return data;
+}
+
+
+} // namespace rtabmap

corelib/src/camera/CameraFreenect2.cpp

@@ -0,0 +1,730 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraFreenect2.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/core/util2d.h>
+
+#ifdef RTABMAP_FREENECT2
+#include <libfreenect2/libfreenect2.hpp>
+#include <libfreenect2/frame_listener_impl.h>
+#include <libfreenect2/registration.h>
+#include <libfreenect2/packet_pipeline.h>
+#include <libfreenect2/config.h>
+#endif
+
+namespace rtabmap
+{
+
+bool CameraFreenect2::available()
+{
+#ifdef RTABMAP_FREENECT2
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraFreenect2::CameraFreenect2(
+	int deviceId, 
+	Type type, 
+	float imageRate, 
+	const Transform & localTransform, 
+	float minDepth,
+	float maxDepth,
+	bool bilateralFiltering,
+	bool edgeAwareFiltering,
+	bool noiseFiltering,
+	const std::string & pipelineName) :
+		Camera(imageRate, localTransform)
+#ifdef RTABMAP_FREENECT2
+        ,
+		deviceId_(deviceId),
+		type_(type),
+		freenect2_(0),
+		dev_(0),
+		listener_(0),
+		reg_(0),
+		minKinect2Depth_(minDepth),
+		maxKinect2Depth_(maxDepth),
+		bilateralFiltering_(bilateralFiltering),
+		edgeAwareFiltering_(edgeAwareFiltering),
+		noiseFiltering_(noiseFiltering),
+		pipelineName_(pipelineName)
+#endif
+{
+#ifdef RTABMAP_FREENECT2
+	UASSERT(minKinect2Depth_ < maxKinect2Depth_ && minKinect2Depth_>0 && maxKinect2Depth_>0 && maxKinect2Depth_<=65.535f);
+	freenect2_ = new libfreenect2::Freenect2();
+	switch(type_)
+	{
+	case kTypeColorIR:
+		listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
+		break;
+	case kTypeIRDepth:
+		listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);
+		break;
+	case kTypeColor2DepthSD:
+	case kTypeDepth2ColorHD:
+	case kTypeDepth2ColorSD:
+	default:
+		listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color  | libfreenect2::Frame::Depth);
+		break;
+	}
+#endif
+}
+
+CameraFreenect2::~CameraFreenect2()
+{
+#ifdef RTABMAP_FREENECT2
+	UDEBUG("");
+	if(dev_)
+	{
+		dev_->stop();
+		dev_->close();
+		//deleted in freenect2_ destructor (Freeenect2Impl::clearDevices())
+	}
+	delete listener_;
+	delete reg_;
+	delete freenect2_;
+	UDEBUG("");
+#endif
+}
+
+#ifdef RTABMAP_FREENECT2
+libfreenect2::PacketPipeline *createPacketPipelineByName(const std::string & name)
+{
+	std::string availablePipelines;
+#if defined(LIBFREENECT2_WITH_OPENGL_SUPPORT)
+	availablePipelines += "gl ";
+	if (name == "gl")
+	{
+		UINFO("Using 'gl' pipeline.");
+		return new libfreenect2::OpenGLPacketPipeline();
+	}
+#endif
+#if defined(LIBFREENECT2_WITH_CUDA_SUPPORT)
+	availablePipelines += "cuda cudakde ";
+	if (name == "cuda")
+	{
+		UINFO("Using 'cuda' pipeline.");
+		return new libfreenect2::CudaPacketPipeline();
+	}
+	if (name == "cudakde")
+	{
+		UINFO("Using 'cudakde' pipeline.");
+		return new libfreenect2::CudaKdePacketPipeline();
+	}
+#endif
+#if defined(LIBFREENECT2_WITH_OPENCL_SUPPORT)
+	availablePipelines += "cl clkde ";
+	if (name == "cl")
+	{
+		UINFO("Using 'cl' pipeline.");
+		return new libfreenect2::OpenCLPacketPipeline();
+	}
+	if (name == "clkde")
+	{
+		UINFO("Using 'clkde' pipeline.");
+		return new libfreenect2::OpenCLKdePacketPipeline();
+	}
+#endif
+	availablePipelines += "cpu";
+	if (name == "cpu")
+	{
+		UINFO("Using 'cpu' pipeline.");
+		return new libfreenect2::CpuPacketPipeline();
+	}
+
+	if (!name.empty())
+	{
+		UERROR("'%s' pipeline is not available. Available pipelines are: \"%s\". Default one is used instead (first one in the list).", 
+			name.c_str(), availablePipelines.c_str());
+	}
+
+	// create default pipeline
+#if defined(LIBFREENECT2_WITH_OPENGL_SUPPORT)
+	UINFO("Using 'gl' pipeline.");
+	return new libfreenect2::OpenGLPacketPipeline();
+#elif defined(LIBFREENECT2_WITH_CUDA_SUPPORT)
+	UINFO("Using 'cuda' pipeline.");
+	return new libfreenect2::CudaPacketPipeline();
+#elif defined(LIBFREENECT2_WITH_OPENCL_SUPPORT)
+	UINFO("Using 'cl' pipeline.");
+	return new libfreenect2::OpenCLPacketPipeline();
+#else
+	UINFO("Using 'cpu' pipeline.");
+	return new libfreenect2::CpuPacketPipeline();
+#endif
+}
+#endif
+
+bool CameraFreenect2::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_FREENECT2
+	if(dev_)
+	{
+		dev_->stop();
+		dev_->close();
+		dev_ = 0; //deleted in freenect2_ destructor (Freeenect2Impl::clearDevices())
+	}
+
+	if(reg_)
+	{
+		delete reg_;
+		reg_ = 0;
+	}
+
+	libfreenect2::PacketPipeline * pipeline = createPacketPipelineByName(pipelineName_);
+
+	if(deviceId_ <= 0)
+	{
+		UDEBUG("Opening default device...");
+		dev_ = freenect2_->openDefaultDevice(pipeline);
+		pipeline = 0;// pipeline deleted in dev_ (Freenect2DeviceImpl::~Freenect2DeviceImpl())
+	}
+	else
+	{
+		UDEBUG("Opening device ID=%d...", deviceId_);
+		dev_ = freenect2_->openDevice(deviceId_, pipeline);
+		pipeline = 0;// pipeline deleted in dev_ (Freenect2DeviceImpl::~Freenect2DeviceImpl())
+	}
+
+	if(dev_)
+	{
+		//default
+		//MinDepth(0.5f),
+		//MaxDepth(4.5f),
+		//EnableBilateralFilter(true),
+		//EnableEdgeAwareFilter(true)
+		libfreenect2::Freenect2Device::Config config;
+		config.EnableBilateralFilter = bilateralFiltering_;
+		config.EnableEdgeAwareFilter = edgeAwareFiltering_;
+		config.MinDepth = minKinect2Depth_;
+		config.MaxDepth = maxKinect2Depth_;
+		dev_->setConfiguration(config);
+
+		dev_->setColorFrameListener(listener_);
+		dev_->setIrAndDepthFrameListener(listener_);
+
+		dev_->start();
+
+		UINFO("CameraFreenect2: device serial: %s", dev_->getSerialNumber().c_str());
+		UINFO("CameraFreenect2: device firmware: %s", dev_->getFirmwareVersion().c_str());
+
+		//default registration params
+		libfreenect2::Freenect2Device::IrCameraParams depthParams = dev_->getIrCameraParams();
+		libfreenect2::Freenect2Device::ColorCameraParams colorParams = dev_->getColorCameraParams();
+		reg_ = new libfreenect2::Registration(depthParams, colorParams);
+
+		// look for calibration files
+		stereoModel_ = StereoCameraModel();
+		if(!calibrationFolder.empty())
+		{
+			std::string calibrationName = dev_->getSerialNumber();
+			if(!cameraName.empty())
+			{
+				calibrationName = cameraName;
+			}
+			stereoModel_.setName(calibrationName, "depth", "rgb");
+			if(!stereoModel_.load(calibrationFolder, calibrationName, false))
+			{
+				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration "
+					"is used. Note that from version 0.11.10, calibration suffixes for Freenect2 driver have "
+					"changed from \"_left\"->\"_depth\" and \"_right\"->\"_rgb\". You can safely rename "
+					"the calibration files to avoid recalibrating.",
+						calibrationName.c_str(), calibrationFolder.c_str());
+			}
+			else
+			{
+				UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
+					  "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
+
+				if(type_==kTypeColor2DepthSD)
+				{
+					UWARN("Freenect2: When using custom calibration file, type "
+						  "kTypeColor2DepthSD is not supported. kTypeDepth2ColorSD is used instead...");
+					type_ = kTypeDepth2ColorSD;
+				}
+
+				// downscale color image by 2
+				cv::Mat colorP = stereoModel_.right().P();
+				cv::Size colorSize = stereoModel_.right().imageSize();
+				if(type_ == kTypeDepth2ColorSD)
+				{
+					colorP.at<double>(0,0)/=2.0f; //fx
+					colorP.at<double>(1,1)/=2.0f; //fy
+					colorP.at<double>(0,2)/=2.0f; //cx
+					colorP.at<double>(1,2)/=2.0f; //cy
+					colorSize.width/=2;
+					colorSize.height/=2;
+				}
+				cv::Mat depthP = stereoModel_.left().P();
+				cv::Size depthSize = stereoModel_.left().imageSize();
+				float ratioY = float(colorSize.height)/float(depthSize.height);
+				float ratioX = float(colorSize.width)/float(depthSize.width);
+				depthP.at<double>(0,0)*=ratioX; //fx
+				depthP.at<double>(1,1)*=ratioY; //fy
+				depthP.at<double>(0,2)*=ratioX; //cx
+				depthP.at<double>(1,2)*=ratioY; //cy
+				depthSize.width*=ratioX;
+				depthSize.height*=ratioY;
+				const CameraModel & l = stereoModel_.left();
+				const CameraModel & r = stereoModel_.right();
+				stereoModel_ = StereoCameraModel(stereoModel_.name(),
+						depthSize, l.K_raw(), l.D_raw(), l.R(), depthP,
+						colorSize, r.K_raw(), r.D_raw(), r.R(), colorP,
+						stereoModel_.R(), stereoModel_.T(), stereoModel_.E(), stereoModel_.F());
+				stereoModel_.initRectificationMap();
+			}
+		}
+
+		return true;
+	}
+	else
+	{
+		UERROR("CameraFreenect2: no device connected or failure opening the default one! Note that rtabmap should link on libusb of libfreenect2. "
+					"Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
+	}
+#else
+	UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
+#endif
+	return false;
+}
+
+bool CameraFreenect2::isCalibrated() const
+{
+	return true;
+}
+
+std::string CameraFreenect2::getSerial() const
+{
+#ifdef RTABMAP_FREENECT2
+	if(dev_)
+	{
+		return dev_->getSerialNumber();
+	}
+#endif
+	return "";
+}
+
+SensorData CameraFreenect2::captureImage(CameraInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_FREENECT2
+	if(dev_ && listener_)
+	{
+		libfreenect2::FrameMap frames;
+#ifndef LIBFREENECT2_THREADING_STDLIB
+		UDEBUG("Waiting for new frames... If it is stalled here, rtabmap should link on libusb of libfreenect2. "
+				"Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
+		listener_->waitForNewFrame(frames);
+#else
+		if(!listener_->waitForNewFrame(frames, 1000))
+		{
+			UWARN("CameraFreenect2: Failed to get frames! rtabmap should link on libusb of libfreenect2. "
+					"Tip, before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
+		}
+		else
+#endif
+		{
+			double stamp = UTimer::now();
+			libfreenect2::Frame *rgbFrame = 0;
+			libfreenect2::Frame *irFrame = 0;
+			libfreenect2::Frame *depthFrame = 0;
+
+			switch(type_)
+			{
+			case kTypeColorIR: //used for calibration
+				rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
+				irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
+				break;
+			case kTypeIRDepth:
+				irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
+				depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
+				break;
+			case kTypeColor2DepthSD:
+			case kTypeDepth2ColorSD:
+			case kTypeDepth2ColorHD:
+			case kTypeDepth2ColorHD2:
+			default:
+				rgbFrame = uValue(frames, libfreenect2::Frame::Color, (libfreenect2::Frame*)0);
+				depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);
+				break;
+			}
+
+			cv::Mat rgb, depth;
+			float fx=0,fy=0,cx=0,cy=0;
+			if(irFrame && depthFrame)
+			{
+				cv::Mat irMat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data);
+				//convert to gray scaled
+				float maxIr_ = 0x7FFF;
+				float minIr_ = 0x0;
+				const float factor = 255.0f / float((maxIr_ - minIr_));
+				rgb = cv::Mat(irMat.rows, irMat.cols, CV_8UC1);
+				for(int i=0; i<irMat.rows; ++i)
+				{
+					for(int j=0; j<irMat.cols; ++j)
+					{
+						rgb.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(irMat.at<float>(i,j) - minIr_, 0.0f)) * factor, 255.0f);
+					}
+				}
+
+				cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
+
+				cv::flip(rgb, rgb, 1);
+				cv::flip(depth, depth, 1);
+
+				if(stereoModel_.isValidForRectification())
+				{
+					//rectify
+					rgb = stereoModel_.left().rectifyImage(rgb);
+					depth = stereoModel_.left().rectifyDepth(depth);
+					fx = stereoModel_.left().fx();
+					fy = stereoModel_.left().fy();
+					cx = stereoModel_.left().cx();
+					cy = stereoModel_.left().cy();
+				}
+				else
+				{
+					libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
+					fx = params.fx;
+					fy = params.fy;
+					cx = params.cx;
+					cy = params.cy;
+				}
+			}
+			else
+			{
+				//rgb + ir or rgb + depth
+				if(stereoModel_.isValidForRectification())
+				{
+					cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
+					cv::Mat rgbMat; // rtabmap uses 3 channels RGB
+					#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT 
+
+					 cv::cvtColor(rgbMatC4, rgbMat, CV_RGBA2BGR); 
+
+					#else 
+
+					cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR); 
+
+					#endif 
+					cv::flip(rgbMat, rgb, 1);
+
+					//rectify color
+					rgb = stereoModel_.right().rectifyImage(rgb);
+					if(irFrame)
+					{
+						//rectify IR
+						cv::Mat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
+						cv::flip(depth, depth, 1);
+						depth = stereoModel_.left().rectifyImage(depth);
+					}
+					else
+					{
+						//rectify depth
+						cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
+						cv::flip(depth, depth, 1);
+
+						//depth = stereoModel_.left().rectifyImage(depth, 0); // ~0.5/4 ms but is more noisy
+						depth = stereoModel_.left().rectifyDepth(depth);      // ~16/25 ms
+
+						bool registered = true;
+						if(registered)
+						{
+							depth = util2d::registerDepth(
+									depth,
+									stereoModel_.left().P().colRange(0,3).rowRange(0,3), //scaled depth K
+									depth.size(),
+									stereoModel_.right().P().colRange(0,3).rowRange(0,3), //scaled color K
+									stereoModel_.stereoTransform());
+							util2d::fillRegisteredDepthHoles(depth, true, false);
+							fx = stereoModel_.right().fx();
+							fy = stereoModel_.right().fy();
+							cx = stereoModel_.right().cx();
+							cy = stereoModel_.right().cy();
+						}
+						else
+						{
+							fx = stereoModel_.left().fx();
+							fy = stereoModel_.left().fy();
+							cx = stereoModel_.left().cx();
+							cy = stereoModel_.left().cy();
+						}
+					}
+				}
+				else
+				{
+					//use data from libfreenect2
+					if(irFrame)
+					{
+						cv::Mat rgbMatC4((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
+						cv::Mat rgbMat; // rtabmap uses 3 channels RGB
+						#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT 
+
+						 cv::cvtColor(rgbMatC4, rgbMat, CV_RGB2BGR); 
+
+						#else 
+
+						cv::cvtColor(rgbMatC4, rgbMat, CV_BGRA2BGR); 
+
+						#endif 
+						cv::flip(rgbMat, rgb, 1);
+
+						cv::Mat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data).convertTo(depth, CV_16U, 1);
+						cv::flip(depth, depth, 1);
+					}
+					else
+					{
+						//registration of the depth
+						UASSERT(reg_!=0);
+
+						float maxDepth = maxKinect2Depth_*1000.0f;
+						float minDepth =  minKinect2Depth_*1000.0f;
+						if(type_ == kTypeColor2DepthSD || type_ == kTypeDepth2ColorHD)
+						{
+							cv::Mat rgbMatBGRA;
+							libfreenect2::Frame depthUndistorted(512, 424, 4);
+							libfreenect2::Frame rgbRegistered(512, 424, 4);
+
+							// do it before registration
+							if(noiseFiltering_)
+							{
+								cv::Mat depthMat = cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data);
+								for(int dx=0; dx<depthMat.cols; ++dx)
+								{
+									bool onEdgeX = dx==depthMat.cols-1;
+									for(int dy=0; dy<depthMat.rows; ++dy)
+									{
+										bool onEdge = onEdgeX || dy==depthMat.rows-1;
+										float z = 0.0f;
+										float & dz = depthMat.at<float>(dy,dx);
+										if(dz>=minDepth && dz <= maxDepth)
+										{
+											z = dz;
+											if(noiseFiltering_ && !onEdge)
+											{
+												z=0;
+												const float & dz1 = depthMat.at<float>(dy,dx+1);
+												const float & dz2 = depthMat.at<float>(dy+1,dx);
+												const float & dz3 = depthMat.at<float>(dy+1,dx+1);
+												if( dz1>=minDepth && dz1 <= maxDepth &&
+													dz2>=minDepth && dz2 <= maxDepth &&
+													dz3>=minDepth && dz3 <= maxDepth)
+												{
+													float avg = (dz + dz1 + dz2 + dz3) / 4.0f;
+													float thres = 0.01f*avg;
+											
+													if( fabs(dz-avg) < thres &&
+														fabs(dz1-avg) < thres &&
+														fabs(dz2-avg) < thres &&
+														fabs(dz3-avg) < thres)
+													{
+														z = dz;
+													}
+												}
+											}
+										}
+										dz = z; 
+									}
+								}
+							}
+	
+							libfreenect2::Frame bidDepth(1920, 1082, 4); // HD
+							reg_->apply(rgbFrame, depthFrame, &depthUndistorted, &rgbRegistered, true, &bidDepth);
+	
+							cv::Mat depthMat;
+							if(type_ == kTypeColor2DepthSD)
+							{						
+								rgbMatBGRA = cv::Mat((int)rgbRegistered.height, (int)rgbRegistered.width, CV_8UC4, rgbRegistered.data);
+								depthMat = cv::Mat((int)depthUndistorted.height, (int)depthUndistorted.width, CV_32FC1, depthUndistorted.data);
+						
+								//use IR params
+								libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
+								fx = params.fx;
+								fy = params.fy;
+								cx = params.cx;
+								cy = params.cy;
+							}
+							else
+							{
+								rgbMatBGRA = cv::Mat((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
+								depthMat = cv::Mat((int)bidDepth.height, (int)bidDepth.width, CV_32FC1, bidDepth.data);
+								depthMat = depthMat(cv::Range(1, 1081), cv::Range::all());
+								
+								//use color params
+								libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
+								fx = params.fx;
+								fy = params.fy;
+								cx = params.cx;
+								cy = params.cy;
+							}
+
+							//filter max depth and flip
+							depth = cv::Mat(depthMat.size(), CV_16UC1);
+							for(int dx=0; dx<depthMat.cols; ++dx)
+							{
+								for(int dy=0; dy<depthMat.rows; ++dy)
+								{
+									unsigned short z = 0;
+									const float & dz = depthMat.at<float>(dy,dx);
+									if(dz>=minDepth && dz <= maxDepth)
+									{
+										z = (unsigned short)dz;
+									}
+									depth.at<unsigned short>(dy,(depthMat.cols-1)-dx) = z;  //flip
+								}
+							}
+	
+							// rtabmap uses 3 channels RGB
+							#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT 
+
+							 cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR); 
+
+							#else 
+
+							cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR); 
+
+							#endif 
+							cv::flip(rgb, rgb, 1);
+						}
+						else //register depth to color (OLD WAY)
+						{
+							UASSERT(type_ == kTypeDepth2ColorSD || type_ == kTypeDepth2ColorHD2);
+							cv::Mat rgbMatBGRA = cv::Mat((int)rgbFrame->height, (int)rgbFrame->width, CV_8UC4, rgbFrame->data);
+							if(type_ == kTypeDepth2ColorSD)
+							{
+								cv::Mat tmp;
+								cv::resize(rgbMatBGRA, tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
+								rgbMatBGRA = tmp;
+							}
+							// rtabmap uses 3 channels RGB
+							#ifdef LIBFREENECT2_WITH_TEGRAJPEG_SUPPORT 
+
+							 cv::cvtColor(rgbMatBGRA, rgb, CV_RGBA2BGR); 
+
+							#else 
+
+							cv::cvtColor(rgbMatBGRA, rgb, CV_BGRA2BGR); 
+
+							#endif 
+							cv::flip(rgb, rgb, 1);	
+
+							cv::Mat depthFrameMat = cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data);
+							depth = cv::Mat::zeros(rgbMatBGRA.rows, rgbMatBGRA.cols, CV_16U);
+							for(int dx=0; dx<depthFrameMat.cols-1; ++dx)
+							{
+								for(int dy=0; dy<depthFrameMat.rows-1; ++dy)
+								{
+									float dz = depthFrameMat.at<float>(dy,dx);
+									if(dz>=minDepth && dz<=maxDepth)
+									{
+										bool goodDepth = true;
+										if(noiseFiltering_)
+										{
+											goodDepth = false;
+											float dz1 = depthFrameMat.at<float>(dy,dx+1);
+											float dz2 = depthFrameMat.at<float>(dy+1,dx);
+											float dz3 = depthFrameMat.at<float>(dy+1,dx+1);
+											if(dz1>=minDepth && dz1 <= maxDepth &&
+											   dz2>=minDepth && dz2 <= maxDepth &&
+											   dz3>=minDepth && dz3 <= maxDepth)
+											{
+												float avg = (dz + dz1 + dz2 + dz3) / 4.0f;
+												float thres = 0.01 * avg;
+												if( fabs(dz-avg) < thres &&
+													fabs(dz1-avg) < thres &&
+													fabs(dz2-avg) < thres &&
+													fabs(dz3-avg) < thres)
+												{
+													goodDepth = true;
+												}
+											}
+										}
+										if(goodDepth)
+										{
+											float cx=-1,cy=-1;
+											reg_->apply(dx, dy, dz, cx, cy);
+											if(type_ == kTypeDepth2ColorSD)
+											{
+												cx/=2.0f;
+												cy/=2.0f;
+											}
+											int rcx = cvRound(cx);
+											int rcy = cvRound(cy);
+											if(uIsInBounds(rcx, 0, depth.cols) && uIsInBounds(rcy, 0, depth.rows))
+											{
+												unsigned short & zReg = depth.at<unsigned short>(rcy, rcx);
+												if(zReg == 0 || zReg > (unsigned short)dz)
+												{
+													zReg = (unsigned short)dz;
+												}
+											}
+										}
+									}
+								}
+							}
+							util2d::fillRegisteredDepthHoles(depth, true, true, type_==kTypeDepth2ColorHD2);
+							util2d::fillRegisteredDepthHoles(depth, type_==kTypeDepth2ColorSD, type_==kTypeDepth2ColorHD2);//second pass
+							cv::flip(depth, depth, 1);
+							libfreenect2::Freenect2Device::ColorCameraParams params = dev_->getColorCameraParams();
+							fx = params.fx*(type_==kTypeDepth2ColorSD?0.5:1.0f);
+							fy = params.fy*(type_==kTypeDepth2ColorSD?0.5:1.0f);
+							cx = params.cx*(type_==kTypeDepth2ColorSD?0.5:1.0f);
+							cy = params.cy*(type_==kTypeDepth2ColorSD?0.5:1.0f);
+						}
+					}
+				}
+			}
+
+			CameraModel model;
+			if(fx && fy)
+			{
+				model=CameraModel(
+						fx, //fx
+						fy, //fy
+						cx,  //cx
+						cy, // cy
+						this->getLocalTransform(),
+						0,
+						rgb.size());
+			}
+			data = SensorData(rgb, depth, model, this->getNextSeqID(), stamp);
+
+			listener_->release(frames);
+		}
+	}
+#else
+	UERROR("CameraFreenect2: RTAB-Map is not built with Freenect2 support!");
+#endif
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/camera/CameraImages.cpp

@@ -25,39 +25,19 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "rtabmap/core/CameraRGB.h"
-#include "rtabmap/core/Graph.h"
-
-#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/core/camera/CameraImages.h>
 #include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/utilite/UStl.h>
-#include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UDirectory.h>
-#include <rtabmap/utilite/UTimer.h>
-
-#include <opencv2/imgproc/imgproc.hpp>
-#include <opencv2/imgproc/types_c.h>
-#if CV_MAJOR_VERSION >= 3
-#include <opencv2/videoio/videoio_c.h>
-#endif
-
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UThreadC.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
-#include <rtabmap/core/util3d_surface.h>
-
-#include <pcl/common/io.h>
-
-#include <iostream>
+#include <rtabmap/core/Graph.h>
 #include <fstream>
-#include <cmath>
 
 namespace rtabmap
 {
 
-/////////////////////////
-// CameraImages
-/////////////////////////
 CameraImages::CameraImages() :
 		_startAt(0),
 		_refreshDir(false),
@@ -776,151 +756,4 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 	return data;
 }
 
-
-
-/////////////////////////
-// CameraVideo
-/////////////////////////
-CameraVideo::CameraVideo(
-		int usbDevice,
-		bool rectifyImages,
-		float imageRate,
-		const Transform & localTransform) :
-	Camera(imageRate, localTransform),
-	_rectifyImages(rectifyImages),
-	_src(kUsbDevice),
-	_usbDevice(usbDevice)
-{
-
-}
-
-CameraVideo::CameraVideo(
-		const std::string & filePath,
-		bool rectifyImages,
-		float imageRate,
-		const Transform & localTransform) :
-	Camera(imageRate, localTransform),
-	_filePath(filePath),
-	_rectifyImages(rectifyImages),
-	_src(kVideoFile),
-	_usbDevice(0)
-{
-}
-
-CameraVideo::~CameraVideo()
-{
-	_capture.release();
-}
-
-bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
-{
-	_guid = cameraName;
-	if(_capture.isOpened())
-	{
-		_capture.release();
-	}
-
-	if(_src == kUsbDevice)
-	{
-		ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d", _usbDevice);
-		_capture.open(_usbDevice);
-	}
-	else if(_src == kVideoFile)
-	{
-		ULOGGER_DEBUG("Camera: filename=\"%s\"", _filePath.c_str());
-		_capture.open(_filePath.c_str());
-	}
-	else
-	{
-		ULOGGER_ERROR("Camera: Unknown source...");
-	}
-	if(!_capture.isOpened())
-	{
-		ULOGGER_ERROR("Camera: Failed to create a capture object!");
-		_capture.release();
-		return false;
-	}
-	else
-	{
-		if (_guid.empty())
-		{
-			unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
-			if (guid != 0 && guid != 0xffffffff)
-			{
-				_guid = uFormat("%08x", guid);
-			}
-		}
-
-		// look for calibration files
-		if(!calibrationFolder.empty() && !_guid.empty())
-		{
-			if(!_model.load(calibrationFolder, _guid))
-			{
-				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-						_guid.c_str(), calibrationFolder.c_str());
-			}
-			else
-			{
-				UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
-						_model.fx(),
-						_model.fy(),
-						_model.cx(),
-						_model.cy());
-			}
-		}
-		_model.setLocalTransform(this->getLocalTransform());
-		if(_rectifyImages && !_model.isValidForRectification())
-		{
-			UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");
-			return false;
-		}
-	}
-	return true;
-}
-
-bool CameraVideo::isCalibrated() const
-{
-	return _model.isValidForProjection();
-}
-
-std::string CameraVideo::getSerial() const
-{
-	return _guid;
-}
-
-SensorData CameraVideo::captureImage(CameraInfo * info)
-{
-	cv::Mat img;
-	if(_capture.isOpened())
-	{
-		if(_capture.read(img))
-		{
-			if(_model.imageHeight() == 0 || _model.imageWidth() == 0)
-			{
-				_model.setImageSize(img.size());
-			}
-
-			if(_model.isValidForRectification() && _rectifyImages)
-			{
-				img = _model.rectifyImage(img);
-			}
-			else
-			{
-				// clone required
-				img = img.clone();
-			}
-		}
-		else if(_usbDevice)
-		{
-			UERROR("Camera has been disconnected!");
-		}
-	}
-	else
-	{
-		ULOGGER_WARN("The camera must be initialized before requesting an image.");
-	}
-
-	return SensorData(img, _model, this->getNextSeqID(), UTimer::now());
-}
-
 } // namespace rtabmap

corelib/src/camera/CameraK4W2.cpp

@@ -0,0 +1,579 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraK4W2.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UThreadC.h>
+#include <rtabmap/core/util2d.h>
+
+#ifdef RTABMAP_K4W2
+#include <Kinect.h>
+#endif
+
+namespace rtabmap
+{
+
+#ifdef RTABMAP_K4W2
+// Safe release for interfaces
+template<class Interface>
+inline void SafeRelease(Interface *& pInterfaceToRelease)
+{
+	if (pInterfaceToRelease != NULL)
+	{
+		pInterfaceToRelease->Release();
+		pInterfaceToRelease = NULL;
+	}
+}
+#endif
+
+bool CameraK4W2::available()
+{
+#ifdef RTABMAP_K4W2
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraK4W2::CameraK4W2(
+	int deviceId,
+	Type type,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform)
+#ifdef RTABMAP_K4W2
+	,
+	type_(type),
+	pKinectSensor_(NULL),
+	pCoordinateMapper_(NULL),
+	pDepthCoordinates_(new DepthSpacePoint[cColorWidth * cColorHeight]),
+	pColorCoordinates_(new ColorSpacePoint[cDepthWidth * cDepthHeight]),
+	pMultiSourceFrameReader_(NULL),
+	pColorRGBX_(new RGBQUAD[cColorWidth * cColorHeight]),
+	hMSEvent(NULL)
+#endif
+{
+}
+
+CameraK4W2::~CameraK4W2()
+{
+#ifdef RTABMAP_K4W2
+	if (pDepthCoordinates_)
+	{
+		delete[] pDepthCoordinates_;
+		pDepthCoordinates_ = NULL;
+	}
+
+	if (pColorCoordinates_)
+	{
+		delete[] pColorCoordinates_;
+		pColorCoordinates_ = NULL;
+	}
+
+	if (pColorRGBX_)
+	{
+		delete[] pColorRGBX_;
+		pColorRGBX_ = NULL;
+	}
+
+	close();
+#endif
+}
+
+void CameraK4W2::close()
+{
+#ifdef RTABMAP_K4W2
+	if (pMultiSourceFrameReader_)
+	{
+		pMultiSourceFrameReader_->UnsubscribeMultiSourceFrameArrived(hMSEvent);
+		CloseHandle((HANDLE)hMSEvent);
+		hMSEvent = NULL;
+	}
+
+	// done with frame reader
+	SafeRelease(pMultiSourceFrameReader_);
+
+	// done with coordinate mapper
+	SafeRelease(pCoordinateMapper_);
+
+	// close the Kinect Sensor
+	if (pKinectSensor_)
+	{
+		pKinectSensor_->Close();
+	}
+
+	SafeRelease(pKinectSensor_);
+
+	colorCameraModel_ = CameraModel();
+#endif
+}
+
+bool CameraK4W2::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_K4W2
+	HRESULT hr;
+
+	close();
+
+	hr = GetDefaultKinectSensor(&pKinectSensor_);
+	if (FAILED(hr))
+	{
+		return false;
+	}
+
+	if (pKinectSensor_)
+	{
+		// Initialize the Kinect and get coordinate mapper and the frame reader
+
+		hr = pKinectSensor_->Open();
+
+		if (SUCCEEDED(hr))
+		{
+			hr = pKinectSensor_->get_CoordinateMapper(&pCoordinateMapper_);
+
+			if (SUCCEEDED(hr))
+			{
+				hr = pKinectSensor_->OpenMultiSourceFrameReader(
+					FrameSourceTypes::FrameSourceTypes_Depth | FrameSourceTypes::FrameSourceTypes_Color,
+					&pMultiSourceFrameReader_);
+
+				if (SUCCEEDED(hr))
+				{
+					hr = pMultiSourceFrameReader_->SubscribeMultiSourceFrameArrived(&hMSEvent);
+				}
+			}
+		}
+	}
+
+	if (!pKinectSensor_ || FAILED(hr))
+	{
+		UERROR("No ready Kinect found!");
+		close();
+		return false;
+	}
+
+	// to query camera parameters, we should wait a little
+	uSleep(3000);
+
+	// initialize color calibration if not set yet
+	CameraIntrinsics intrinsics;
+	hr = pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
+	if (SUCCEEDED(hr) && intrinsics.FocalLengthX > 0.0f)
+	{
+		// guess color intrinsics by comparing two reprojections
+		CameraModel depthModel(
+			intrinsics.FocalLengthX,
+			intrinsics.FocalLengthY,
+			intrinsics.PrincipalPointX,
+			intrinsics.PrincipalPointY);
+
+		cv::Mat fakeDepth = cv::Mat::ones(cDepthHeight, cDepthWidth, CV_16UC1) * 1000;
+		hr = pCoordinateMapper_->MapDepthFrameToColorSpace(cDepthWidth * cDepthHeight, (UINT16*)fakeDepth.data, cDepthWidth * cDepthHeight, pColorCoordinates_);
+		if (SUCCEEDED(hr))
+		{
+			int firstIndex = -1;
+			int lastIndex = -1;
+			for (int depthIndex = 0; depthIndex < (cDepthWidth*cDepthHeight); ++depthIndex)
+			{
+				ColorSpacePoint p = pColorCoordinates_[depthIndex];
+				// Values that are negative infinity means it is an invalid color to depth mapping so we
+				// skip processing for this pixel
+				if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
+				{
+					if (firstIndex == -1)
+					{
+						firstIndex = depthIndex;
+					}
+					lastIndex = depthIndex;
+				}
+			}
+
+			UASSERT(firstIndex >= 0 && lastIndex >= 0);
+			float fx, fy, cx, cy;
+			float x1, y1, z1, x2, y2, z2;
+			depthModel.project(firstIndex - (firstIndex / cDepthWidth)*cDepthWidth, firstIndex / cDepthWidth, 1.0f, x1, y1, z1);
+			depthModel.project(lastIndex - (lastIndex / cDepthWidth)*cDepthWidth, lastIndex / cDepthWidth, 1.0f, x2, y2, z2);
+			ColorSpacePoint uv1 = pColorCoordinates_[firstIndex];
+			ColorSpacePoint uv2 = pColorCoordinates_[lastIndex];
+			fx = ((uv1.X - uv2.X)*z1*z2) / (x1*z2 - x2*z1);
+			cx = uv1.X - (x1 / z1) * fx;
+			fy = ((uv1.Y - uv2.Y)*z1*z2) / (y1*z2 - y2*z1);
+			cy = uv1.Y - (y1 / z1) * fy;
+
+			colorCameraModel_ = CameraModel(
+				fx,
+				fy,
+				float(cColorWidth) - cx,
+				cy,
+				this->getLocalTransform(),
+				0,
+				cv::Size(cColorWidth, cColorHeight));
+		}
+	}
+
+	if (!colorCameraModel_.isValidForProjection())
+	{
+		UERROR("Failed to get camera parameters! Is the camera connected? Try restarting the camera again or use kTypeColor2DepthSD.");
+		close();
+		return false;
+	}
+
+	std::string serial = getSerial();
+	if (!serial.empty())
+	{
+		UINFO("Running kinect device \"%s\"", serial.c_str());
+	}
+
+	return true;
+#else
+	UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
+	return false;
+#endif
+}
+
+bool CameraK4W2::isCalibrated() const
+{
+	return true;
+}
+
+std::string CameraK4W2::getSerial() const
+{
+#ifdef RTABMAP_K4W2
+	if (pKinectSensor_)
+	{
+		wchar_t uid[255] = { 0 };
+		// It seems to fail every time!?
+		HRESULT hr = pKinectSensor_->get_UniqueKinectId(255, uid);
+		if (SUCCEEDED(hr))
+		{
+			std::wstring ws(uid);
+			return std::string(ws.begin(), ws.end());
+		}
+	}
+#endif
+	return "";
+}
+
+SensorData CameraK4W2::captureImage(CameraInfo * info)
+{
+	SensorData data;
+
+#ifdef RTABMAP_K4W2
+
+	if (!pMultiSourceFrameReader_)
+	{
+		return data;
+	}
+
+	HRESULT hr;
+
+	//now check for frame events
+	HANDLE handles[] = { reinterpret_cast<HANDLE>(hMSEvent) };
+
+	double t = UTimer::now();
+	while((UTimer::now()-t < 5.0) && WaitForMultipleObjects(_countof(handles), handles, false, 5000) == WAIT_OBJECT_0)
+	{
+		IMultiSourceFrameArrivedEventArgs* pArgs = NULL;
+
+		hr = pMultiSourceFrameReader_->GetMultiSourceFrameArrivedEventData(hMSEvent, &pArgs);
+		if (SUCCEEDED(hr))
+		{
+			IMultiSourceFrameReference * pFrameRef = NULL;
+			hr = pArgs->get_FrameReference(&pFrameRef);
+			if (SUCCEEDED(hr))
+			{
+				IMultiSourceFrame* pMultiSourceFrame = NULL;
+				IDepthFrame* pDepthFrame = NULL;
+				IColorFrame* pColorFrame = NULL;
+
+				hr = pFrameRef->AcquireFrame(&pMultiSourceFrame);
+				if (FAILED(hr))
+				{
+					UERROR("Failed getting latest frame.");
+				}
+
+				IDepthFrameReference* pDepthFrameReference = NULL;
+				hr = pMultiSourceFrame->get_DepthFrameReference(&pDepthFrameReference);
+				if (SUCCEEDED(hr))
+				{
+					hr = pDepthFrameReference->AcquireFrame(&pDepthFrame);
+				}
+				SafeRelease(pDepthFrameReference);
+
+				IColorFrameReference* pColorFrameReference = NULL;
+				hr = pMultiSourceFrame->get_ColorFrameReference(&pColorFrameReference);
+				if (SUCCEEDED(hr))
+				{
+					hr = pColorFrameReference->AcquireFrame(&pColorFrame);
+				}
+				SafeRelease(pColorFrameReference);
+
+				if (pDepthFrame && pColorFrame)
+				{
+					IFrameDescription* pDepthFrameDescription = NULL;
+					int nDepthWidth = 0;
+					int nDepthHeight = 0;
+					UINT nDepthBufferSize = 0;
+					UINT16 *pDepthBuffer = NULL;
+
+					IFrameDescription* pColorFrameDescription = NULL;
+					int nColorWidth = 0;
+					int nColorHeight = 0;
+					ColorImageFormat imageFormat = ColorImageFormat_None;
+					UINT nColorBufferSize = 0;
+					RGBQUAD *pColorBuffer = NULL;
+
+					// get depth frame data
+					if (SUCCEEDED(hr))
+						hr = pDepthFrame->get_FrameDescription(&pDepthFrameDescription);
+					if (SUCCEEDED(hr))
+						hr = pDepthFrameDescription->get_Width(&nDepthWidth);
+					if (SUCCEEDED(hr))
+						hr = pDepthFrameDescription->get_Height(&nDepthHeight);
+					if (SUCCEEDED(hr))
+						hr = pDepthFrame->AccessUnderlyingBuffer(&nDepthBufferSize, &pDepthBuffer);
+
+					// get color frame data
+					if (SUCCEEDED(hr))
+						hr = pColorFrame->get_FrameDescription(&pColorFrameDescription);
+					if (SUCCEEDED(hr))
+						hr = pColorFrameDescription->get_Width(&nColorWidth);
+					if (SUCCEEDED(hr))
+						hr = pColorFrameDescription->get_Height(&nColorHeight);
+					if (SUCCEEDED(hr))
+						hr = pColorFrame->get_RawColorImageFormat(&imageFormat);
+					if (SUCCEEDED(hr))
+					{
+						if (imageFormat == ColorImageFormat_Bgra)
+						{
+							hr = pColorFrame->AccessRawUnderlyingBuffer(&nColorBufferSize, reinterpret_cast<BYTE**>(&pColorBuffer));
+						}
+						else if (pColorRGBX_)
+						{
+							pColorBuffer = pColorRGBX_;
+							nColorBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD);
+							hr = pColorFrame->CopyConvertedFrameDataToArray(nColorBufferSize, reinterpret_cast<BYTE*>(pColorBuffer), ColorImageFormat_Bgra);
+						}
+						else
+						{
+							hr = E_FAIL;
+						}
+					}
+
+					if(SUCCEEDED(hr))
+					{
+						//ProcessFrame(nDepthTime, pDepthBuffer, nDepthWidth, nDepthHeight,
+						//	pColorBuffer, nColorWidth, nColorHeight,
+						//	pBodyIndexBuffer, nBodyIndexWidth, nBodyIndexHeight);
+
+						// Make sure we've received valid data
+						if (pCoordinateMapper_ &&
+							pDepthBuffer && (nDepthWidth == cDepthWidth) && (nDepthHeight == cDepthHeight) &&
+							pColorBuffer && (nColorWidth == cColorWidth) && (nColorHeight == cColorHeight))
+						{
+							if (type_ == kTypeColor2DepthSD)
+							{
+								HRESULT hr = pCoordinateMapper_->MapColorFrameToDepthSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nColorWidth * nColorHeight, pDepthCoordinates_);
+								if (SUCCEEDED(hr))
+								{
+									cv::Mat depth = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_16UC1);
+									cv::Mat imageColorRegistered = cv::Mat::zeros(nDepthHeight, nDepthWidth, CV_8UC3);
+									// loop over output pixels
+									for (int colorIndex = 0; colorIndex < (nColorWidth*nColorHeight); ++colorIndex)
+									{
+										DepthSpacePoint p = pDepthCoordinates_[colorIndex];
+										// Values that are negative infinity means it is an invalid color to depth mapping so we
+										// skip processing for this pixel
+										if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
+										{
+											// To avoid black lines caused by rounding pixel values, we should set 4 pixels
+											// At the same do mirror
+											int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+											pixel_x_l = nDepthWidth - static_cast<int>(p.X);
+											pixel_y_l = static_cast<int>(p.Y);
+											pixel_x_h = pixel_x_l - 1;
+											pixel_y_h = pixel_y_l + 1;
+
+											const RGBQUAD* pSrc = pColorBuffer + colorIndex;
+											if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
+											{
+												unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_l);
+												ptr[0] = pSrc->rgbBlue;
+												ptr[1] = pSrc->rgbGreen;
+												ptr[2] = pSrc->rgbRed;
+												depth.at<unsigned short>(pixel_y_l, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_l*nDepthWidth);
+											}
+											if ((pixel_x_l >= 0 && pixel_x_l < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
+											{
+												unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_l);
+												ptr[0] = pSrc->rgbBlue;
+												ptr[1] = pSrc->rgbGreen;
+												ptr[2] = pSrc->rgbRed;
+												depth.at<unsigned short>(pixel_y_h, pixel_x_l) = *(pDepthBuffer + nDepthWidth - pixel_x_l + pixel_y_h*nDepthWidth);
+											}
+											if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_l >= 0 && pixel_y_l < nDepthHeight))
+											{
+												unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_l, pixel_x_h);
+												ptr[0] = pSrc->rgbBlue;
+												ptr[1] = pSrc->rgbGreen;
+												ptr[2] = pSrc->rgbRed;
+												depth.at<unsigned short>(pixel_y_l, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_l*nDepthWidth);
+											}
+											if ((pixel_x_h >= 0 && pixel_x_h < nDepthWidth) && (pixel_y_h >= 0 && pixel_y_h < nDepthHeight))
+											{
+												unsigned char *  ptr = imageColorRegistered.ptr<unsigned char>(pixel_y_h, pixel_x_h);
+												ptr[0] = pSrc->rgbBlue;
+												ptr[1] = pSrc->rgbGreen;
+												ptr[2] = pSrc->rgbRed;
+												depth.at<unsigned short>(pixel_y_h, pixel_x_h) = *(pDepthBuffer + nDepthWidth - pixel_x_h + pixel_y_h*nDepthWidth);
+											}
+										}
+									}
+
+									CameraIntrinsics intrinsics;
+									pCoordinateMapper_->GetDepthCameraIntrinsics(&intrinsics);
+									CameraModel model(
+										intrinsics.FocalLengthX,
+										intrinsics.FocalLengthY,
+										intrinsics.PrincipalPointX,
+										intrinsics.PrincipalPointY,
+										this->getLocalTransform(),
+										0,
+										depth.size());
+									data = SensorData(imageColorRegistered, depth, model, this->getNextSeqID(), UTimer::now());
+								}
+								else
+								{
+									UERROR("Failed color to depth registration!");
+								}
+							}
+							else //depthToColor
+							{
+								HRESULT hr = pCoordinateMapper_->MapDepthFrameToColorSpace(nDepthWidth * nDepthHeight, (UINT16*)pDepthBuffer, nDepthWidth * nDepthHeight, pColorCoordinates_);
+								if (SUCCEEDED(hr))
+								{
+									cv::Mat depthSource(nDepthHeight, nDepthWidth, CV_16UC1, pDepthBuffer);
+									cv::Mat depthRegistered = cv::Mat::zeros(
+										type_ == kTypeDepth2ColorSD ? nColorHeight/2 : nColorHeight, 
+										type_ == kTypeDepth2ColorSD ? nColorWidth/2 : nColorWidth,
+										CV_16UC1);
+									cv::Mat imageColor;
+									if(type_ == kTypeDepth2ColorSD)
+									{
+										cv::Mat tmp;
+										cv::resize(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), tmp, cv::Size(), 0.5, 0.5, cv::INTER_AREA);
+										cv::cvtColor(tmp, imageColor, CV_BGRA2BGR);
+									}
+									else
+									{
+										cv::cvtColor(cv::Mat(nColorHeight, nColorWidth, CV_8UC4, pColorBuffer), imageColor, CV_BGRA2BGR);
+									}
+									// loop over output pixels
+									for (int depthIndex = 0; depthIndex < (nDepthWidth*nDepthHeight); ++depthIndex)
+									{
+										ColorSpacePoint p = pColorCoordinates_[depthIndex];
+										// Values that are negative infinity means it is an invalid color to depth mapping so we
+										// skip processing for this pixel
+										if (p.X != -std::numeric_limits<float>::infinity() && p.Y != -std::numeric_limits<float>::infinity())
+										{
+											if (type_ == kTypeDepth2ColorSD)
+											{
+												p.X /= 2.0f;
+												p.Y /= 2.0f;
+											}
+											const unsigned short & depth_value = depthSource.at<unsigned short>(0, depthIndex);
+											int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+											// get the coordinate on image plane.
+											pixel_x_l = depthRegistered.cols - p.X; // flip depth
+											pixel_y_l = p.Y;
+											pixel_x_h = pixel_x_l - 1;
+											pixel_y_h = pixel_y_l + 1;
+
+											if (pixel_x_l >= 0 && pixel_x_l < depthRegistered.cols &&
+												pixel_y_l>0 && pixel_y_l < depthRegistered.rows && // ignore first line
+												depth_value)
+											{
+												unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_l, pixel_x_l);
+												if (depthPixel == 0 || depthPixel > depth_value)
+												{
+													depthPixel = depth_value;
+												}
+											}
+											if (pixel_x_h >= 0 && pixel_x_h < depthRegistered.cols &&
+												pixel_y_h>0 && pixel_y_h < depthRegistered.rows && // ignore first line
+												depth_value)
+											{
+												unsigned short & depthPixel = depthRegistered.at<unsigned short>(pixel_y_h, pixel_x_h);
+												if (depthPixel == 0 || depthPixel > depth_value)
+												{
+													depthPixel = depth_value;
+												}
+											}
+										}
+									}
+
+									CameraModel model = colorCameraModel_;
+									if (type_ == kTypeDepth2ColorSD)
+									{
+										model = model.scaled(0.5);
+									}
+									util2d::fillRegisteredDepthHoles(depthRegistered, true, true, type_ == kTypeDepth2ColorHD);
+									depthRegistered = rtabmap::util2d::fillDepthHoles(depthRegistered, 1);
+									cv::flip(imageColor, imageColor, 1);
+									data = SensorData(imageColor, depthRegistered, model, this->getNextSeqID(), UTimer::now());
+								}
+								else
+								{
+									UERROR("Failed depth to color registration!");
+								}
+							}
+						}
+					}
+
+					SafeRelease(pDepthFrameDescription);
+					SafeRelease(pColorFrameDescription);
+				}
+
+				pFrameRef->Release();
+
+				SafeRelease(pDepthFrame);
+				SafeRelease(pColorFrame);
+				SafeRelease(pMultiSourceFrame);
+			}	
+			pArgs->Release();
+		}
+		if (!data.imageRaw().empty())
+		{
+			break;
+		}
+	}
+#else
+	UERROR("CameraK4W2: RTAB-Map is not built with Kinect for Windows 2 SDK support!");
+#endif
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/camera/CameraOpenNI2.cpp

@@ -0,0 +1,584 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraOpenNI2.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UThreadC.h>
+#include <rtabmap/core/util2d.h>
+
+#ifdef RTABMAP_OPENNI2
+#include <OniVersion.h>
+#include <OpenNI.h>
+#endif
+
+namespace rtabmap
+{
+
+bool CameraOpenNI2::available()
+{
+#ifdef RTABMAP_OPENNI2
+	return true;
+#else
+	return false;
+#endif
+}
+
+bool CameraOpenNI2::exposureGainAvailable()
+{
+#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraOpenNI2::CameraOpenNI2(
+		const std::string & deviceId,
+		Type type,
+		float imageRate,
+		const rtabmap::Transform & localTransform) :
+	Camera(imageRate, localTransform)
+#ifdef RTABMAP_OPENNI2
+    ,
+	_type(type),
+	_device(new openni::Device()),
+	_color(new openni::VideoStream()),
+	_depth(new openni::VideoStream()),
+	_depthFx(0.0f),
+	_depthFy(0.0f),
+	_deviceId(deviceId),
+	_openNI2StampsAndIDsUsed(false),
+	_depthHShift(0),
+	_depthVShift(0)
+#endif
+{
+}
+
+CameraOpenNI2::~CameraOpenNI2()
+{
+#ifdef RTABMAP_OPENNI2
+	_color->stop();
+	_color->destroy();
+	_depth->stop();
+	_depth->destroy();
+	_device->close();
+	openni::OpenNI::shutdown();
+
+	delete _device;
+	delete _color;
+	delete _depth;
+#endif
+}
+
+bool CameraOpenNI2::setAutoWhiteBalance(bool enabled)
+{
+#ifdef RTABMAP_OPENNI2
+	if(_color && _color->getCameraSettings())
+	{
+		return _color->getCameraSettings()->setAutoWhiteBalanceEnabled(enabled) == openni::STATUS_OK;
+	}
+#else
+	UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
+#endif
+	return false;
+}
+
+bool CameraOpenNI2::setAutoExposure(bool enabled)
+{
+#ifdef RTABMAP_OPENNI2
+	if(_color && _color->getCameraSettings())
+	{
+		return _color->getCameraSettings()->setAutoExposureEnabled(enabled) == openni::STATUS_OK;
+	}
+#else
+	UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
+#endif
+	return false;
+}
+
+bool CameraOpenNI2::setExposure(int value)
+{
+#ifdef RTABMAP_OPENNI2
+#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
+	if(_color && _color->getCameraSettings())
+	{
+		return _color->getCameraSettings()->setExposure(value) == openni::STATUS_OK;
+	}
+#else
+	UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
+#endif
+#else
+	UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
+#endif
+	return false;
+}
+
+bool CameraOpenNI2::setGain(int value)
+{
+#ifdef RTABMAP_OPENNI2
+#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
+	if(_color && _color->getCameraSettings())
+	{
+		return _color->getCameraSettings()->setGain(value) == openni::STATUS_OK;
+	}
+#else
+	UERROR("CameraOpenNI2: OpenNI >= 2.2 required to use this method.");
+#endif
+#else
+	UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
+#endif
+	return false;
+}
+
+bool CameraOpenNI2::setMirroring(bool enabled)
+{
+#ifdef RTABMAP_OPENNI2
+	if(_color->isValid() && _depth->isValid())
+	{
+		return _depth->setMirroringEnabled(enabled) == openni::STATUS_OK &&
+				_color->setMirroringEnabled(enabled) == openni::STATUS_OK;
+	}
+#endif
+	return false;
+}
+
+void CameraOpenNI2::setOpenNI2StampsAndIDsUsed(bool used)
+{
+#ifdef RTABMAP_OPENNI2
+	_openNI2StampsAndIDsUsed = used;
+#endif
+}
+
+void CameraOpenNI2::setIRDepthShift(int horizontal, int vertical)
+{
+#ifdef RTABMAP_OPENNI2
+	UASSERT(horizontal >= 0);
+	UASSERT(vertical >= 0);
+	_depthHShift = horizontal;
+	_depthVShift = vertical;
+#endif
+}
+
+bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_OPENNI2
+	openni::OpenNI::initialize();
+
+	openni::Array<openni::DeviceInfo> devices;
+	openni::OpenNI::enumerateDevices(&devices);
+	for(int i=0; i<devices.getSize(); ++i)
+	{
+		UINFO("Device %d: Name=%s URI=%s Vendor=%s",
+				i,
+				devices[i].getName(),
+				devices[i].getUri(),
+				devices[i].getVendor());
+	}
+	if(_deviceId.empty() && devices.getSize() == 0)
+	{
+		UERROR("CameraOpenNI2: No device detected!");
+		return false;
+	}
+
+	openni::Status error = _device->open(_deviceId.empty()?openni::ANY_DEVICE:_deviceId.c_str());
+	if(error != openni::STATUS_OK)
+	{
+		if(!_deviceId.empty())
+		{
+			UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d).", _deviceId.c_str(), error);
+		}
+		else
+		{
+#ifdef _WIN32
+			UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d).", devices[0].getName(), error);
+#else
+			UERROR("CameraOpenNI2: Cannot open device \"%s\" (error=%d). Verify if \"%s\" is in udev rules: \"/lib/udev/rules.d/40-libopenni2-0.rules\". If not, add it and reboot.", devices[0].getName(), error, devices[0].getUri());
+#endif
+		}
+
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+
+	// look for calibration files
+	_stereoModel = StereoCameraModel();
+	bool hardwareRegistration = true;
+	if(!calibrationFolder.empty())
+	{
+		// we need the serial
+		std::string calibrationName = _device->getDeviceInfo().getName();
+		if(!cameraName.empty())
+		{
+			calibrationName = cameraName;
+		}
+		_stereoModel.setName(calibrationName, "depth", "rgb");
+		hardwareRegistration = !_stereoModel.load(calibrationFolder, calibrationName, false);
+
+		if(_type != kTypeColorDepth)
+		{
+			hardwareRegistration = false;
+		}
+
+
+		if((_type != kTypeColorDepth && !_stereoModel.left().isValidForRectification()) ||
+		   (_type == kTypeColorDepth && !_stereoModel.right().isValidForRectification()))
+		{
+			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
+					calibrationName.c_str(), calibrationFolder.c_str());
+		}
+		else if(_type == kTypeColorDepth && _stereoModel.right().isValidForRectification() && hardwareRegistration)
+		{
+			UWARN("Missing extrinsic calibration file for camera \"%s\" in \"%s\" folder, default registration is used even if rgb is rectified!",
+					calibrationName.c_str(), calibrationFolder.c_str());
+		}
+		else if(_type == kTypeColorDepth && _stereoModel.right().isValidForRectification() && !hardwareRegistration)
+		{
+			UINFO("Custom calibration files for \"%s\" were found in \"%s\" folder. To use "
+				  "factory calibration, remove the corresponding files from that directory.", calibrationName.c_str(), calibrationFolder.c_str());
+		}
+	}
+
+	if(UFile::getExtension(_deviceId).compare("oni")==0)
+	{
+		if(_device->getPlaybackControl() &&
+		   _device->getPlaybackControl()->setRepeatEnabled(false) != openni::STATUS_OK)
+		{
+			UERROR("CameraOpenNI2: Cannot set repeat mode to false.");
+			_device->close();
+			openni::OpenNI::shutdown();
+			return false;
+		}
+	}
+	else if(_type==kTypeColorDepth && hardwareRegistration &&
+			!_device->isImageRegistrationModeSupported(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR))
+	{
+		UERROR("CameraOpenNI2: Device doesn't support depth/color registration.");
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+
+	if(_device->getSensorInfo(openni::SENSOR_DEPTH) == NULL ||
+	  _device->getSensorInfo(_type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) == NULL)
+	{
+		UERROR("CameraOpenNI2: Cannot get sensor info for depth and %s.", _type==kTypeColorDepth?"color":"ir");
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+
+	if(_depth->create(*_device, openni::SENSOR_DEPTH) != openni::STATUS_OK)
+	{
+		UERROR("CameraOpenNI2: Cannot create depth stream.");
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+
+	if(_color->create(*_device, _type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) != openni::STATUS_OK)
+	{
+		UERROR("CameraOpenNI2: Cannot create %s stream.", _type==kTypeColorDepth?"color":"ir");
+		_depth->destroy();
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+
+	if(_type==kTypeColorDepth && hardwareRegistration &&
+	   _device->setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR ) != openni::STATUS_OK)
+	{
+		UERROR("CameraOpenNI2: Failed to set depth/color registration.");
+	}
+
+	if (_device->setDepthColorSyncEnabled(true) != openni::STATUS_OK)
+	{
+		UERROR("CameraOpenNI2: Failed to set depth color sync");
+	}
+
+	_depth->setMirroringEnabled(false);
+	_color->setMirroringEnabled(false);
+
+	const openni::Array<openni::VideoMode>& depthVideoModes = _depth->getSensorInfo().getSupportedVideoModes();
+	for(int i=0; i<depthVideoModes.getSize(); ++i)
+	{
+		UINFO("CameraOpenNI2: Depth video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
+				i,
+				depthVideoModes[i].getFps(),
+				depthVideoModes[i].getPixelFormat(),
+				depthVideoModes[i].getResolutionX(),
+				depthVideoModes[i].getResolutionY());
+	}
+
+	const openni::Array<openni::VideoMode>& colorVideoModes = _color->getSensorInfo().getSupportedVideoModes();
+	for(int i=0; i<colorVideoModes.getSize(); ++i)
+	{
+		UINFO("CameraOpenNI2: %s video mode %d: fps=%d, pixel=%d, w=%d, h=%d",
+				_type==kTypeColorDepth?"color":"ir",
+				i,
+				colorVideoModes[i].getFps(),
+				colorVideoModes[i].getPixelFormat(),
+				colorVideoModes[i].getResolutionX(),
+				colorVideoModes[i].getResolutionY());
+	}
+
+	openni::VideoMode mMode;
+	mMode.setFps(30);
+	mMode.setResolution(640,480);
+	mMode.setPixelFormat(openni::PIXEL_FORMAT_DEPTH_1_MM);
+	_depth->setVideoMode(mMode);
+
+	openni::VideoMode mModeColor;
+	mModeColor.setFps(30);
+	mModeColor.setResolution(640,480);
+	mModeColor.setPixelFormat(openni::PIXEL_FORMAT_RGB888);
+	_color->setVideoMode(mModeColor);
+
+	UINFO("CameraOpenNI2: Using depth video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
+			_depth->getVideoMode().getFps(),
+			_depth->getVideoMode().getPixelFormat(),
+			_depth->getVideoMode().getResolutionX(),
+			_depth->getVideoMode().getResolutionY(),
+			_depth->getHorizontalFieldOfView(),
+			_depth->getVerticalFieldOfView());
+	UINFO("CameraOpenNI2: Using %s video mode: fps=%d, pixel=%d, w=%d, h=%d, H-FOV=%f rad, V-FOV=%f rad",
+			_type==kTypeColorDepth?"color":"ir",
+			_color->getVideoMode().getFps(),
+			_color->getVideoMode().getPixelFormat(),
+			_color->getVideoMode().getResolutionX(),
+			_color->getVideoMode().getResolutionY(),
+			_color->getHorizontalFieldOfView(),
+			_color->getVerticalFieldOfView());
+
+	if(_depth->getVideoMode().getResolutionX() != 640 ||
+		_depth->getVideoMode().getResolutionY() != 480 ||
+		_depth->getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_DEPTH_1_MM)
+	{
+		UERROR("Could not set depth format to 640x480 pixel=%d(mm)!",
+				openni::PIXEL_FORMAT_DEPTH_1_MM);
+		_depth->destroy();
+		_color->destroy();
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+	if(_color->getVideoMode().getResolutionX() != 640 ||
+		_color->getVideoMode().getResolutionY() != 480 ||
+		_color->getVideoMode().getPixelFormat() != openni::PIXEL_FORMAT_RGB888)
+	{
+		UERROR("Could not set %s format to 640x480 pixel=%d!",
+				_type==kTypeColorDepth?"color":"ir",
+				openni::PIXEL_FORMAT_RGB888);
+		_depth->destroy();
+		_color->destroy();
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+
+	if(_color->getCameraSettings())
+	{
+		UINFO("CameraOpenNI2: AutoWhiteBalanceEnabled = %d", _color->getCameraSettings()->getAutoWhiteBalanceEnabled()?1:0);
+		UINFO("CameraOpenNI2: AutoExposureEnabled = %d", _color->getCameraSettings()->getAutoExposureEnabled()?1:0);
+#if ONI_VERSION_MAJOR > 2 || (ONI_VERSION_MAJOR==2 && ONI_VERSION_MINOR >= 2)
+		UINFO("CameraOpenNI2: Exposure = %d", _color->getCameraSettings()->getExposure());
+		UINFO("CameraOpenNI2: GAIN = %d", _color->getCameraSettings()->getGain());
+#endif
+	}
+
+	if(_type==kTypeColorDepth && hardwareRegistration)
+	{
+		_depthFx = float(_color->getVideoMode().getResolutionX()/2) / std::tan(_color->getHorizontalFieldOfView()/2.0f);
+		_depthFy = float(_color->getVideoMode().getResolutionY()/2) / std::tan(_color->getVerticalFieldOfView()/2.0f);
+	}
+	else
+	{
+		_depthFx = float(_depth->getVideoMode().getResolutionX()/2) / std::tan(_depth->getHorizontalFieldOfView()/2.0f);
+		_depthFy = float(_depth->getVideoMode().getResolutionY()/2) / std::tan(_depth->getVerticalFieldOfView()/2.0f);
+	}
+	UINFO("depth fx=%f fy=%f", _depthFx, _depthFy);
+
+	if(_type == kTypeIR)
+	{
+		UWARN("With type IR-only, depth stream will not be started");
+	}
+
+	if((_type != kTypeIR && _depth->start() != openni::STATUS_OK) ||
+		_color->start() != openni::STATUS_OK)
+	{
+		UERROR("CameraOpenNI2: Cannot start depth and/or color streams.");
+		_depth->stop();
+		_color->stop();
+		_depth->destroy();
+		_color->destroy();
+		_device->close();
+		openni::OpenNI::shutdown();
+		return false;
+	}
+
+	uSleep(3000); // just to make sure the sensor is correctly initialized and exposure is set
+
+	return true;
+#else
+	UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
+	return false;
+#endif
+}
+
+bool CameraOpenNI2::isCalibrated() const
+{
+	return true;
+}
+
+std::string CameraOpenNI2::getSerial() const
+{
+#ifdef RTABMAP_OPENNI2
+	if(_device)
+	{
+		return _device->getDeviceInfo().getName();
+	}
+#endif
+	return "";
+}
+
+SensorData CameraOpenNI2::captureImage(CameraInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_OPENNI2
+	int readyStream = -1;
+	if(_device->isValid() &&
+		_depth->isValid() &&
+		_color->isValid() &&
+		_device->getSensorInfo(openni::SENSOR_DEPTH) != NULL &&
+		_device->getSensorInfo(_type==kTypeColorDepth?openni::SENSOR_COLOR:openni::SENSOR_IR) != NULL)
+	{
+		openni::VideoStream* depthStream[] = {_depth};
+		openni::VideoStream* colorStream[] = {_color};
+		if((_type != kTypeIR && openni::OpenNI::waitForAnyStream(depthStream, 1, &readyStream, 5000) != openni::STATUS_OK) ||
+		   openni::OpenNI::waitForAnyStream(colorStream, 1, &readyStream, 5000) != openni::STATUS_OK)
+		{
+			UWARN("No frames received since the last 5 seconds, end of stream is reached!");
+		}
+		else
+		{
+			openni::VideoFrameRef depthFrame, colorFrame;
+			if(_type != kTypeIR)
+			{
+				_depth->readFrame(&depthFrame);
+			}
+			_color->readFrame(&colorFrame);
+			cv::Mat depth, rgb;
+			if((_type == kTypeIR || depthFrame.isValid()) && colorFrame.isValid())
+			{
+				int h,w;
+				if(_type != kTypeIR)
+				{
+					h=depthFrame.getHeight();
+					w=depthFrame.getWidth();
+					depth = cv::Mat(h, w, CV_16U, (void*)depthFrame.getData()).clone();
+				}
+				h=colorFrame.getHeight();
+				w=colorFrame.getWidth();
+				cv::Mat tmp(h, w, CV_8UC3, (void *)colorFrame.getData());
+				if(_type==kTypeColorDepth)
+				{
+					cv::cvtColor(tmp, rgb, CV_RGB2BGR);
+				}
+				else // IR
+				{
+					rgb = tmp.clone();
+				}
+			}
+			UASSERT(_depthFx != 0.0f && _depthFy != 0.0f);
+			if(!rgb.empty() && (_type == kTypeIR || !depth.empty()))
+			{
+				// default calibration
+				CameraModel model(
+						_depthFx, //fx
+						_depthFy, //fy
+						float(rgb.cols/2) - 0.5f,  //cx
+						float(rgb.rows/2) - 0.5f,  //cy
+						this->getLocalTransform(),
+						0,
+						rgb.size());
+
+				if(_type==kTypeColorDepth)
+				{
+					if(_stereoModel.right().isValidForRectification())
+					{
+						rgb = _stereoModel.right().rectifyImage(rgb);
+						model = _stereoModel.right();
+
+						if(_stereoModel.left().isValidForRectification() && !_stereoModel.stereoTransform().isNull())
+						{
+							if (_depthHShift > 0 || _depthVShift > 0)
+							{
+								cv::Mat out = cv::Mat::zeros(depth.size(), depth.type());
+								depth(cv::Rect(_depthHShift, _depthVShift, depth.cols - _depthHShift, depth.rows - _depthVShift)).copyTo(out(cv::Rect(0, 0, depth.cols - _depthHShift, depth.rows - _depthVShift)));
+								depth = out;
+							}
+							depth = _stereoModel.left().rectifyImage(depth, 0);
+							depth = util2d::registerDepth(depth, _stereoModel.left().K(), rgb.size(), _stereoModel.right().K(), _stereoModel.stereoTransform());
+						}
+					}
+				}
+				else // IR
+				{
+					if(_stereoModel.left().isValidForRectification())
+					{
+						rgb = _stereoModel.left().rectifyImage(rgb);
+						if(_type!=kTypeIR)
+						{
+							depth = _stereoModel.left().rectifyImage(depth, 0);
+						}
+						model = _stereoModel.left();
+					}
+				}
+				model.setLocalTransform(this->getLocalTransform());
+
+				if(_openNI2StampsAndIDsUsed)
+				{
+					data = SensorData(rgb, depth, model, depthFrame.getFrameIndex(), double(depthFrame.getTimestamp()) / 1000000.0);
+				}
+				else
+				{
+					data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
+				}
+			}
+		}
+	}
+	else
+	{
+		ULOGGER_WARN("The camera must be initialized before requesting an image.");
+	}
+#else
+	UERROR("CameraOpenNI2: RTAB-Map is not built with OpenNI2 support!");
+#endif
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/camera/CameraOpenNICV.cpp

@@ -0,0 +1,139 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraOpenNICV.h>
+#include <rtabmap/utilite/UTimer.h>
+
+namespace rtabmap
+{
+
+bool CameraOpenNICV::available()
+{
+	return cv::getBuildInformation().find("OpenNI:                      YES") != std::string::npos;
+}
+
+CameraOpenNICV::CameraOpenNICV(bool asus, float imageRate, const rtabmap::Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	_asus(asus),
+	_depthFocal(0.0f)
+{
+
+}
+
+CameraOpenNICV::~CameraOpenNICV()
+{
+	_capture.release();
+}
+
+bool CameraOpenNICV::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	if(_capture.isOpened())
+	{
+		_capture.release();
+	}
+
+	ULOGGER_DEBUG("Camera::init()");
+	_capture.open( _asus?CV_CAP_OPENNI_ASUS:CV_CAP_OPENNI );
+	if(_capture.isOpened())
+	{
+		_capture.set( CV_CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, CV_CAP_OPENNI_VGA_30HZ );
+		_depthFocal = _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH );
+		// Print some avalible device settings.
+		UINFO("Depth generator output mode:");
+		UINFO("FRAME_WIDTH        %f", _capture.get( CV_CAP_PROP_FRAME_WIDTH ));
+		UINFO("FRAME_HEIGHT       %f", _capture.get( CV_CAP_PROP_FRAME_HEIGHT ));
+		UINFO("FRAME_MAX_DEPTH    %f mm", _capture.get( CV_CAP_PROP_OPENNI_FRAME_MAX_DEPTH ));
+		UINFO("BASELINE           %f mm", _capture.get( CV_CAP_PROP_OPENNI_BASELINE ));
+		UINFO("FPS                %f", _capture.get( CV_CAP_PROP_FPS ));
+		UINFO("Focal              %f", _capture.get( CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH ));
+		UINFO("REGISTRATION       %f", _capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ));
+		if(_capture.get( CV_CAP_PROP_OPENNI_REGISTRATION ) == 0.0)
+		{
+			UERROR("Depth registration is not activated on this device!");
+		}
+		if( _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR_PRESENT ) )
+		{
+			UINFO("Image generator output mode:");
+			UINFO("FRAME_WIDTH    %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_WIDTH ));
+			UINFO("FRAME_HEIGHT   %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FRAME_HEIGHT ));
+			UINFO("FPS            %f", _capture.get( CV_CAP_OPENNI_IMAGE_GENERATOR+CV_CAP_PROP_FPS ));
+		}
+		else
+		{
+			UERROR("Camera: Device doesn't contain image generator.");
+			_capture.release();
+			return false;
+		}
+	}
+	else
+	{
+		ULOGGER_ERROR("Camera: Failed to create a capture object!");
+		_capture.release();
+		return false;
+	}
+	return true;
+}
+
+bool CameraOpenNICV::isCalibrated() const
+{
+	return true;
+}
+
+SensorData CameraOpenNICV::captureImage(CameraInfo * info)
+{
+	SensorData data;
+	if(_capture.isOpened())
+	{
+		_capture.grab();
+		cv::Mat depth, rgb;
+		_capture.retrieve(depth, CV_CAP_OPENNI_DEPTH_MAP );
+		_capture.retrieve(rgb, CV_CAP_OPENNI_BGR_IMAGE );
+
+		depth = depth.clone();
+		rgb = rgb.clone();
+
+		UASSERT(_depthFocal>0.0f);
+		if(!rgb.empty() && !depth.empty())
+		{
+			CameraModel model(
+					_depthFocal, //fx
+					_depthFocal, //fy
+					float(rgb.cols/2) - 0.5f,  //cx
+					float(rgb.rows/2) - 0.5f,  //cy
+					this->getLocalTransform(),
+					0,
+					rgb.size());
+			data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
+		}
+	}
+	else
+	{
+		ULOGGER_WARN("The camera must be initialized before requesting an image.");
+	}
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/camera/CameraOpenni.cpp

@@ -0,0 +1,207 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraOpenni.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UThreadC.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#ifdef RTABMAP_OPENNI
+#include <pcl/io/openni_grabber.h>
+#include <pcl/io/oni_grabber.h>
+#include <pcl/io/openni_camera/openni_depth_image.h>
+#include <pcl/io/openni_camera/openni_image.h>
+#endif
+
+namespace rtabmap
+{
+
+CameraOpenni::CameraOpenni(const std::string & deviceId, float imageRate, const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		interface_(0),
+		deviceId_(deviceId),
+		depthConstant_(0.0f)
+{
+}
+
+bool CameraOpenni::available() 
+{
+#ifdef RTABMAP_OPENNI
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraOpenni::~CameraOpenni()
+{
+#ifdef RTABMAP_OPENNI
+	UDEBUG("");
+	if(connection_.connected())
+	{
+		connection_.disconnect();
+	}
+
+	if(interface_)
+	{
+		interface_->stop();
+		uSleep(1000); // make sure it is stopped
+		delete interface_;
+		interface_ = 0;
+	}
+#endif
+}
+#ifdef RTABMAP_OPENNI
+void CameraOpenni::image_cb (
+		const boost::shared_ptr<openni_wrapper::Image>& rgb,
+		const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
+		float constant)
+{
+	UScopeMutex s(dataMutex_);
+
+	bool notify = rgb_.empty();
+
+	cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
+	rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
+	cv::cvtColor(rgbFrame, rgb_, CV_RGB2BGR);
+
+	depth_ = cv::Mat(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
+	depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depth_.data);
+
+	depthConstant_ = constant;
+
+	if(notify)
+	{
+		dataReady_.release();
+	}
+}
+#endif
+
+bool CameraOpenni::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_OPENNI
+	if(interface_)
+	{
+		interface_->stop();
+		uSleep(100); // make sure it is stopped
+		delete interface_;
+		interface_ = 0;
+	}
+
+	try
+	{
+		if(UFile::getExtension(deviceId_).compare("oni") == 0)
+		{
+			interface_ = new pcl::ONIGrabber(deviceId_, false, true);
+		}
+		else
+		{
+			interface_ = new pcl::OpenNIGrabber(deviceId_);
+		}
+
+		boost::function<void (
+				const boost::shared_ptr<openni_wrapper::Image>&,
+				const boost::shared_ptr<openni_wrapper::DepthImage>&,
+				float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
+		connection_ = interface_->registerCallback (f);
+
+		interface_->start ();
+	}
+	catch(const pcl::IOException& ex)
+	{
+		UERROR("OpenNI exception: %s", ex.what());
+		if(interface_)
+		{
+			delete interface_;
+			interface_ = 0;
+		}
+		return false;
+	}
+	return true;
+#else
+	UERROR("PCL not built with OpenNI! Cannot initialize CameraOpenNI");
+	return false;
+#endif
+}
+
+bool CameraOpenni::isCalibrated() const
+{
+#ifdef RTABMAP_OPENNI
+	return true;
+#else
+	return false;
+#endif
+}
+
+std::string CameraOpenni::getSerial() const
+{
+#ifdef RTABMAP_OPENNI
+	if(interface_)
+	{
+		return interface_->getName();
+	}
+#endif
+	return "";
+}
+
+SensorData CameraOpenni::captureImage(CameraInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_OPENNI
+	if(interface_ && interface_->isRunning())
+	{
+		if(!dataReady_.acquire(1, 5000))
+		{
+			UWARN("Not received new frames since 5 seconds, end of stream reached!");
+		}
+		else
+		{
+			UScopeMutex s(dataMutex_);
+			if(depthConstant_ && !rgb_.empty() && !depth_.empty())
+			{
+				CameraModel model(
+						1.0f/depthConstant_, //fx
+						1.0f/depthConstant_, //fy
+						float(rgb_.cols/2) - 0.5f,  //cx
+						float(rgb_.rows/2) - 0.5f,  //cy
+						this->getLocalTransform(),
+						0,
+						rgb_.size());
+				data = SensorData(rgb_, depth_, model, this->getNextSeqID(), UTimer::now());
+			}
+
+			depth_ = cv::Mat();
+			rgb_ = cv::Mat();
+			depthConstant_ = 0.0f;
+		}
+	}
+#else
+	UERROR("CameraOpenNI: RTAB-Map is not built with PCL having OpenNI support!");
+#endif
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/camera/CameraRGBDImages.cpp

@@ -0,0 +1,102 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraRGBDImages.h>
+
+namespace rtabmap
+{
+
+bool CameraRGBDImages::available()
+{
+	return true;
+}
+
+CameraRGBDImages::CameraRGBDImages(
+		const std::string & pathRGBImages,
+		const std::string & pathDepthImages,
+		float depthScaleFactor,
+		float imageRate,
+		const Transform & localTransform) :
+		CameraImages(pathRGBImages, imageRate, localTransform)
+{
+	UASSERT(depthScaleFactor >= 1.0);
+	cameraDepth_.setPath(pathDepthImages);
+	cameraDepth_.setDepth(true, depthScaleFactor);
+}
+
+CameraRGBDImages::~CameraRGBDImages()
+{
+}
+
+bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	bool success = false;
+	if(CameraImages::init(calibrationFolder, cameraName) && cameraDepth_.init())
+	{
+		if(this->imagesCount() == cameraDepth_.imagesCount())
+		{
+			success = true;
+		}
+		else
+		{
+			UERROR("Cameras don't have the same number of images (%d vs %d)",
+					this->imagesCount(), cameraDepth_.imagesCount());
+		}
+	}
+
+	return success;
+}
+
+bool CameraRGBDImages::isCalibrated() const
+{
+	return this->cameraModel().isValidForProjection();
+}
+
+std::string CameraRGBDImages::getSerial() const
+{
+	return this->cameraModel().name();
+}
+
+SensorData CameraRGBDImages::captureImage(CameraInfo * info)
+{
+	SensorData data;
+
+	SensorData rgb, depth;
+	rgb = CameraImages::captureImage(info);
+	if(!rgb.imageRaw().empty())
+	{
+		depth = cameraDepth_.takeImage();
+		if(!depth.depthRaw().empty())
+		{
+			data = SensorData(rgb.imageRaw(), depth.depthRaw(), rgb.cameraModels(), rgb.id(), rgb.stamp());
+			data.setGroundTruth(rgb.groundTruth());
+		}
+	}
+	return data;
+}
+
+
+} // namespace rtabmap

corelib/src/camera/CameraRealSense2.cpp

@@ -0,0 +1,450 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraRealSense2.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UThreadC.h>
+#include <rtabmap/utilite/UConversion.h>
+
+#ifdef RTABMAP_REALSENSE2
+#include <librealsense2/rs.hpp>
+#include <librealsense2/rsutil.h>
+#include <librealsense2/hpp/rs_processing.hpp>
+#include <librealsense2/rs_advanced_mode.hpp>
+#endif
+
+namespace rtabmap
+{
+
+bool CameraRealSense2::available()
+{
+#ifdef RTABMAP_REALSENSE2
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraRealSense2::CameraRealSense2(
+		const std::string & device,
+		float imageRate,
+		const rtabmap::Transform & localTransform) :
+	Camera(imageRate, localTransform)
+#ifdef RTABMAP_REALSENSE2
+    ,
+	ctx_(new rs2::context),
+	dev_(new rs2::device),
+	deviceId_(device),
+	syncer_(new rs2::syncer),
+	depth_scale_meters_(1.0f),
+	depthIntrinsics_(new rs2_intrinsics),
+	rgbIntrinsics_(new rs2_intrinsics),
+	depthToRGBExtrinsics_(new rs2_extrinsics),
+	emitterEnabled_(true),
+	irDepth_(false)
+#endif
+{
+	UDEBUG("");
+}
+
+CameraRealSense2::~CameraRealSense2()
+{
+#ifdef RTABMAP_REALSENSE2
+	delete ctx_;
+	delete dev_;
+	delete syncer_;
+	delete depthIntrinsics_;
+	delete rgbIntrinsics_;
+	delete depthToRGBExtrinsics_;
+#endif
+	UDEBUG("");
+}
+
+#ifdef RTABMAP_REALSENSE2
+void alignFrame(const rs2_intrinsics& from_intrin,
+                                   const rs2_intrinsics& other_intrin,
+                                   rs2::frame from_image,
+                                   uint32_t output_image_bytes_per_pixel,
+                                   const rs2_extrinsics& from_to_other,
+								   cv::Mat & registeredDepth,
+								   float depth_scale_meters)
+{
+    static const auto meter_to_mm = 0.001f;
+    uint8_t* p_out_frame = registeredDepth.data;
+    auto from_vid_frame = from_image.as<rs2::video_frame>();
+    auto from_bytes_per_pixel = from_vid_frame.get_bytes_per_pixel();
+
+    static const auto blank_color = 0x00;
+    UASSERT(registeredDepth.total()*registeredDepth.channels()*registeredDepth.depth() == other_intrin.height * other_intrin.width * output_image_bytes_per_pixel);
+    memset(p_out_frame, blank_color, other_intrin.height * other_intrin.width * output_image_bytes_per_pixel);
+
+    auto p_from_frame = reinterpret_cast<const uint8_t*>(from_image.get_data());
+    auto from_stream_type = from_image.get_profile().stream_type();
+    float depth_units = ((from_stream_type == RS2_STREAM_DEPTH)? depth_scale_meters:1.f);
+    UASSERT(from_stream_type == RS2_STREAM_DEPTH);
+    UASSERT_MSG(depth_units > 0.0f, uFormat("depth_scale_meters=%f", depth_scale_meters).c_str());
+#pragma omp parallel for schedule(dynamic)
+    for (int from_y = 0; from_y < from_intrin.height; ++from_y)
+    {
+        int from_pixel_index = from_y * from_intrin.width;
+        for (int from_x = 0; from_x < from_intrin.width; ++from_x, ++from_pixel_index)
+        {
+            // Skip over depth pixels with the value of zero
+            float depth = (from_stream_type == RS2_STREAM_DEPTH)?(depth_units * ((const uint16_t*)p_from_frame)[from_pixel_index]): 1.f;
+            if (depth)
+            {
+                // Map the top-left corner of the depth pixel onto the other image
+                float from_pixel[2] = { from_x - 0.5f, from_y - 0.5f }, from_point[3], other_point[3], other_pixel[2];
+                rs2_deproject_pixel_to_point(from_point, &from_intrin, from_pixel, depth);
+                rs2_transform_point_to_point(other_point, &from_to_other, from_point);
+                rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
+                const int other_x0 = static_cast<int>(other_pixel[0] + 0.5f);
+                const int other_y0 = static_cast<int>(other_pixel[1] + 0.5f);
+
+                // Map the bottom-right corner of the depth pixel onto the other image
+                from_pixel[0] = from_x + 0.5f; from_pixel[1] = from_y + 0.5f;
+                rs2_deproject_pixel_to_point(from_point, &from_intrin, from_pixel, depth);
+                rs2_transform_point_to_point(other_point, &from_to_other, from_point);
+                rs2_project_point_to_pixel(other_pixel, &other_intrin, other_point);
+                const int other_x1 = static_cast<int>(other_pixel[0] + 0.5f);
+                const int other_y1 = static_cast<int>(other_pixel[1] + 0.5f);
+
+                if (other_x0 < 0 || other_y0 < 0 || other_x1 >= other_intrin.width || other_y1 >= other_intrin.height)
+                    continue;
+
+                for (int y = other_y0; y <= other_y1; ++y)
+                {
+                    for (int x = other_x0; x <= other_x1; ++x)
+                    {
+                        int out_pixel_index = y * other_intrin.width + x;
+                        //Tranfer n-bit pixel to n-bit pixel
+                        for (int i = 0; i < from_bytes_per_pixel; i++)
+                        {
+                            const auto out_offset = out_pixel_index * output_image_bytes_per_pixel + i;
+                            const auto from_offset = from_pixel_index * output_image_bytes_per_pixel + i;
+                            p_out_frame[out_offset] = p_from_frame[from_offset] * (depth_units / meter_to_mm);
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+#endif
+
+bool CameraRealSense2::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	UDEBUG("");
+#ifdef RTABMAP_REALSENSE2
+
+	UINFO("setupDevice...");
+
+	auto list = ctx_->query_devices();
+	if (0 == list.size())
+	{
+		UERROR("No RealSense2 devices were found!");
+		return false;
+	}
+
+	bool found=false;
+	for (auto&& dev : list)
+	{
+		auto sn = dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
+		auto pid_str = dev.get_info(RS2_CAMERA_INFO_PRODUCT_ID);
+		uint16_t pid;
+		std::stringstream ss;
+		ss << std::hex << pid_str;
+		ss >> pid;
+		UINFO("Device with serial number %s was found with product ID=%d.", sn, (int)pid);
+		if (deviceId_.empty() || deviceId_ == sn)
+		{
+			*dev_ = dev;
+			found=true;
+			break;
+		}
+	}
+
+	if (!found)
+	{
+		UERROR("The requested device %s is NOT found!", deviceId_.c_str());
+		return false;
+	}
+
+	ctx_->set_devices_changed_callback([this](rs2::event_information& info)
+	{
+		if (info.was_removed(*dev_))
+		{
+			UERROR("The device has been disconnected!");
+		}
+	});
+
+
+	auto camera_name = dev_->get_info(RS2_CAMERA_INFO_NAME);
+	UINFO("Device Name: %s", camera_name);
+
+	auto sn = dev_->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
+	UINFO("Device Serial No: %s", sn);
+
+	auto fw_ver = dev_->get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION);
+	UINFO("Device FW version: %s", fw_ver);
+
+	auto pid = dev_->get_info(RS2_CAMERA_INFO_PRODUCT_ID);
+	UINFO("Device Product ID: 0x%s", pid);
+
+	auto dev_sensors = dev_->query_sensors();
+
+	UINFO("Device Sensors: ");
+	std::vector<rs2::sensor> sensors(2); //0=rgb 1=depth
+	for(auto&& elem : dev_sensors)
+	{
+		std::string module_name = elem.get_info(RS2_CAMERA_INFO_NAME);
+		if ("Stereo Module" == module_name)
+		{
+			sensors[1] = elem;
+			sensors[1].set_option(rs2_option::RS2_OPTION_EMITTER_ENABLED, emitterEnabled_);
+			if(irDepth_)
+			{
+				sensors[0] = elem;
+			}
+		}
+		else if ("Coded-Light Depth Sensor" == module_name)
+		{
+		}
+		else if ("RGB Camera" == module_name)
+		{
+			if(!irDepth_)
+			{
+				sensors[0] = elem;
+			}
+		}
+		else if ("Wide FOV Camera" == module_name)
+		{
+		}
+		else if ("Motion Module" == module_name)
+		{
+		}
+		else
+		{
+			UERROR("Module Name \"%s\" isn't supported by LibRealSense!", module_name.c_str());
+			return false;
+		}
+		UINFO("%s was found.", elem.get_info(RS2_CAMERA_INFO_NAME));
+	}
+
+	UDEBUG("");
+
+	model_ = CameraModel();
+	rs2::stream_profile depthStreamProfile;
+	rs2::stream_profile rgbStreamProfile;
+	std::vector<std::vector<rs2::stream_profile> > profilesPerSensor(2);
+	 for (unsigned int i=0; i<sensors.size(); ++i)
+	 {
+		 UDEBUG("i=%d", (int)i);
+		auto profiles = sensors[i].get_stream_profiles();
+		bool added = false;
+		UDEBUG("profiles=%d", (int)profiles.size());
+		for (auto& profile : profiles)
+		{
+			auto video_profile = profile.as<rs2::video_stream_profile>();
+			if (video_profile.format() == (i==1?RS2_FORMAT_Z16:irDepth_?RS2_FORMAT_Y8:RS2_FORMAT_RGB8) &&
+				video_profile.width()  == 640 &&
+				video_profile.height() == 480 &&
+				video_profile.fps()    == 30)
+			{
+				profilesPerSensor[irDepth_?1:i].push_back(profile);
+				auto intrinsic = video_profile.get_intrinsics();
+				if(i==1)
+				{
+					depthBuffer_ = cv::Mat(cv::Size(640, 480), CV_16UC1, cv::Scalar(0));
+					depthStreamProfile = profile;
+					*depthIntrinsics_ = intrinsic;
+				}
+				else
+				{
+					rgbBuffer_ = cv::Mat(cv::Size(640, 480), irDepth_?CV_8UC1:CV_8UC3, irDepth_?cv::Scalar(0):cv::Scalar(0, 0, 0));
+					model_ = CameraModel(camera_name, intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy, this->getLocalTransform(), 0, cv::Size(intrinsic.width, intrinsic.height));
+					rgbStreamProfile = profile;
+					*rgbIntrinsics_ = intrinsic;
+				}
+				added = true;
+				break;
+			}
+		}
+		if (!added)
+		{
+			UERROR("Given stream configuration is not supported by the device! "
+					"Stream Index: %d, Width: %d, Height: %d, FPS: %d", i, 640, 480, 30);
+			return false;
+		}
+	 }
+
+	 if(!model_.isValidForProjection())
+	 {
+		 UERROR("Calibration info not valid!");
+		 return false;
+	 }
+	 *depthToRGBExtrinsics_ = depthStreamProfile.get_extrinsics_to(rgbStreamProfile);
+
+	 for (unsigned int i=0; i<sensors.size(); ++i)
+	 {
+		 if(profilesPerSensor[i].size())
+		 {
+			 UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
+			 sensors[i].open(profilesPerSensor[i]);
+			 if(i ==1)
+			 {
+				 auto depth_sensor = sensors[i].as<rs2::depth_sensor>();
+				 depth_scale_meters_ = depth_sensor.get_depth_scale();
+			 }
+			 sensors[i].start(*syncer_);
+		 }
+	 }
+
+	uSleep(1000); // ignore the first frames
+	UINFO("Enabling streams...done!");
+
+	return true;
+
+#else
+	UERROR("CameraRealSense: RTAB-Map is not built with RealSense2 support!");
+	return false;
+#endif
+}
+
+bool CameraRealSense2::isCalibrated() const
+{
+	return true;
+}
+
+std::string CameraRealSense2::getSerial() const
+{
+#ifdef RTABMAP_REALSENSE2
+	return dev_->get_info(RS2_CAMERA_INFO_SERIAL_NUMBER);
+#endif
+	return "NA";
+}
+
+void CameraRealSense2::setEmitterEnabled(bool enabled)
+{
+#ifdef RTABMAP_REALSENSE2
+	emitterEnabled_ = enabled;
+#endif
+}
+
+void CameraRealSense2::setIRDepthFormat(bool enabled)
+{
+#ifdef RTABMAP_REALSENSE2
+	irDepth_ = enabled;
+#endif
+}
+
+SensorData CameraRealSense2::captureImage(CameraInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_REALSENSE2
+
+	try{
+		auto frameset = syncer_->wait_for_frames(5000);
+		UTimer timer;
+		while (frameset.size() != 2 && timer.elapsed() < 2.0)
+		{
+			// maybe there is a latency with the USB, try again in 100 ms (for the next 2 seconds)
+			frameset = syncer_->wait_for_frames(100);
+		}
+		if (frameset.size() == 2)
+		{
+			double stamp = UTimer::now();
+			UDEBUG("Frameset arrived.");
+			bool is_rgb_arrived = false;
+			bool is_depth_arrived = false;
+			rs2::frame rgb_frame;
+			rs2::frame depth_frame;
+			for (auto it = frameset.begin(); it != frameset.end(); ++it)
+			{
+				auto f = (*it);
+				auto stream_type = f.get_profile().stream_type();
+				if (stream_type == RS2_STREAM_COLOR || stream_type == RS2_STREAM_INFRARED)
+				{
+					rgb_frame = f;
+					is_rgb_arrived = true;
+				}
+				else if (stream_type == RS2_STREAM_DEPTH)
+				{
+					depth_frame = f;
+					is_depth_arrived = true;
+				}
+			}
+
+			if(is_rgb_arrived && is_depth_arrived)
+			{
+				auto from_image_frame = depth_frame.as<rs2::video_frame>();
+				cv::Mat depth;
+				if(irDepth_)
+				{
+					depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type(), (void*)depth_frame.get_data()).clone();
+				}
+				else
+				{
+					depth = cv::Mat(depthBuffer_.size(), depthBuffer_.type());
+					alignFrame(*depthIntrinsics_, *rgbIntrinsics_,
+							depth_frame, from_image_frame.get_bytes_per_pixel(),
+							*depthToRGBExtrinsics_, depth, depth_scale_meters_);
+				}
+
+				cv::Mat rgb = cv::Mat(rgbBuffer_.size(), rgbBuffer_.type(), (void*)rgb_frame.get_data());
+				cv::Mat bgr;
+				if(rgb.channels() == 3)
+				{
+					cv::cvtColor(rgb, bgr, CV_RGB2BGR);
+				}
+				else
+				{
+					bgr = rgb.clone();
+				}
+
+				data = SensorData(bgr, depth, model_, this->getNextSeqID(), stamp);
+			}
+			else
+			{
+				UERROR("Not received depth and rgb");
+			}
+		}
+		else
+		{
+			UERROR("Missing frames (received %d)", (int)frameset.size());
+		}
+	}
+	catch(const std::exception& ex)
+	{
+		UERROR("An error has occurred during frame callback: %s", ex.what());
+	}
+#else
+	UERROR("CameraRealSense2: RTAB-Map is not built with RealSense2 support!");
+#endif
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/camera/CameraStereoDC1394.cpp

@@ -0,0 +1,439 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraStereoDC1394.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
+
+#ifdef RTABMAP_DC1394
+#include <dc1394/dc1394.h>
+#endif
+
+namespace rtabmap
+{
+
+#ifdef RTABMAP_DC1394
+class DC1394Device
+{
+public:
+	DC1394Device() :
+		camera_(0),
+		context_(0)
+	{
+
+	}
+	~DC1394Device()
+	{
+		if (camera_)
+		{
+			if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_OFF) ||
+				DC1394_SUCCESS != dc1394_capture_stop(camera_))
+			{
+				UWARN("unable to stop camera");
+			}
+
+			// Free resources
+			dc1394_capture_stop(camera_);
+			dc1394_camera_free(camera_);
+			camera_ = NULL;
+		}
+		if(context_)
+		{
+			dc1394_free(context_);
+			context_ = NULL;
+		}
+	}
+
+	const std::string & guid() const {return guid_;}
+
+	bool init()
+	{
+		if(camera_)
+		{
+			// Free resources
+			dc1394_capture_stop(camera_);
+			dc1394_camera_free(camera_);
+			camera_ = NULL;
+		}
+
+		// look for a camera
+		int err;
+		if(context_ == NULL)
+		{
+			context_ = dc1394_new ();
+			if (context_ == NULL)
+			{
+				UERROR(    "Could not initialize dc1394_context.\n"
+				"Make sure /dev/raw1394 exists, you have access permission,\n"
+				"and libraw1394 development package is installed.");
+				return false;
+			}
+		}
+
+		dc1394camera_list_t *list;
+		err = dc1394_camera_enumerate(context_, &list);
+		if (err != DC1394_SUCCESS)
+		{
+			UERROR("Could not get camera list");
+			return false;
+		}
+
+		if (list->num == 0)
+		{
+			UERROR("No cameras found");
+			dc1394_camera_free_list (list);
+			return false;
+		}
+		uint64_t guid = list->ids[0].guid;
+		dc1394_camera_free_list (list);
+
+		// Create a camera
+		camera_ = dc1394_camera_new (context_, guid);
+		if (!camera_)
+		{
+			UERROR("Failed to initialize camera with GUID [%016lx]", guid);
+			return false;
+		}
+
+		uint32_t value[3];
+		value[0]= camera_->guid & 0xffffffff;
+		value[1]= (camera_->guid >>32) & 0x000000ff;
+		value[2]= (camera_->guid >>40) & 0xfffff;
+		guid_ = uFormat("%06x%02x%08x", value[2], value[1], value[0]);
+
+		UINFO("camera model: %s %s", camera_->vendor, camera_->model);
+
+		// initialize camera
+		// Enable IEEE1394b mode if the camera and bus support it
+		bool bmode = camera_->bmode_capable;
+		if (bmode
+			&& (DC1394_SUCCESS !=
+			dc1394_video_set_operation_mode(camera_,
+									DC1394_OPERATION_MODE_1394B)))
+		{
+			bmode = false;
+			UWARN("failed to set IEEE1394b mode");
+		}
+
+		// start with highest speed supported
+		dc1394speed_t request = DC1394_ISO_SPEED_3200;
+		int rate = 3200;
+		if (!bmode)
+		{
+			// not IEEE1394b capable: so 400Mb/s is the limit
+			request = DC1394_ISO_SPEED_400;
+			rate = 400;
+		}
+
+		// round requested speed down to next-lower defined value
+		while (rate > 400)
+		{
+			if (request <= DC1394_ISO_SPEED_MIN)
+			{
+				// get current ISO speed of the device
+				dc1394speed_t curSpeed;
+				if (DC1394_SUCCESS == dc1394_video_get_iso_speed(camera_, &curSpeed) && curSpeed <= DC1394_ISO_SPEED_MAX)
+				{
+					// Translate curSpeed back to an int for the parameter
+					// update, works as long as any new higher speeds keep
+					// doubling.
+					request = curSpeed;
+					rate = 100 << (curSpeed - DC1394_ISO_SPEED_MIN);
+				}
+				else
+				{
+					UWARN("Unable to get ISO speed; assuming 400Mb/s");
+					rate = 400;
+					request = DC1394_ISO_SPEED_400;
+				}
+				break;
+			}
+			// continue with next-lower possible value
+			request = (dc1394speed_t) ((int) request - 1);
+			rate = rate / 2;
+		}
+
+		// set the requested speed
+		if (DC1394_SUCCESS != dc1394_video_set_iso_speed(camera_, request))
+		{
+			UERROR("Failed to set iso speed");
+			return false;
+		}
+
+		// set video mode
+		dc1394video_modes_t vmodes;
+		err = dc1394_video_get_supported_modes(camera_, &vmodes);
+		if (err != DC1394_SUCCESS)
+		{
+			UERROR("unable to get supported video modes");
+			return (dc1394video_mode_t) 0;
+		}
+
+		// see if requested mode is available
+		bool found = false;
+		dc1394video_mode_t videoMode = DC1394_VIDEO_MODE_FORMAT7_3; // bumblebee
+		for (uint32_t i = 0; i < vmodes.num; ++i)
+		{
+			if (vmodes.modes[i] == videoMode)
+			{
+				found = true;
+			}
+		}
+		if(!found)
+		{
+			UERROR("unable to get video mode %d", videoMode);
+			return false;
+		}
+
+		if (DC1394_SUCCESS != dc1394_video_set_mode(camera_, videoMode))
+		{
+			UERROR("Failed to set video mode %d", videoMode);
+			return false;
+		}
+
+		// special handling for Format7 modes
+		if (dc1394_is_video_mode_scalable(videoMode) == DC1394_TRUE)
+		{
+			if (DC1394_SUCCESS != dc1394_format7_set_color_coding(camera_, videoMode, DC1394_COLOR_CODING_RAW16))
+			{
+				UERROR("Could not set color coding");
+				return false;
+			}
+			uint32_t packetSize;
+			if (DC1394_SUCCESS != dc1394_format7_get_recommended_packet_size(camera_, videoMode, &packetSize))
+			{
+				UERROR("Could not get default packet size");
+				return false;
+			}
+
+			if (DC1394_SUCCESS != dc1394_format7_set_packet_size(camera_, videoMode, packetSize))
+			{
+				UERROR("Could not set packet size");
+				return false;
+			}
+		}
+		else
+		{
+			UERROR("Video is not in mode scalable");
+		}
+
+		// start the device streaming data
+		// Set camera to use DMA, improves performance.
+		if (DC1394_SUCCESS != dc1394_capture_setup(camera_, 4, DC1394_CAPTURE_FLAGS_DEFAULT))
+		{
+			UERROR("Failed to open device!");
+			return false;
+		}
+
+		// Start transmitting camera data
+		if (DC1394_SUCCESS != dc1394_video_set_transmission(camera_, DC1394_ON))
+		{
+			UERROR("Failed to start device!");
+			return false;
+		}
+
+		return true;
+	}
+
+	bool getImages(cv::Mat & left, cv::Mat & right)
+	{
+		if(camera_)
+		{
+			dc1394video_frame_t * frame = NULL;
+			UDEBUG("[%016lx] waiting camera", camera_->guid);
+			dc1394_capture_dequeue (camera_, DC1394_CAPTURE_POLICY_WAIT, &frame);
+			if (!frame)
+			{
+				UERROR("Unable to capture frame");
+				return false;
+			}
+			dc1394video_frame_t frame1 = *frame;
+			// deinterlace frame into two imagesCount one on top the other
+			size_t frame1_size = frame->total_bytes;
+			frame1.image = (unsigned char *) malloc(frame1_size);
+			frame1.allocated_image_bytes = frame1_size;
+			frame1.color_coding = DC1394_COLOR_CODING_RAW8;
+			int err = dc1394_deinterlace_stereo_frames(frame, &frame1, DC1394_STEREO_METHOD_INTERLACED);
+			if (err != DC1394_SUCCESS)
+			{
+				free(frame1.image);
+				dc1394_capture_enqueue(camera_, frame);
+				UERROR("Could not extract stereo frames");
+				return false;
+			}
+
+			uint8_t* capture_buffer = reinterpret_cast<uint8_t *>(frame1.image);
+			UASSERT(capture_buffer);
+
+			cv::Mat image(frame->size[1], frame->size[0], CV_8UC3);
+			cv::Mat image2 = image.clone();
+
+			//DC1394_COLOR_CODING_RAW16:
+			//DC1394_COLOR_FILTER_BGGR
+			cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer), left, CV_BayerRG2BGR);
+			cv::cvtColor(cv::Mat(frame->size[1], frame->size[0], CV_8UC1, capture_buffer+image.total()), right, CV_BayerRG2GRAY);
+
+			dc1394_capture_enqueue(camera_, frame);
+
+			free(frame1.image);
+
+			return true;
+		}
+		return false;
+	}
+
+private:
+	dc1394camera_t *camera_;
+	dc1394_t *context_;
+	std::string guid_;
+};
+#endif
+
+bool CameraStereoDC1394::available()
+{
+#ifdef RTABMAP_DC1394
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraStereoDC1394::CameraStereoDC1394(float imageRate, const Transform & localTransform) :
+		Camera(imageRate, localTransform)
+#ifdef RTABMAP_DC1394
+        ,
+		device_(0)
+#endif
+{
+#ifdef RTABMAP_DC1394
+	device_ = new DC1394Device();
+#endif
+}
+
+CameraStereoDC1394::~CameraStereoDC1394()
+{
+#ifdef RTABMAP_DC1394
+	delete device_;
+#endif
+}
+
+bool CameraStereoDC1394::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+#ifdef RTABMAP_DC1394
+	if(device_)
+	{
+		bool ok = device_->init();
+		if(ok)
+		{
+			// look for calibration files
+			if(!calibrationFolder.empty())
+			{
+				if(!stereoModel_.load(calibrationFolder, cameraName.empty()?device_->guid():cameraName, false))
+				{
+					UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+							cameraName.empty()?device_->guid().c_str():cameraName.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;
+	}
+#else
+	UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
+#endif
+	return false;
+}
+
+bool CameraStereoDC1394::isCalibrated() const
+{
+#ifdef RTABMAP_DC1394
+	return stereoModel_.isValidForProjection();
+#else
+	return false;
+#endif
+}
+
+std::string CameraStereoDC1394::getSerial() const
+{
+#ifdef RTABMAP_DC1394
+	if(device_)
+	{
+		return device_->guid();
+	}
+#endif
+	return "";
+}
+
+SensorData CameraStereoDC1394::captureImage(CameraInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_DC1394
+	if(device_)
+	{
+		cv::Mat left, right;
+		device_->getImages(left, right);
+
+		if(!left.empty() && !right.empty())
+		{
+			// Rectification
+			if(stereoModel_.left().isValidForRectification())
+			{
+				left = stereoModel_.left().rectifyImage(left);
+			}
+			if(stereoModel_.right().isValidForRectification())
+			{
+				right = stereoModel_.right().rectifyImage(right);
+			}
+			StereoCameraModel model;
+			if(stereoModel_.isValidForProjection())
+			{
+				model = StereoCameraModel(
+						stereoModel_.left().fx(), //fx
+						stereoModel_.left().fy(), //fy
+						stereoModel_.left().cx(), //cx
+						stereoModel_.left().cy(), //cy
+						stereoModel_.baseline(),
+						this->getLocalTransform(),
+						left.size());
+			}
+			data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
+		}
+	}
+#else
+	UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
+#endif
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/camera/CameraStereoFlyCapture2.cpp

@@ -0,0 +1,319 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraStereoFlyCapture2.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
+
+#ifdef RTABMAP_FLYCAPTURE2
+#include <triclops.h>
+#include <fc2triclops.h>
+#endif
+
+namespace rtabmap
+{
+
+CameraStereoFlyCapture2::CameraStereoFlyCapture2(float imageRate, const Transform & localTransform) :
+		Camera(imageRate, localTransform)
+#ifdef RTABMAP_FLYCAPTURE2
+        ,
+		camera_(0),
+		triclopsCtx_(0)
+#endif
+{
+#ifdef RTABMAP_FLYCAPTURE2
+	camera_ = new FlyCapture2::Camera();
+#endif
+}
+
+CameraStereoFlyCapture2::~CameraStereoFlyCapture2()
+{
+#ifdef RTABMAP_FLYCAPTURE2
+	// Close the camera
+	camera_->StopCapture();
+	camera_->Disconnect();
+
+	// Destroy the Triclops context
+	triclopsDestroyContext( triclopsCtx_ ) ;
+
+	delete camera_;
+#endif
+}
+
+bool CameraStereoFlyCapture2::available()
+{
+#ifdef RTABMAP_FLYCAPTURE2
+	return true;
+#else
+	return false;
+#endif
+}
+
+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;
+	}
+
+	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 RTABMAP_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 RTABMAP_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 RTABMAP_FLYCAPTURE2
+struct ImageContainer
+{
+	FlyCapture2::Image tmp[2];
+    FlyCapture2::Image unprocessed[2];
+} ;
+#endif
+
+SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_FLYCAPTURE2
+	if(camera_ && triclopsCtx_ && camera_->IsConnected())
+	{
+		// grab image from camera.
+		// this image contains both right and left imagesCount
+		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 );
+
+						cv::Mat left,right;
+						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
+						float fx, cy, cx, baseline;
+						triclopsGetFocalLength(triclopsCtx_, &fx);
+						triclopsGetImageCenter(triclopsCtx_, &cy, &cx);
+						triclopsGetBaseline(triclopsCtx_, &baseline);
+
+						StereoCameraModel model(
+								fx,
+								fx,
+								cx,
+								cy,
+								baseline,
+								this->getLocalTransform(),
+								left.size());
+						data = SensorData(left, right, model, this->getNextSeqID(), UTimer::now());
+					}
+				}
+			}
+		}
+	}
+
+#else
+	UERROR("CameraStereoFlyCapture2: RTAB-Map is not built with Triclops support!");
+#endif
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/camera/CameraStereoImages.cpp

@@ -0,0 +1,203 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraStereoImages.h>
+#include <rtabmap/utilite/UStl.h>
+
+namespace rtabmap
+{
+
+bool CameraStereoImages::available()
+{
+	return true;
+}
+
+CameraStereoImages::CameraStereoImages(
+		const std::string & pathLeftImages,
+		const std::string & pathRightImages,
+		bool rectifyImages,
+		float imageRate,
+		const Transform & localTransform) :
+		CameraImages(pathLeftImages, imageRate, localTransform),
+		camera2_(new CameraImages(pathRightImages))
+{
+	this->setImagesRectified(rectifyImages);
+}
+
+CameraStereoImages::CameraStereoImages(
+		const std::string & pathLeftRightImages,
+		bool rectifyImages,
+		float imageRate,
+		const Transform & localTransform) :
+		CameraImages("", imageRate, localTransform),
+		camera2_(0)
+{
+	std::vector<std::string> paths = uListToVector(uSplit(pathLeftRightImages, uStrContains(pathLeftRightImages, ":")?':':';'));
+	if(paths.size() >= 1)
+	{
+		this->setPath(paths[0]);
+		this->setImagesRectified(rectifyImages);
+
+		if(paths.size() >= 2)
+		{
+			camera2_ = new CameraImages(paths[1]);
+		}
+	}
+	else
+	{
+		UERROR("The path is empty!");
+	}
+}
+
+CameraStereoImages::~CameraStereoImages()
+{
+	UDEBUG("");
+	delete camera2_;
+	UDEBUG("");
+}
+
+bool CameraStereoImages::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	// look for calibration files
+	if(!calibrationFolder.empty() && !cameraName.empty())
+	{
+		if(!stereoModel_.load(calibrationFolder, cameraName, false) && !stereoModel_.isValidForProjection())
+		{
+			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+					cameraName.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());
+		}
+	}
+
+	stereoModel_.setLocalTransform(this->getLocalTransform());
+	stereoModel_.setName(cameraName);
+	if(this->isImagesRectified() && !stereoModel_.isValidForRectification())
+	{
+		UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
+		return false;
+	}
+
+	//desactivate before init as we will do it in this class instead for convenience
+	bool rectify = this->isImagesRectified();
+	this->setImagesRectified(false);
+
+	bool success = false;
+	if(CameraImages::init())
+	{
+		if(camera2_)
+		{
+			camera2_->setBayerMode(this->getBayerMode());
+			if(camera2_->init())
+			{
+				if(this->imagesCount() == camera2_->imagesCount())
+				{
+					success = true;
+				}
+				else
+				{
+					UERROR("Cameras don't have the same number of images (%d vs %d)",
+							this->imagesCount(), camera2_->imagesCount());
+				}
+			}
+			else
+			{
+				UERROR("Cannot initialize the second camera.");
+			}
+		}
+		else
+		{
+			success = true;
+		}
+	}
+	this->setImagesRectified(rectify); // reset the flag
+	return success;
+}
+
+bool CameraStereoImages::isCalibrated() const
+{
+	return stereoModel_.isValidForProjection();
+}
+
+std::string CameraStereoImages::getSerial() const
+{
+	return stereoModel_.name();
+}
+
+SensorData CameraStereoImages::captureImage(CameraInfo * info)
+{
+	SensorData data;
+
+	SensorData left, right;
+	left = CameraImages::captureImage(info);
+	if(!left.imageRaw().empty())
+	{
+		if(camera2_)
+		{
+			right = camera2_->takeImage(info);
+		}
+		else
+		{
+			right = this->takeImage(info);
+		}
+
+		if(!right.imageRaw().empty())
+		{
+			// Rectification
+			cv::Mat leftImage = left.imageRaw();
+			cv::Mat rightImage = right.imageRaw();
+			if(rightImage.type() != CV_8UC1)
+			{
+				cv::Mat tmp;
+				cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
+				rightImage = tmp;
+			}
+			if(this->isImagesRectified() && stereoModel_.isValidForRectification())
+			{
+				leftImage = stereoModel_.left().rectifyImage(leftImage);
+				rightImage = stereoModel_.right().rectifyImage(rightImage);
+			}
+
+			if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
+			{
+				stereoModel_.setImageSize(leftImage.size());
+			}
+
+			data = SensorData(left.laserScanRaw(), leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
+			data.setGroundTruth(left.groundTruth());
+		}
+	}
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/camera/CameraStereoVideo.cpp

@@ -0,0 +1,270 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraStereoVideo.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
+
+namespace rtabmap
+{
+
+bool CameraStereoVideo::available()
+{
+	return true;
+}
+
+CameraStereoVideo::CameraStereoVideo(
+		const std::string & path,
+		bool rectifyImages,
+		float imageRate,
+		const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		path_(path),
+		rectifyImages_(rectifyImages),
+		src_(CameraVideo::kVideoFile),
+		usbDevice_(0),
+		usbDevice2_(-1)
+{
+}
+
+CameraStereoVideo::CameraStereoVideo(
+		const std::string & pathLeft,
+		const std::string & pathRight,
+		bool rectifyImages,
+		float imageRate,
+		const Transform & localTransform) :
+		Camera(imageRate, localTransform),
+		path_(pathLeft),
+		path2_(pathRight),
+		rectifyImages_(rectifyImages),
+		src_(CameraVideo::kVideoFile),
+		usbDevice_(0),
+		usbDevice2_(-1)
+{
+}
+
+CameraStereoVideo::CameraStereoVideo(
+	int device,
+	bool rectifyImages,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	rectifyImages_(rectifyImages),
+	src_(CameraVideo::kUsbDevice),
+	usbDevice_(device),
+	usbDevice2_(-1)
+{
+}
+
+CameraStereoVideo::CameraStereoVideo(
+	int deviceLeft,
+	int deviceRight,
+	bool rectifyImages,
+	float imageRate,
+	const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	rectifyImages_(rectifyImages),
+	src_(CameraVideo::kUsbDevice),
+	usbDevice_(deviceLeft),
+	usbDevice2_(deviceRight)
+{
+}
+
+CameraStereoVideo::~CameraStereoVideo()
+{
+	capture_.release();
+	capture2_.release();
+}
+
+bool CameraStereoVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	cameraName_ = cameraName;
+	if(capture_.isOpened())
+	{
+		capture_.release();
+	}
+	if(capture2_.isOpened())
+	{
+		capture2_.release();
+	}
+
+	if (src_ == CameraVideo::kUsbDevice)
+	{
+		capture_.open(usbDevice_);
+		if(usbDevice2_ < 0)
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on device %d", usbDevice_);
+		}
+		else
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: Usb device initialization on devices %d and %d", usbDevice_, usbDevice2_);
+			capture2_.open(usbDevice2_);
+		}
+	}
+	else if (src_ == CameraVideo::kVideoFile)
+	{
+		capture_.open(path_.c_str());
+		if(path2_.empty())
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: filename=\"%s\"", path_.c_str());
+		}
+		else
+		{
+			ULOGGER_DEBUG("CameraStereoVideo: filenames=\"%s\" and \"%s\"", path_.c_str(), path2_.c_str());
+			capture2_.open(path2_.c_str());
+		}
+	}
+	else
+	{
+		ULOGGER_ERROR("CameraStereoVideo: Unknown source...");
+	}
+
+	if(!capture_.isOpened() || ((!path2_.empty() || usbDevice2_>=0) && !capture2_.isOpened()))
+	{
+		ULOGGER_ERROR("CameraStereoVideo: Failed to create a capture object!");
+		capture_.release();
+		capture2_.release();
+		return false;
+	}
+
+	if (cameraName_.empty())
+	{
+		unsigned int guid = (unsigned int)capture_.get(CV_CAP_PROP_GUID);
+		if (guid != 0 && guid != 0xffffffff)
+		{
+			cameraName_ = uFormat("%08x", guid);
+		}
+	}
+
+	// look for calibration files
+	if(!calibrationFolder.empty() && !cameraName_.empty())
+	{
+		if(!stereoModel_.load(calibrationFolder, cameraName_, false))
+		{
+			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+				cameraName_.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());
+		}
+	}
+
+	stereoModel_.setLocalTransform(this->getLocalTransform());
+	if(rectifyImages_ && !stereoModel_.isValidForRectification())
+	{
+		UERROR("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid.");
+		return false;
+	}
+	return true;
+}
+
+bool CameraStereoVideo::isCalibrated() const
+{
+	return stereoModel_.isValidForProjection();
+}
+
+std::string CameraStereoVideo::getSerial() const
+{
+	return cameraName_;
+}
+
+SensorData CameraStereoVideo::captureImage(CameraInfo * info)
+{
+	SensorData data;
+
+	cv::Mat img;
+	if(capture_.isOpened() && ((path2_.empty() && usbDevice2_ < 0) || capture2_.isOpened()))
+	{
+		cv::Mat leftImage;
+		cv::Mat rightImage;
+		if(path2_.empty() && usbDevice2_ < 0)
+		{
+			if(!capture_.read(img))
+			{
+				return data;
+			}
+			// Side by side stream
+			leftImage = cv::Mat(img, cv::Rect( 0, 0, img.size().width/2, img.size().height ));
+			rightImage = cv::Mat(img, cv::Rect( img.size().width/2, 0, img.size().width/2, img.size().height ));
+		}
+		else if(!capture_.read(leftImage) || !capture2_.read(rightImage))
+		{
+			return data;
+		}
+		else if(leftImage.cols != rightImage.cols || leftImage.rows != rightImage.rows)
+		{
+			UERROR("Left and right streams don't have image of the same size: left=%dx%d right=%dx%d",
+					leftImage.cols, leftImage.rows, rightImage.cols, rightImage.rows);
+			return data;
+		}
+
+		// Rectification
+		bool rightCvt = false;
+		if(rightImage.type() != CV_8UC1)
+		{
+			cv::Mat tmp;
+			cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
+			rightImage = tmp;
+			rightCvt = true;
+		}
+
+		if(rectifyImages_ && stereoModel_.left().isValidForRectification() && stereoModel_.right().isValidForRectification())
+		{
+			leftImage = stereoModel_.left().rectifyImage(leftImage);
+			rightImage = stereoModel_.right().rectifyImage(rightImage);
+		}
+		else
+		{
+			leftImage = leftImage.clone();
+			if(!rightCvt)
+			{
+				rightImage = rightImage.clone();
+			}
+		}
+
+		if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0)
+		{
+			stereoModel_.setImageSize(leftImage.size());
+		}
+
+		data = SensorData(leftImage, rightImage, stereoModel_, this->getNextSeqID(), UTimer::now());
+	}
+	else
+	{
+		ULOGGER_WARN("The camera must be initialized before requesting an image.");
+	}
+
+	return data;
+}
+
+
+} // namespace rtabmap

corelib/src/camera/CameraStereoZed.cpp

@@ -0,0 +1,390 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraStereoZed.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UThreadC.h>
+#include <rtabmap/utilite/UConversion.h>
+
+#ifdef RTABMAP_ZED
+#include <sl/Camera.hpp>
+#endif
+
+namespace rtabmap
+{
+
+bool CameraStereoZed::available()
+{
+#ifdef RTABMAP_ZED
+	return true;
+#else
+	return false;
+#endif
+}
+
+CameraStereoZed::CameraStereoZed(
+		int deviceId,
+		int resolution,
+		int quality,
+		int sensingMode,
+		int confidenceThr,
+		bool computeOdometry,
+		float imageRate,
+		const Transform & localTransform,
+		bool selfCalibration) :
+	Camera(imageRate, localTransform)
+#ifdef RTABMAP_ZED
+    ,
+	zed_(0),
+	src_(CameraVideo::kUsbDevice),
+	usbDevice_(deviceId),
+	svoFilePath_(""),
+	resolution_(resolution),
+	quality_(quality),
+	selfCalibration_(selfCalibration),
+	sensingMode_(sensingMode),
+	confidenceThr_(confidenceThr),
+	computeOdometry_(computeOdometry),
+	lost_(true)
+#endif
+{
+	UDEBUG("");
+#ifdef RTABMAP_ZED
+	UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
+	UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
+	UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
+	UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
+#endif
+}
+
+CameraStereoZed::CameraStereoZed(
+		const std::string & filePath,
+		int quality,
+		int sensingMode,
+		int confidenceThr,
+		bool computeOdometry,
+		float imageRate,
+		const Transform & localTransform,
+		bool selfCalibration) :
+	Camera(imageRate, localTransform)
+#ifdef RTABMAP_ZED
+    ,
+	zed_(0),
+	src_(CameraVideo::kVideoFile),
+	usbDevice_(0),
+	svoFilePath_(filePath),
+	resolution_(2),
+	quality_(quality),
+	selfCalibration_(selfCalibration),
+	sensingMode_(sensingMode),
+	confidenceThr_(confidenceThr),
+	computeOdometry_(computeOdometry),
+	lost_(true)
+#endif
+{
+	UDEBUG("");
+#ifdef RTABMAP_ZED
+	UASSERT(resolution_ >= sl::RESOLUTION_HD2K && resolution_ <sl::RESOLUTION_LAST);
+	UASSERT(quality_ >= sl::DEPTH_MODE_NONE && quality_ <sl::DEPTH_MODE_LAST);
+	UASSERT(sensingMode_ >= sl::SENSING_MODE_STANDARD && sensingMode_ <sl::SENSING_MODE_LAST);
+	UASSERT(confidenceThr_ >= 0 && confidenceThr_ <=100);
+#endif
+}
+
+CameraStereoZed::~CameraStereoZed()
+{
+#ifdef RTABMAP_ZED
+	delete zed_;
+#endif
+}
+
+bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	UDEBUG("");
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		delete zed_;
+		zed_ = 0;
+	}
+	
+	lost_ = true;
+
+	sl::InitParameters param;
+	param.camera_resolution=static_cast<sl::RESOLUTION>(resolution_);
+	param.camera_fps=getImageRate();
+	param.camera_linux_id=usbDevice_;
+	param.depth_mode=(sl::DEPTH_MODE)quality_;
+	param.coordinate_units=sl::UNIT_METER;
+	param.coordinate_system=(sl::COORDINATE_SYSTEM)sl::COORDINATE_SYSTEM_IMAGE ;
+	param.sdk_verbose=true;
+	param.sdk_gpu_id=-1;
+	param.depth_minimum_distance=-1;
+	param.camera_disable_self_calib=!selfCalibration_;
+
+	sl::ERROR_CODE r = sl::ERROR_CODE::SUCCESS;
+	if(src_ == CameraVideo::kVideoFile)
+	{
+		UINFO("svo file = %s", svoFilePath_.c_str());
+		zed_ = new sl::Camera(); // Use in SVO playback mode
+		param.svo_input_filename=svoFilePath_.c_str();
+		r = zed_->open(param);
+	}
+	else
+	{
+		UINFO("Resolution=%d imagerate=%f device=%d", resolution_, getImageRate(), usbDevice_);
+		zed_ = new sl::Camera(); // Use in Live Mode
+		r = zed_->open(param);
+	}
+
+	if(r!=sl::ERROR_CODE::SUCCESS)
+	{
+		UERROR("Camera initialization failed: \"%s\"", toString(r).c_str());
+		delete zed_;
+		zed_ = 0;
+		return false;
+	}
+
+
+	UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
+	      quality_, sl::UNIT_METER, sl::COORDINATE_SYSTEM_IMAGE , selfCalibration_?"true":"false");
+	UDEBUG("");
+
+	if(quality_!=sl::DEPTH_MODE_NONE)
+	{
+		zed_->setConfidenceThreshold(confidenceThr_);
+	}
+
+	if (computeOdometry_)
+	{
+		sl::TrackingParameters tparam;
+		tparam.enable_spatial_memory=false;
+		zed_->enableTracking(tparam);
+		if(r!=sl::ERROR_CODE::SUCCESS)
+		{
+			UERROR("Camera tracking initialization failed: \"%s\"", toString(r).c_str());
+		}
+	}
+
+	sl::CameraInformation infos = zed_->getCameraInformation();
+	sl::CalibrationParameters *stereoParams = &(infos.calibration_parameters );
+	sl::Resolution res = stereoParams->left_cam.image_size;
+
+	stereoModel_ = StereoCameraModel(
+		stereoParams->left_cam.fx, 
+		stereoParams->left_cam.fy, 
+		stereoParams->left_cam.cx, 
+		stereoParams->left_cam.cy, 
+		stereoParams->T[0],//baseline
+		this->getLocalTransform(),
+		cv::Size(res.width, res.height));
+
+	UINFO("Calibration: fx=%f, fy=%f, cx=%f, cy=%f, baseline=%f, width=%d, height=%d, transform=%s",
+			stereoParams->left_cam.fx,
+			stereoParams->left_cam.fy,
+			stereoParams->left_cam.cx,
+			stereoParams->left_cam.cy,
+			stereoParams->T[0],//baseline
+			(int)res.width,
+			(int)res.height,
+			this->getLocalTransform().prettyPrint().c_str());
+
+	return true;
+#else
+	UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
+#endif
+	return false;
+}
+
+bool CameraStereoZed::isCalibrated() const
+{
+#ifdef RTABMAP_ZED
+	return stereoModel_.isValidForProjection();
+#else
+	return false;
+#endif
+}
+
+std::string CameraStereoZed::getSerial() const
+{
+#ifdef RTABMAP_ZED
+	if(zed_)
+	{
+		return uFormat("%x", zed_->getCameraInformation ().serial_number);
+	}
+#endif
+	return "";
+}
+
+bool CameraStereoZed::odomProvided() const
+{
+#ifdef RTABMAP_ZED
+	return computeOdometry_;
+#else
+	return false;
+#endif
+}
+#ifdef RTABMAP_ZED
+static cv::Mat slMat2cvMat(sl::Mat& input) {
+	//convert MAT_TYPE to CV_TYPE
+	int cv_type = -1;
+	switch (input.getDataType()) {
+	case sl::MAT_TYPE_32F_C1: cv_type = CV_32FC1; break;
+	case sl::MAT_TYPE_32F_C2: cv_type = CV_32FC2; break;
+	case sl::MAT_TYPE_32F_C3: cv_type = CV_32FC3; break;
+	case sl::MAT_TYPE_32F_C4: cv_type = CV_32FC4; break;
+	case sl::MAT_TYPE_8U_C1: cv_type = CV_8UC1; break;
+	case sl::MAT_TYPE_8U_C2: cv_type = CV_8UC2; break;
+	case sl::MAT_TYPE_8U_C3: cv_type = CV_8UC3; break;
+	case sl::MAT_TYPE_8U_C4: cv_type = CV_8UC4; break;
+	default: break;
+	}	
+	// cv::Mat data requires a uchar* pointer. Therefore, we get the uchar1 pointer from sl::Mat (getPtr<T>())
+	//cv::Mat and sl::Mat will share the same memory pointer
+	return cv::Mat(input.getHeight(), input.getWidth(), cv_type, input.getPtr<sl::uchar1>(sl::MEM_CPU));
+}
+
+Transform zedPoseToTransform(const sl::Pose & pose)
+{
+	return Transform(
+			pose.pose_data.m[0], pose.pose_data.m[1], pose.pose_data.m[2], pose.pose_data.m[3],
+			pose.pose_data.m[4], pose.pose_data.m[5], pose.pose_data.m[6], pose.pose_data.m[7],
+			pose.pose_data.m[8], pose.pose_data.m[9], pose.pose_data.m[10], pose.pose_data.m[11]);
+}
+#endif
+
+SensorData CameraStereoZed::captureImage(CameraInfo * info)
+{
+	SensorData data;
+#ifdef RTABMAP_ZED
+	sl::RuntimeParameters rparam((sl::SENSING_MODE)sensingMode_, quality_ > 0, quality_ > 0, sl::REFERENCE_FRAME_CAMERA);
+	if(zed_)
+	{
+		UTimer timer;
+		bool res = zed_->grab(rparam);
+		while (src_ == CameraVideo::kUsbDevice && res!=sl::SUCCESS && timer.elapsed() < 2.0)
+		{
+			// maybe there is a latency with the USB, try again in 10 ms (for the next 2 seconds)
+			uSleep(10);
+			res = zed_->grab(rparam);
+		}
+		if(res==sl::SUCCESS)
+		{
+			// get left image
+			sl::Mat tmp;
+			zed_->retrieveImage(tmp,sl::VIEW_LEFT);
+			cv::Mat rgbaLeft = slMat2cvMat(tmp);
+
+			cv::Mat left;
+			cv::cvtColor(rgbaLeft, left, cv::COLOR_BGRA2BGR);
+
+			if(quality_ > 0)
+			{
+				// get depth image
+				cv::Mat depth;
+				sl::Mat tmp;
+				zed_->retrieveMeasure(tmp,sl::MEASURE_DEPTH);
+				slMat2cvMat(tmp).copyTo(depth);
+
+				data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
+			}
+			else
+			{
+				// get right image
+				sl::Mat tmp;zed_->retrieveImage(tmp,sl::VIEW_RIGHT );
+				cv::Mat rgbaRight = slMat2cvMat(tmp);
+				cv::Mat right;
+				cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
+			
+				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
+			}
+
+			if (computeOdometry_ && info)
+			{
+				sl::Pose pose;
+				sl::TRACKING_STATE tracking_state = zed_->getPosition(pose);
+				if (tracking_state == sl::TRACKING_STATE_OK)
+				{
+					int trackingConfidence = pose.pose_confidence;
+					// FIXME What does pose_confidence == -1 mean?
+					if (trackingConfidence>0)
+					{
+						info->odomPose = zedPoseToTransform(pose);
+						if (!info->odomPose.isNull())
+						{
+							//transform x->forward, y->left, z->up
+							Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+							info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
+
+							if (lost_)
+							{
+								info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
+								lost_ = false;
+								UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
+							}
+							else
+							{
+								info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
+								UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
+							}
+						}
+						else
+						{
+							info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
+							lost_ = true;
+							UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
+						}
+					}
+					else
+					{
+						info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
+						lost_ = true;
+						UWARN("ZED lost! (trackingConfidence=%d)", trackingConfidence);
+					}
+				}
+				else
+				{
+					UWARN("Tracking not ok: state=\"%s\"", toString(tracking_state).c_str());
+				}
+			}
+		}
+		else if(src_ == CameraVideo::kUsbDevice)
+		{
+			UERROR("CameraStereoZed: Failed to grab images after 2 seconds!");
+		}
+		else
+		{
+			UWARN("CameraStereoZed: end of stream is reached!");
+		}
+	}
+#else
+	UERROR("CameraStereoZED: RTAB-Map is not built with ZED sdk support!");
+#endif
+	return data;
+}
+
+} // namespace rtabmap

corelib/src/camera/CameraVideo.cpp

@@ -0,0 +1,177 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/core/camera/CameraVideo.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
+
+namespace rtabmap
+{
+
+CameraVideo::CameraVideo(
+		int usbDevice,
+		bool rectifyImages,
+		float imageRate,
+		const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	_rectifyImages(rectifyImages),
+	_src(kUsbDevice),
+	_usbDevice(usbDevice)
+{
+
+}
+
+CameraVideo::CameraVideo(
+		const std::string & filePath,
+		bool rectifyImages,
+		float imageRate,
+		const Transform & localTransform) :
+	Camera(imageRate, localTransform),
+	_filePath(filePath),
+	_rectifyImages(rectifyImages),
+	_src(kVideoFile),
+	_usbDevice(0)
+{
+}
+
+CameraVideo::~CameraVideo()
+{
+	_capture.release();
+}
+
+bool CameraVideo::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	_guid = cameraName;
+	if(_capture.isOpened())
+	{
+		_capture.release();
+	}
+
+	if(_src == kUsbDevice)
+	{
+		ULOGGER_DEBUG("CameraVideo::init() Usb device initialization on device %d", _usbDevice);
+		_capture.open(_usbDevice);
+	}
+	else if(_src == kVideoFile)
+	{
+		ULOGGER_DEBUG("Camera: filename=\"%s\"", _filePath.c_str());
+		_capture.open(_filePath.c_str());
+	}
+	else
+	{
+		ULOGGER_ERROR("Camera: Unknown source...");
+	}
+	if(!_capture.isOpened())
+	{
+		ULOGGER_ERROR("Camera: Failed to create a capture object!");
+		_capture.release();
+		return false;
+	}
+	else
+	{
+		if (_guid.empty())
+		{
+			unsigned int guid = (unsigned int)_capture.get(CV_CAP_PROP_GUID);
+			if (guid != 0 && guid != 0xffffffff)
+			{
+				_guid = uFormat("%08x", guid);
+			}
+		}
+
+		// look for calibration files
+		if(!calibrationFolder.empty() && !_guid.empty())
+		{
+			if(!_model.load(calibrationFolder, _guid))
+			{
+				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+						_guid.c_str(), calibrationFolder.c_str());
+			}
+			else
+			{
+				UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
+						_model.fx(),
+						_model.fy(),
+						_model.cx(),
+						_model.cy());
+			}
+		}
+		_model.setLocalTransform(this->getLocalTransform());
+		if(_rectifyImages && !_model.isValidForRectification())
+		{
+			UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");
+			return false;
+		}
+	}
+	return true;
+}
+
+bool CameraVideo::isCalibrated() const
+{
+	return _model.isValidForProjection();
+}
+
+std::string CameraVideo::getSerial() const
+{
+	return _guid;
+}
+
+SensorData CameraVideo::captureImage(CameraInfo * info)
+{
+	cv::Mat img;
+	if(_capture.isOpened())
+	{
+		if(_capture.read(img))
+		{
+			if(_model.imageHeight() == 0 || _model.imageWidth() == 0)
+			{
+				_model.setImageSize(img.size());
+			}
+
+			if(_model.isValidForRectification() && _rectifyImages)
+			{
+				img = _model.rectifyImage(img);
+			}
+			else
+			{
+				// clone required
+				img = img.clone();
+			}
+		}
+		else if(_usbDevice)
+		{
+			UERROR("Camera has been disconnected!");
+		}
+	}
+	else
+	{
+		ULOGGER_WARN("The camera must be initialized before requesting an image.");
+	}
+
+	return SensorData(img, _model, this->getNextSeqID(), UTimer::now());
+}
+
+} // namespace rtabmap

corelib/src/odometry/OdometryDVO.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "rtabmap/core/OdometryDVO.h"
+#include "rtabmap/core/odometry/OdometryDVO.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/utilite/ULogger.h"

corelib/src/odometry/OdometryF2F.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "rtabmap/core/OdometryF2F.h"
+#include "rtabmap/core/odometry/OdometryF2F.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/Registration.h"
 #include "rtabmap/core/EpipolarGeometry.h"

corelib/src/odometry/OdometryF2M.cpp

@@ -45,7 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UMath.h"
 #include "rtabmap/utilite/UConversion.h"
 #include <opencv2/calib3d/calib3d.hpp>
-#include <rtabmap/core/OdometryF2M.h>
+#include <rtabmap/core/odometry/OdometryF2M.h>
 #include <pcl/common/io.h>
 
 #if _MSC_VER

corelib/src/odometry/OdometryFovis.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "rtabmap/core/OdometryFovis.h"
+#include "rtabmap/core/odometry/OdometryFovis.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/utilite/ULogger.h"

corelib/src/odometry/OdometryLOAM.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "rtabmap/core/OdometryLOAM.h"
+#include "rtabmap/core/odometry/OdometryLOAM.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/utilite/ULogger.h"

corelib/src/odometry/OdometryMSCKF.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include "rtabmap/core/OdometryMSCKF.h"
+#include "rtabmap/core/odometry/OdometryMSCKF.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/utilite/ULogger.h"

corelib/src/odometry/OdometryMono.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "rtabmap/core/OdometryMono.h"
+#include "rtabmap/core/odometry/OdometryMono.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/Signature.h"

corelib/src/odometry/OdometryORBSLAM2.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "rtabmap/core/OdometryORBSLAM2.h"
+#include "rtabmap/core/odometry/OdometryORBSLAM2.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/core/util3d_transforms.h"

corelib/src/odometry/OdometryOkvis.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "rtabmap/core/OdometryOkvis.h"
+#include "rtabmap/core/odometry/OdometryOkvis.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/utilite/ULogger.h"

corelib/src/odometry/OdometryViso2.cpp

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include "rtabmap/core/OdometryViso2.h"
+#include "rtabmap/core/odometry/OdometryViso2.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/utilite/ULogger.h"

corelib/src/optimizer/OptimizerCVSBA.cpp

@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <set>
 
-#include <rtabmap/core/OptimizerCVSBA.h>
+#include <rtabmap/core/optimizer/OptimizerCVSBA.h>
 
 #ifdef RTABMAP_CVSBA
 #include <cvsba/cvsba.h>

corelib/src/optimizer/OptimizerG2O.cpp

@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <set>
 
 #include <rtabmap/core/Version.h>
-#include <rtabmap/core/OptimizerG2O.h>
+#include <rtabmap/core/optimizer/OptimizerG2O.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_motion_estimation.h>
 #include <rtabmap/core/util3d.h>

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <set>
 
-#include <rtabmap/core/OptimizerGTSAM.h>
+#include <rtabmap/core/optimizer/OptimizerGTSAM.h>
 
 #ifdef RTABMAP_GTSAM
 #include <gtsam/geometry/Pose2.h>

corelib/src/optimizer/OptimizerTORO.cpp

@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <set>
 
-#include <rtabmap/core/OptimizerTORO.h>
+#include <rtabmap/core/optimizer/OptimizerTORO.h>
 
 #ifdef RTABMAP_TORO
 #include "toro3d/treeoptimizer3.hh"

corelib/src/optimizer/vertigo/g2o/edge_se2Switchable.cpp

@@ -8,8 +8,8 @@
  *      Author: Christian Kerl <christian.kerl@in.tum.de>
  */
 
-#include "vertigo/g2o/edge_se2Switchable.h"
-#include "vertigo/g2o/vertex_switchLinear.h"
+#include "edge_se2Switchable.h"
+#include "vertex_switchLinear.h"
 
 using namespace std;
 using namespace Eigen;

corelib/src/optimizer/vertigo/g2o/edge_se3Switchable.cpp

@@ -8,8 +8,8 @@
  *      Author: Christian Kerl <christian.kerl@in.tum.de>
  */
 
-#include "vertigo/g2o/edge_se3Switchable.h"
-#include "vertigo/g2o/vertex_switchLinear.h"
+#include "edge_se3Switchable.h"
+#include "vertex_switchLinear.h"
 #include "g2o/types/slam3d/vertex_se3.h"
 #include "g2o/types/slam3d/isometry3d_gradients.h"
 

corelib/src/optimizer/vertigo/g2o/edge_switchPrior.cpp

@@ -1,4 +1,4 @@
-#include "vertigo/g2o/edge_switchPrior.h"
+#include "edge_switchPrior.h"
 using namespace std;
 
     EdgeSwitchPrior::EdgeSwitchPrior()

corelib/src/optimizer/vertigo/g2o/types_g2o_robust.cpp

@@ -1,11 +1,11 @@
 #include "g2o/core/factory.h"
 #include "g2o/stuff/macros.h"
 
-#include "vertigo/g2o/edge_switchPrior.h"
-#include "vertigo/g2o/edge_se2Switchable.h"
-//#include "vertigo/g2o/edge_se2MaxMixture.h"
-#include "vertigo/g2o/edge_se3Switchable.h"
-#include "vertigo/g2o/vertex_switchLinear.h"
+#include "edge_switchPrior.h"
+#include "edge_se2Switchable.h"
+//#include "edge_se2MaxMixture.h"
+#include "edge_se3Switchable.h"
+#include "vertex_switchLinear.h"
 
 
 G2O_REGISTER_TYPE(EDGE_SWITCH_PRIOR, EdgeSwitchPrior);

corelib/src/optimizer/vertigo/g2o/vertex_switchLinear.cpp

@@ -8,7 +8,7 @@
  *      Author: Christian Kerl <christian.kerl@in.tum.de>
  */
 
-#include "vertigo/g2o/vertex_switchLinear.h"
+#include "vertex_switchLinear.h"
 #include <iostream>
 
 using namespace std;