feat(slam): add rtabmap_ros

rtabmap/corelib/CMakeLists.txt

@@ -0,0 +1 @@
+ADD_SUBDIRECTORY( src )

rtabmap/corelib/include/rtabmap/core/BayesFilter.h

@@ -0,0 +1,87 @@
+/*
+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.
+*/
+
+#ifndef BAYESFILTER_H_
+#define BAYESFILTER_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+#include <list>
+#include <set>
+#include "rtabmap/utilite/UEventsHandler.h"
+#include "rtabmap/core/Parameters.h"
+
+namespace rtabmap {
+
+class Memory;
+class Signature;
+
+class RTABMAP_CORE_EXPORT BayesFilter
+{
+public:
+	BayesFilter(const ParametersMap & parameters = ParametersMap());
+	virtual ~BayesFilter();
+	virtual void parseParameters(const ParametersMap & parameters);
+	const std::map<int, float> & computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
+	void reset();
+
+	//setters
+	void setPredictionLC(const std::string & prediction);
+
+	//getters
+	const std::map<int, float> & getPosterior() const {return _posterior;}
+	float getVirtualPlacePrior() const {return _virtualPlacePrior;}
+	const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
+	std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
+
+	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);
+
+	unsigned long getMemoryUsed() const;
+
+private:
+	cv::Mat updatePrediction(const cv::Mat & oldPrediction,
+			const Memory * memory,
+			const std::vector<int> & oldIds,
+			const std::vector<int> & newIds);
+	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
+	void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
+
+private:
+	std::map<int, float> _posterior;
+	cv::Mat _prediction;
+	float _virtualPlacePrior;
+	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
+	bool _fullPredictionUpdate;
+	float _totalPredictionLCValues;
+	float _predictionEpsilon;
+	std::map<int, std::map<int, int> > _neighborsIndex;
+};
+
+} // namespace rtabmap
+
+#endif /* BAYESFILTER_H_ */

rtabmap/corelib/include/rtabmap/core/Camera.h

@@ -0,0 +1,79 @@
+/*
+Copyright (c) 2010-2022, 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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+#include <rtabmap/core/SensorCapture.h>
+#include <rtabmap/core/IMU.h>
+
+namespace rtabmap
+{
+
+class IMUFilter;
+
+/**
+ * Class Camera
+ *
+ */
+class RTABMAP_CORE_EXPORT Camera : public SensorCapture
+{
+public:
+	virtual ~Camera();
+
+	SensorData takeImage(SensorCaptureInfo * info = 0) {return takeData(info);}
+	float getImageRate() const {return getFrameRate();}
+	void setImageRate(float imageRate) {setFrameRate(imageRate);}
+	void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0); // Take ownership of filter
+	bool isInterIMUPublishing() const {return publishInterIMU_;}
+
+	bool initFromFile(const std::string & calibrationPath);
+	virtual bool isCalibrated() const = 0;
+
+protected:
+	/**
+	 * Constructor
+	 *
+	 * @param imageRate the frame rate (Hz), 0 for fast as the camera can
+	 * @param localTransform the transform from base frame to camera frame (without optical rotation)
+	 */
+	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
+
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0) = 0;
+
+	void postInterIMU(const IMU & imu, double stamp);
+
+private:
+	virtual SensorData captureData(SensorCaptureInfo * info = 0) {return captureImage(info);}
+
+private:
+	IMUFilter * imuFilter_;
+	bool publishInterIMU_;
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/CameraEvent.h

@@ -0,0 +1,30 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/SensorEvent.h"

rtabmap/corelib/include/rtabmap/core/CameraInfo.h

@@ -0,0 +1,30 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/SensorCaptureInfo.h"

rtabmap/corelib/include/rtabmap/core/CameraModel.h

@@ -0,0 +1,165 @@
+/*
+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.
+*/
+
+#ifndef CAMERAMODEL_H_
+#define CAMERAMODEL_H_
+
+#include <opencv2/opencv.hpp>
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+#include "rtabmap/core/Transform.h"
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT CameraModel
+{
+public:
+	/**
+	 * Optical rotation used to transform image coordinate frame (x->right, y->down, z->forward)
+	 * to robot coordinate frame (x->forward, y->left, z->up).
+	 */
+	static Transform opticalRotation() {return Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);}
+
+public:
+	CameraModel();
+	// K is the camera intrinsic 3x3 CV_64FC1
+	// D is the distortion coefficients 1x5 CV_64FC1
+	// R is the rectification matrix 3x3 CV_64FC1 (computed from stereo or Identity)
+	// P is the projection matrix 3x4 CV_64FC1 (computed from stereo or equal to [K [0 0 1]'])
+	CameraModel(
+			const std::string & name,
+			const cv::Size & imageSize,
+			const cv::Mat & K,
+			const cv::Mat & D,
+			const cv::Mat & R,
+			const cv::Mat & P,
+			const Transform & localTransform = opticalRotation());
+
+	// minimal
+	CameraModel(
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			const Transform & localTransform = opticalRotation(),
+			double Tx = 0.0f,
+			const cv::Size & imageSize = cv::Size(0,0));
+	// minimal to be saved
+	CameraModel(
+			const std::string & name,
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			const Transform & localTransform = opticalRotation(),
+			double Tx = 0.0f,
+			const cv::Size & imageSize = cv::Size(0,0));
+
+	virtual ~CameraModel() {}
+
+	bool initRectificationMap();
+	bool isRectificationMapInitialized() const {return !mapX_.empty() && !mapY_.empty();}
+
+	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}
+	bool isValidForReprojection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0 && imageWidth()>0 && imageHeight()>0;}
+	bool isValidForRectification() const
+	{
+		return imageSize_.width>0 &&
+			   imageSize_.height>0 &&
+			   !K_.empty() &&
+			   !D_.empty() &&
+			   !R_.empty() &&
+			   !P_.empty();
+	}
+
+	void setName(const std::string & name) {name_=name;}
+	const std::string & name() const {return name_;}
+
+	double fx() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(0,0):P_.at<double>(0,0);}
+	double fy() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(1,1):P_.at<double>(1,1);}
+	double cx() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(0,2):P_.at<double>(0,2);}
+	double cy() const {return P_.empty()?K_.empty()?0.0:K_.at<double>(1,2):P_.at<double>(1,2);}
+	double Tx() const {return P_.empty()?0.0:P_.at<double>(0,3);}
+
+	cv::Mat K_raw() const {return K_;} //intrinsic camera matrix (before rectification)
+	cv::Mat D_raw() const {return D_;} //intrinsic distorsion matrix (before rectification)
+	cv::Mat K() const {return !P_.empty()?P_.colRange(0,3):K_;} // if P exists, return rectified version
+	cv::Mat D() const {return P_.empty()&&!D_.empty()?D_:cv::Mat::zeros(1,5,CV_64FC1);} // if P exists, return rectified version
+	cv::Mat R() const {return R_;} //rectification matrix
+	cv::Mat P() const {return P_;} //projection matrix
+
+	void setLocalTransform(const Transform & transform) {localTransform_ = transform;}
+	const Transform & localTransform() const {return localTransform_;}
+
+	void setImageSize(const cv::Size & size);
+	const cv::Size & imageSize() const {return imageSize_;}
+	int imageWidth() const {return imageSize_.width;}
+	int imageHeight() const {return imageSize_.height;}
+
+	double fovX() const; // in radians
+	double fovY() const; // in radians
+	double horizontalFOV() const; // in degrees
+	double verticalFOV() const;   // in degrees
+	bool isFisheye() const {return D_.cols == 6;}
+
+	bool load(const std::string & filePath);
+	bool load(const std::string & directory, const std::string & cameraName);
+	bool save(const std::string & directory) const;
+	std::vector<unsigned char> serialize() const;
+	unsigned int deserialize(const std::vector<unsigned char>& data);
+	unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
+
+	CameraModel scaled(double scale) const;
+	CameraModel roi(const cv::Rect & roi) const;
+
+	// For depth images, your should use cv::INTER_NEAREST
+	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
+	cv::Mat rectifyDepth(const cv::Mat & raw) const;
+
+	// Project 2D pixel to 3D (in /camera_link frame)
+	void project(float u, float v, float depth, float & x, float & y, float & z) const;
+	// Reproject 3D point (in /camera_link frame) to pixel
+	void reproject(float x, float y, float z, float & u, float & v) const;
+	void reproject(float x, float y, float z, int & u, int & v) const;
+	bool inFrame(int u, int v) const;
+
+private:
+	std::string name_;
+	cv::Size imageSize_;
+	cv::Mat K_;
+	cv::Mat D_;
+	cv::Mat R_;
+	cv::Mat P_;
+	cv::Mat mapX_;
+	cv::Mat mapY_;
+	Transform localTransform_;
+};
+
+RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const CameraModel& model);
+
+} /* namespace rtabmap */
+#endif /* CAMERAMODEL_H_ */

rtabmap/corelib/include/rtabmap/core/CameraRGB.h

@@ -0,0 +1,31 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include <rtabmap/core/camera/CameraImages.h>
+#include <rtabmap/core/camera/CameraVideo.h>

rtabmap/corelib/include/rtabmap/core/CameraRGBD.h

@@ -0,0 +1,40 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include <rtabmap/core/camera/CameraFreenect.h>
+#include <rtabmap/core/camera/CameraFreenect2.h>
+#include <rtabmap/core/camera/CameraK4W2.h>
+#include <rtabmap/core/camera/CameraOpenni.h>
+#include <rtabmap/core/camera/CameraOpenNI2.h>
+#include <rtabmap/core/camera/CameraOpenNICV.h>
+#include <rtabmap/core/camera/CameraRealSense.h>
+#include <rtabmap/core/camera/CameraRealSense2.h>
+#include <rtabmap/core/camera/CameraRGBDImages.h>
+#include <rtabmap/core/camera/CameraK4A.h>
+#include <rtabmap/core/camera/CameraSeerSense.h>

rtabmap/corelib/include/rtabmap/core/CameraStereo.h

@@ -0,0 +1,38 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include <rtabmap/core/camera/CameraStereoDC1394.h>
+#include <rtabmap/core/camera/CameraStereoFlyCapture2.h>
+#include <rtabmap/core/camera/CameraStereoImages.h>
+#include <rtabmap/core/camera/CameraStereoVideo.h>
+#include <rtabmap/core/camera/CameraStereoZed.h>
+#include <rtabmap/core/camera/CameraStereoZedOC.h>
+#include <rtabmap/core/camera/CameraStereoTara.h>
+#include <rtabmap/core/camera/CameraMyntEye.h>
+#include <rtabmap/core/camera/CameraDepthAI.h>

rtabmap/corelib/include/rtabmap/core/CameraThread.h

@@ -0,0 +1,30 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/SensorCaptureThread.h"

rtabmap/corelib/include/rtabmap/core/Compression.h

@@ -0,0 +1,95 @@
+/*
+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.
+*/
+
+#ifndef COMPRESSION_H_
+#define COMPRESSION_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/rvl_codec.h>
+#include <rtabmap/utilite/UThread.h>
+#include <opencv2/opencv.hpp>
+
+namespace rtabmap {
+
+/**
+ * Compress image or data
+ *
+ * Example compression:
+ *   cv::Mat image;// an image
+ *   CompressionThread ct(image);
+ *   ct.start();
+ *   ct.join();
+ *   std::vector<unsigned char> bytes = ct.getCompressedData();
+ *
+ * Example uncompression
+ *   std::vector<unsigned char> bytes;// a compressed image
+ *   CompressionThread ct(bytes);
+ *   ct.start();
+ *   ct.join();
+ *   cv::Mat image = ct.getUncompressedData();
+ */
+class RTABMAP_CORE_EXPORT CompressionThread : public UThread
+{
+public:
+	// format : ".png" ".jpg" "" (empty is general)
+	CompressionThread(const cv::Mat & mat, const std::string & format = "");
+	CompressionThread(const cv::Mat & bytes, bool isImage);
+	const cv::Mat & getCompressedData() const {return compressedData_;}
+	cv::Mat & getUncompressedData() {return uncompressedData_;}
+protected:
+	virtual void mainLoop();
+private:
+	cv::Mat compressedData_;
+	cv::Mat uncompressedData_;
+	std::string format_;
+	bool image_;
+	bool compressMode_;
+};
+
+std::vector<unsigned char> RTABMAP_CORE_EXPORT compressImage(const cv::Mat & image, const std::string & format = ".png");
+cv::Mat RTABMAP_CORE_EXPORT compressImage2(const cv::Mat & image, const std::string & format = ".png");
+
+cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const cv::Mat & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const std::vector<unsigned char> & bytes);
+
+std::vector<unsigned char> RTABMAP_CORE_EXPORT compressData(const cv::Mat & data);
+cv::Mat RTABMAP_CORE_EXPORT compressData2(const cv::Mat & data);
+
+cv::Mat RTABMAP_CORE_EXPORT uncompressData(const cv::Mat & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressData(const std::vector<unsigned char> & bytes);
+cv::Mat RTABMAP_CORE_EXPORT uncompressData(const unsigned char * bytes, unsigned long size);
+
+cv::Mat RTABMAP_CORE_EXPORT compressString(const std::string & str);
+std::string RTABMAP_CORE_EXPORT uncompressString(const cv::Mat & bytes);
+
+std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const cv::Mat & bytes);
+std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const std::vector<unsigned char> & bytes);
+std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const unsigned char * bytes, size_t size);
+
+} /* namespace rtabmap */
+#endif /* COMPRESSION_H_ */

rtabmap/corelib/include/rtabmap/core/DBDriver.h

@@ -0,0 +1,322 @@
+/*
+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.
+*/
+
+#ifndef DBDRIVER_H_
+#define DBDRIVER_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <string>
+#include <list>
+#include <map>
+#include <set>
+#include <opencv2/core/core.hpp>
+#include "rtabmap/utilite/UMutex.h"
+#include "rtabmap/utilite/UThreadNode.h"
+#include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/SensorData.h"
+#include <rtabmap/core/Statistics.h>
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Link.h>
+
+namespace rtabmap {
+
+class Signature;
+class VWDictionary;
+class VisualWord;
+
+// Todo This class needs a refactoring, the _dbSafeAccessMutex problem when the trash is emptying (transaction)
+// "Of course, it has always been the case and probably always will be
+//that you cannot use the same sqlite3 connection in two or more
+//threads at the same time.  You can use different sqlite3 connections
+//at the same time in different threads, or you can move the same
+//sqlite3 connection across threads (subject to the constraints above)
+//but never, never try to use the same connection simultaneously in
+//two or more threads."
+//
+class RTABMAP_CORE_EXPORT DBDriver : public UThreadNode
+{
+public:
+	static DBDriver * create(const ParametersMap & parameters = ParametersMap());
+
+public:
+	virtual ~DBDriver();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual bool isInMemory() const {return _url.empty();}
+	const std::string & getUrl() const {return _url;}
+	const std::string & getTargetVersion() const {return _targetVersion;}
+
+	void beginTransaction() const;
+	void commit() const;
+
+	void asyncSave(Signature * s); //ownership transferred
+	void asyncSave(VisualWord * vw); //ownership transferred
+	void emptyTrashes(bool async = false);
+	double getEmptyTrashesTime() const {return _emptyTrashesTime;}
+	void setTimestampUpdateEnabled(bool enabled) {_timestampUpdate = enabled;} // used on Update Signature and Word queries
+
+	// Warning: the following functions don't look in the trash, direct database modifications
+	void generateGraph(
+			const std::string & fileName,
+			const std::set<int> & ids = std::set<int>(),
+			const std::map<int, Signature *> & otherSignatures = std::map<int, Signature *>());
+	void addLink(const Link & link);
+	void removeLink(int from, int to);
+	void updateLink(const Link & link);
+	void updateOccupancyGrid(
+				int nodeId,
+				const cv::Mat & ground,
+				const cv::Mat & obstacles,
+				const cv::Mat & empty,
+				float cellSize,
+				const cv::Point3f & viewpoint);
+	void updateCalibration(
+		int nodeId,
+		const std::vector<CameraModel> & models,
+		const std::vector<StereoCameraModel> & stereoModels);
+	void updateDepthImage(int nodeId, const cv::Mat & image, const std::string & format);
+	void updateLaserScan(int nodeId, const LaserScan & scan);
+
+public:
+	void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
+	void addStatistics(const Statistics & statistics, bool saveWmState) const;
+	void savePreviewImage(const cv::Mat & image) const;
+	cv::Mat loadPreviewImage() const;
+	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose = 0) const;
+	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
+	void saveOptimizedMesh(
+			const cv::Mat & cloud,
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons = std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > >(),      // Textures -> polygons -> vertices
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
+#endif
+			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
+	cv::Mat loadOptimizedMesh(
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons = 0,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
+#endif
+			cv::Mat * textures = 0) const;
+
+public:
+	// Mutex-protected methods of abstract versions below
+
+	bool openConnection(const std::string & url, bool overwritten = false);
+	void closeConnection(bool save = true, const std::string & outputUrl = "");
+	bool isConnected() const;
+	unsigned long getMemoryUsed() const; // In bytes
+	std::string getDatabaseVersion() const;
+	long getNodesMemoryUsed() const;
+	long getLinksMemoryUsed() const;
+	long getImagesMemoryUsed() const;
+	long getDepthImagesMemoryUsed() const;
+	long getCalibrationsMemoryUsed() const;
+	long getGridsMemoryUsed() const;
+	long getLaserScansMemoryUsed() const;
+	long getUserDataMemoryUsed() const;
+	long getWordsMemoryUsed() const;
+	long getFeaturesMemoryUsed() const;
+	long getStatisticsMemoryUsed() const;
+	int getLastNodesSize() const; // working memory
+	int getLastDictionarySize() const; // working memory
+	int getTotalNodesSize() const;
+	int getTotalDictionarySize() const;
+	ParametersMap getLastParameters() const;
+	std::map<std::string, float> getStatistics(int nodeId, double & stamp, std::vector<int> * wmState=0) const;
+	std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatistics() const;
+	std::map<int, std::vector<int> > getAllStatisticsWmStates() const;
+
+	void executeNoResult(const std::string & sql) const;
+
+	// Load objects
+	void load(VWDictionary * dictionary, bool lastStateOnly = true) const;
+	void loadLastNodes(std::list<Signature *> & signatures) const; // returned signatures must be freed after usage
+	Signature * loadSignature(int id, bool * loadedFromTrash = 0); // returned signature must be freed after usage, call loadSignatures() instead if more than one signature should be loaded
+	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0); // returned signatures must be freed after usage
+	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws); // returned words must be freed after usage
+
+	// Specific queries...
+	void loadNodeData(Signature * signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
+	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
+	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
+	bool getCalibration(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const;
+	bool getLaserScanInfo(int signatureId, LaserScan & info) const;
+	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
+	void loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
+	void getWeight(int signatureId, int & weight) const;
+	void getLastNodeIds(std::set<int> & ids) const;
+	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false, bool ignoreIntermediateNodes = false) const;
+	void getAllOdomPoses(std::map<int, Transform> & poses, bool ignoreChildren = false, bool ignoreIntermediateNodes = false) const;
+	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true, bool withLandmarks = false) const;
+	void getLastNodeId(int & id) const;
+	void getLastMapId(int & mapId) const;
+	void getLastWordId(int & id) const;
+	void getInvertedIndexNi(int signatureId, int & ni) const;
+	void getNodesObservingLandmark(int landmarkId, std::map<int, Link> & nodes) const;
+	void getNodeIdByLabel(const std::string & label, int & id) const;
+	void getAllLabels(std::map<int, std::string> & labels) const;
+
+protected:
+	DBDriver(const ParametersMap & parameters = ParametersMap());
+
+	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
+	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
+	virtual bool isConnectedQuery() const = 0;
+	virtual unsigned long getMemoryUsedQuery() const = 0; // In bytes
+	virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
+	virtual long getNodesMemoryUsedQuery() const = 0;
+	virtual long getLinksMemoryUsedQuery() const = 0;
+	virtual long getImagesMemoryUsedQuery() const = 0;
+	virtual long getDepthImagesMemoryUsedQuery() const = 0;
+	virtual long getCalibrationsMemoryUsedQuery() const = 0;
+	virtual long getGridsMemoryUsedQuery() const = 0;
+	virtual long getLaserScansMemoryUsedQuery() const = 0;
+	virtual long getUserDataMemoryUsedQuery() const = 0;
+	virtual long getWordsMemoryUsedQuery() const = 0;
+	virtual long getFeaturesMemoryUsedQuery() const = 0;
+	virtual long getStatisticsMemoryUsedQuery() const = 0;
+	virtual int getLastNodesSizeQuery() const = 0;
+	virtual int getLastDictionarySizeQuery() const = 0;
+	virtual int getTotalNodesSizeQuery() const = 0;
+	virtual int getTotalDictionarySizeQuery() const = 0;
+	virtual ParametersMap getLastParametersQuery() const = 0;
+	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const = 0;
+	virtual std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatisticsQuery() const = 0;
+	virtual std::map<int, std::vector<int> > getAllStatisticsWmStatesQuery() const = 0;
+
+	virtual void executeNoResultQuery(const std::string & sql) const = 0;
+
+	virtual void getWeightQuery(int signatureId, int & weight) const = 0;
+
+	virtual void saveQuery(const std::list<Signature *> & signatures) = 0;
+	virtual void saveQuery(const std::list<VisualWord *> & words) const = 0;
+	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const = 0;
+	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const = 0;
+
+	virtual void addLinkQuery(const Link & link) const = 0;
+	virtual void updateLinkQuery(const Link & link) const = 0;
+
+	virtual void updateOccupancyGridQuery(
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Mat & empty,
+			float cellSize,
+			const cv::Point3f & viewpoint) const = 0;
+
+	virtual void updateCalibrationQuery(
+			int nodeId,
+			const std::vector<CameraModel> & models,
+			const std::vector<StereoCameraModel> & stereoModels) const = 0;
+
+	virtual void updateDepthImageQuery(
+			int nodeId,
+			const cv::Mat & image,
+			const std::string & format) const = 0;
+
+	virtual void updateLaserScanQuery(
+			int nodeId,
+			const LaserScan & scan) const = 0;
+
+	virtual void addStatisticsQuery(const Statistics & statistics, bool saveWmState) const = 0;
+	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
+	virtual cv::Mat loadPreviewImageQuery() const = 0;
+	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const = 0;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose = 0) const = 0;
+	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const = 0;
+	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const = 0;
+	virtual void saveOptimizedMeshQuery(
+				const cv::Mat & cloud,
+				const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+				const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
+				const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
+				const cv::Mat & textures) const = 0;
+	virtual cv::Mat loadOptimizedMeshQuery(
+				std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
+#else
+				std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
+				cv::Mat * textures) const = 0;
+
+	// Load objects
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const = 0;
+	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
+	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
+	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
+	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
+
+	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
+	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const = 0;
+	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
+	virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;
+	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const = 0;
+	virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const = 0;
+	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const = 0;
+	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const = 0;
+	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
+	virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const = 0;
+	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;
+	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const = 0;
+
+private:
+	//non-abstract methods
+	void saveOrUpdate(const std::vector<Signature *> & signatures);
+	void saveOrUpdate(const std::vector<VisualWord *> & words) const;
+
+	//thread stuff
+	virtual void mainLoop();
+
+private:
+	UMutex _transactionMutex;
+	std::map<int, Signature *> _trashSignatures;//<id, Signature*>
+	std::map<int, VisualWord *> _trashVisualWords; //<id, VisualWord*>
+	UMutex _trashesMutex;
+	UMutex _dbSafeAccessMutex;
+	USemaphore _addSem;
+	double _emptyTrashesTime;
+	std::string _url;
+	std::string _targetVersion;
+	bool _timestampUpdate;
+};
+
+}
+
+#endif /* DBDRIVER_H_ */

rtabmap/corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -0,0 +1,211 @@
+/*
+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.
+*/
+
+#ifndef DBDRIVERSQLITE3_H_
+#define DBDRIVERSQLITE3_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+#include "rtabmap/core/DBDriver.h"
+#include <opencv2/features2d/features2d.hpp>
+
+typedef struct sqlite3_stmt sqlite3_stmt;
+typedef struct sqlite3 sqlite3;
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver {
+public:
+	DBDriverSqlite3(const ParametersMap & parameters = ParametersMap());
+	virtual ~DBDriverSqlite3();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual bool isInMemory() const {return getUrl().empty() || _dbInMemory;}
+	void setDbInMemory(bool dbInMemory);
+	void setJournalMode(int journalMode);
+	void setCacheSize(unsigned int cacheSize);
+	void setSynchronous(int synchronous);
+	void setTempStore(int tempStore);
+
+protected:
+	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
+	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
+	virtual bool isConnectedQuery() const;
+	virtual unsigned long getMemoryUsedQuery() const; // In bytes
+	virtual bool getDatabaseVersionQuery(std::string & version) const;
+	virtual long getNodesMemoryUsedQuery() const;
+	virtual long getLinksMemoryUsedQuery() const;
+	virtual long getImagesMemoryUsedQuery() const;
+	virtual long getDepthImagesMemoryUsedQuery() const;
+	virtual long getCalibrationsMemoryUsedQuery() const;
+	virtual long getGridsMemoryUsedQuery() const;
+	virtual long getLaserScansMemoryUsedQuery() const;
+	virtual long getUserDataMemoryUsedQuery() const;
+	virtual long getWordsMemoryUsedQuery() const;
+	virtual long getFeaturesMemoryUsedQuery() const;
+	virtual long getStatisticsMemoryUsedQuery() const;
+	virtual int getLastNodesSizeQuery() const;
+	virtual int getLastDictionarySizeQuery() const;
+	virtual int getTotalNodesSizeQuery() const;
+	virtual int getTotalDictionarySizeQuery() const;
+	virtual ParametersMap getLastParametersQuery() const;
+	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const;
+	virtual std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatisticsQuery() const;
+	virtual std::map<int, std::vector<int> > getAllStatisticsWmStatesQuery() const;
+
+	virtual void executeNoResultQuery(const std::string & sql) const;
+
+	virtual void getWeightQuery(int signatureId, int & weight) const;
+
+	virtual void saveQuery(const std::list<Signature *> & signatures);
+	virtual void saveQuery(const std::list<VisualWord *> & words) const;
+	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const;
+	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const;
+
+	virtual void addLinkQuery(const Link & link) const;
+	virtual void updateLinkQuery(const Link & link) const;
+
+	virtual void updateOccupancyGridQuery(
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Mat & empty,
+			float cellSize,
+			const cv::Point3f & viewpoint) const;
+
+	virtual void updateCalibrationQuery(
+			int nodeId,
+			const std::vector<CameraModel> & models,
+			const std::vector<StereoCameraModel> & stereoModels) const;
+
+	virtual void updateDepthImageQuery(
+			int nodeId,
+			const cv::Mat & image,
+			const std::string & format) const;
+
+	void updateLaserScanQuery(
+			int nodeId,
+			const LaserScan & scan) const;
+
+	virtual void addStatisticsQuery(const Statistics & statistics, bool saveWmState) const;
+	virtual void savePreviewImageQuery(const cv::Mat & image) const;
+	virtual cv::Mat loadPreviewImageQuery() const;
+	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose = 0) const;
+	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const;
+	virtual void saveOptimizedMeshQuery(
+			const cv::Mat & cloud,
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
+			const cv::Mat & textures) const;
+	virtual cv::Mat loadOptimizedMeshQuery(
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
+			cv::Mat * textures) const;
+
+	// Load objects
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const;
+	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
+	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
+	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
+	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
+
+	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
+	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const;
+	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
+	virtual void getLastNodeIdsQuery(std::set<int> & ids) const;
+	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) const;
+	virtual void getAllOdomPosesQuery(std::map<int, Transform> & poses, bool ignoreChildren, bool ignoreIntermediateNodes) const;
+	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const;
+	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const;
+	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
+	virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const;
+	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const;
+	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const;
+
+private:
+	std::string queryStepNode() const;
+	std::string queryStepImage() const;
+	std::string queryStepDepth() const;
+	std::string queryStepCalibrationUpdate() const;
+	std::string queryStepDepthUpdate() const;
+	std::string queryStepScanUpdate() const;
+	std::string queryStepSensorData() const;
+	std::string queryStepLinkUpdate() const;
+	std::string queryStepLink() const;
+	std::string queryStepWordsChanged() const;
+	std::string queryStepKeypoint() const;
+	std::string queryStepGlobalDescriptor() const;
+	std::string queryStepOccupancyGridUpdate() const;
+	void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
+	void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
+	void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
+	void stepCalibrationUpdate(sqlite3_stmt * ppStmt, int nodeId, const std::vector<CameraModel> & models, const std::vector<StereoCameraModel> & stereoModels) const;
+	void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image, const std::string & format) const;
+	void stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & image) const;
+	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
+	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
+	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
+	void stepKeypoint(sqlite3_stmt * ppStmt, int nodeID, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepGlobalDescriptor(sqlite3_stmt * ppStmt, int nodeId, const GlobalDescriptor & descriptor) const;
+	void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Mat & empty,
+			float cellSize,
+			const cv::Point3f & viewpoint) const;
+
+private:
+	void loadLinksQuery(std::list<Signature *> & signatures) const;
+	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
+
+protected:
+	sqlite3 * _ppDb;
+	std::string _version;
+
+private:
+	unsigned long _memoryUsedEstimate;
+	bool _dbInMemory;
+	unsigned int _cacheSize;
+	int _journalMode;
+	int _synchronous;
+	int _tempStore;
+};
+
+}
+
+#endif /* DBDRIVERSQLITE3_H_ */

rtabmap/corelib/include/rtabmap/core/DBReader.h

@@ -0,0 +1,126 @@
+/*
+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.
+*/
+
+#ifndef DBREADER_H_
+#define DBREADER_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Camera.h>
+
+#include <opencv2/core/core.hpp>
+
+#include <set>
+#include <list>
+
+namespace rtabmap {
+
+class DBDriver;
+
+class RTABMAP_CORE_EXPORT DBReader : public Camera {
+public:
+	DBReader(const std::string & databasePath,
+			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
+			 bool odometryIgnored = false,
+			 bool ignoreGoalDelay = false,
+			 bool goalsIgnored = false,
+			 int startId = 0,
+			 const std::vector<unsigned int> & cameraIndices = std::vector<unsigned int>(),
+			 int stopId = 0,
+			 bool intermediateNodesIgnored = false,
+			 bool landmarksIgnored = false,
+			 bool featuresIgnored = false,
+			 int startMapId = 0,
+			 int stopMapId = -1,
+			 bool priorsIgnored = false,
+			 const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
+	DBReader(const std::list<std::string> & databasePaths,
+			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
+			 bool odometryIgnored = false,
+			 bool ignoreGoalDelay = false,
+			 bool goalsIgnored = false,
+			 int startId = 0,
+			 const std::vector<unsigned int> & cameraIndices = std::vector<unsigned int>(),
+			 int stopId = 0,
+			 bool intermediateNodesIgnored = false,
+			 bool landmarksIgnored = false,
+			 bool featuresIgnored = false,
+			 int startMapId = 0,
+			 int stopMapId = -1,
+			 bool priorsIgnored = false,
+			 const std::vector<Transform> & cameraLocalTransformOverrides = std::vector<Transform>());
+	virtual ~DBReader();
+
+	virtual bool init(
+			const std::string & calibrationFolder = ".",
+			const std::string & cameraName = "");
+
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	virtual bool odomProvided() const {return !_odometryIgnored;}
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06);
+
+	const DBDriver * driver() const {return _dbDriver;}
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+	SensorData getNextData(SensorCaptureInfo * info = 0);
+	void checkArguments();
+
+private:
+	std::list<std::string> _paths;
+	bool _odometryIgnored;
+	bool _ignoreGoalDelay;
+	bool _goalsIgnored;
+	int _startId;
+	int _stopId;
+	std::vector<unsigned int> _cameraIndices;
+	bool _intermediateNodesIgnored;
+	bool _landmarksIgnored;
+	bool _featuresIgnored;
+	bool _priorsIgnored;
+	int _startMapId;
+	int _stopMapId;
+	std::vector<Transform> _cameraLocalTransformOverrides;
+
+	DBDriver * _dbDriver;
+	UTimer _timer;
+	std::set<int> _ids;
+	std::set<int>::iterator _currentId;
+	int _previousMapId;
+	cv::Mat _previousInfMatrix;
+	double _previousStamp;
+	int _previousMapID;
+	bool _calibrated;
+};
+
+} /* namespace rtabmap */
+#endif /* DBREADER_H_ */

rtabmap/corelib/include/rtabmap/core/EnvSensor.h

@@ -0,0 +1,85 @@
+/*
+Copyright (c) 2010-2018, 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.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_
+
+namespace rtabmap {
+
+class EnvSensor
+{
+public:
+	enum Type {
+		// built-in types
+		kUndefined = 0,
+		kWifiSignalStrength,      // dBm
+		kAmbientTemperature,      // Celcius
+		kAmbientAirPressure,      // hPa
+		kAmbientLight,            // lx
+		kAmbientRelativeHumidity, // %
+
+		// user types
+		kCustomSensor1 = 100,
+		kCustomSensor2,
+		kCustomSensor3,
+		kCustomSensor4,
+		kCustomSensor5,
+		kCustomSensor6,
+		kCustomSensor7,
+		kCustomSensor8,
+		kCustomSensor9
+	};
+
+public:
+	EnvSensor() :
+		type_(kUndefined),
+		value_(0.0),
+		stamp_(0.0)
+	{}
+	EnvSensor(const Type & type, const double & value,const double & stamp = 0) :
+		type_(type),
+		value_(value),
+		stamp_(stamp)
+	{}
+
+	virtual ~EnvSensor() {}
+
+	const Type & type() const {return type_;}
+	const double & value() const {return value_;}
+	const double & stamp() const {return stamp_;}
+
+private:
+	Type type_;
+	double value_;
+	double stamp_;
+};
+
+typedef std::map<EnvSensor::Type, EnvSensor> EnvSensors;
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_ */

rtabmap/corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -0,0 +1,249 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+#include "rtabmap/core/Parameters.h"
+#include "rtabmap/utilite/UStl.h"
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <list>
+#include <vector>
+
+namespace rtabmap
+{
+
+class Signature;
+
+class RTABMAP_CORE_EXPORT EpipolarGeometry
+{
+public:
+	EpipolarGeometry(const ParametersMap & parameters = ParametersMap());
+	virtual ~EpipolarGeometry();
+	bool check(const Signature * ssA, const Signature * ssB);
+	void parseParameters(const ParametersMap & parameters);
+
+	int getMatchCountMinAccepted() const {return _matchCountMinAccepted;}
+	double getRansacParam1() const {return _ransacParam1;}
+	double getRansacParam2() const {return _ransacParam2;}
+
+	void setMatchCountMinAccepted(int matchCountMinAccepted) {_matchCountMinAccepted = matchCountMinAccepted;}
+	void setRansacParam1(double ransacParam1) {_ransacParam1 = ransacParam1;}
+	void setRansacParam2(double ransacParam2) {_ransacParam2 = ransacParam2;}
+
+
+	// STATIC STUFF
+	//epipolar geometry
+	static void findEpipolesFromF(
+			const cv::Mat & fundamentalMatrix,
+			cv::Vec3d & e1,
+			cv::Vec3d & e2);
+
+	static cv::Mat findPFromE(
+			const cv::Mat & E,
+			const cv::Mat & x,
+			const cv::Mat & xp);
+
+	// return fundamental matrix
+	// status -> inliers = 1, outliers = 0
+	static cv::Mat findFFromWords(
+			const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
+			std::vector<uchar> & status,
+			double ransacReprojThreshold = 3.0,
+			double ransacConfidence = 0.99);
+
+	// assume a canonical camera (without K)
+	static void findRTFromP(
+			const cv::Mat & p,
+			cv::Mat & r,
+			cv::Mat & t);
+
+	static cv::Mat findFFromCalibratedStereoCameras(double fx, double fy, double cx, double cy, double Tx, double Ty);
+
+
+	/**
+	 * if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
+	 * realPairsCount = 4
+	 */
+	template<typename T>
+	static int findPairs(
+			const std::map<int, T> & wordsA,
+			const std::map<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		int realPairsCount = 0;
+		pairs.clear();
+		for(typename std::map<int, T>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && i->first>=0))
+			{
+				std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
+				if(ptB != wordsB.end())
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(i->first, std::make_pair(i->second, ptB->second)));
+					++realPairsCount;
+				}
+			}
+		}
+		return realPairsCount;
+	}
+
+	/**
+	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
+	 * realPairsCount = 5
+	 */
+	template<typename T>
+	static int findPairs(
+			const std::multimap<int, T> & wordsA,
+			const std::multimap<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		typename std::multimap<int, T>::const_iterator iterA;
+		typename std::multimap<int, T>::const_iterator iterB;
+		pairs.clear();
+		int realPairsCount = 0;
+		for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *i >= 0))
+			{
+				iterA = wordsA.find(*i);
+				iterB = wordsB.find(*i);
+				while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::make_pair((*iterA).second, (*iterB).second)));
+					++iterA;
+					++iterB;
+					++realPairsCount;
+				}
+			}
+		}
+		return realPairsCount;
+	}
+
+	/**
+	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
+	 * realPairsCount = 5
+	 */
+	template<typename T>
+	static int findPairsUnique(
+			const std::multimap<int, T> & wordsA,
+			const std::multimap<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		int realPairsCount = 0;
+		pairs.clear();
+		for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *i>=0))
+			{
+				std::list<T> ptsA = uValues(wordsA, *i);
+				std::list<T> ptsB = uValues(wordsB, *i);
+				if(ptsA.size() == 1 && ptsB.size() == 1)
+				{
+					pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::pair<T, T>(ptsA.front(), ptsB.front())));
+					++realPairsCount;
+				}
+				else if(ptsA.size()>1 && ptsB.size()>1)
+				{
+					// just update the count
+					realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+				}
+			}
+		}
+		return realPairsCount;
+	}
+
+	/**
+	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
+	 * realPairsCount = 5
+	 */
+	template<typename T>
+	static int findPairsAll(
+			const std::multimap<int, T> & wordsA,
+			const std::multimap<int, T> & wordsB,
+			std::list<std::pair<int, std::pair<T, T> > > & pairs,
+			bool ignoreNegativeIds = true)
+	{
+		const std::list<int> & ids = uUniqueKeys(wordsA);
+		pairs.clear();
+		int realPairsCount = 0;;
+		for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+		{
+			if(!ignoreNegativeIds || (ignoreNegativeIds && *iter>=0))
+			{
+				std::list<T> ptsA = uValues(wordsA, *iter);
+				std::list<T> ptsB = uValues(wordsB, *iter);
+
+				realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+
+				for(typename std::list<T>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
+				{
+					for(typename std::list<T>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
+					{
+						pairs.push_back(std::pair<int, std::pair<T, T> >(*iter, std::pair<T, T>(*jter, *kter)));
+					}
+				}
+			}
+		}
+		return realPairsCount;
+	}
+
+	static cv::Mat linearLSTriangulation(
+			cv::Point3d u,    //homogenous image point (u,v,1)
+			cv::Matx34d P,        //camera 1 matrix 3x4 double
+			cv::Point3d u1,   //homogenous image point in 2nd camera
+			cv::Matx34d P1);       //camera 2 matrix 3x4 double
+
+	static cv::Mat iterativeLinearLSTriangulation(
+			cv::Point3d u,        //homogenous image point (u,v,1)
+			const cv::Matx34d & P,    //camera 1 matrix 3x4 double
+			cv::Point3d u1,        //homogenous image point in 2nd camera
+			const cv::Matx34d & P1);  //camera 2 matrix 3x4 double
+
+	static double triangulatePoints(
+			const cv::Mat& pt_set1, //2xN double
+			const cv::Mat& pt_set2, //2xN double
+			const cv::Mat& P, // 3x4 double
+			const cv::Mat& P1, // 3x4 double
+			pcl::PointCloud<pcl::PointXYZ>::Ptr & pointcloud,
+			std::vector<double> & reproj_errors);
+
+private:
+	int _matchCountMinAccepted;
+	double _ransacParam1;
+	double _ransacParam2;
+};
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/Features2d.h

@@ -0,0 +1,679 @@
+/*
+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.
+*/
+
+#ifndef FEATURES2D_H_
+#define FEATURES2D_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <list>
+#include <numeric>
+#include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/SensorData.h"
+
+#if CV_MAJOR_VERSION < 3
+namespace cv{
+class SURF;
+class SIFT;
+namespace gpu {
+	class SURF_GPU;
+	class ORB_GPU;
+	class FAST_GPU;
+	class GoodFeaturesToTrackDetector_GPU;
+}
+}
+typedef cv::SIFT CV_SIFT;
+typedef cv::SURF CV_SURF;
+typedef cv::FastFeatureDetector CV_FAST;
+typedef cv::FREAK CV_FREAK;
+typedef cv::GFTTDetector CV_GFTT;
+typedef cv::BriefDescriptorExtractor CV_BRIEF;
+typedef cv::BRISK CV_BRISK;
+typedef cv::gpu::SURF_GPU CV_SURF_GPU;
+typedef cv::gpu::ORB_GPU CV_ORB_GPU;
+typedef cv::gpu::FAST_GPU CV_FAST_GPU;
+typedef cv::gpu::GoodFeaturesToTrackDetector_GPU CV_GFTT_GPU;
+#else
+namespace cv{
+namespace xfeatures2d {
+class FREAK;
+class DAISY;
+class BriefDescriptorExtractor;
+#if (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
+class SIFT;
+#endif
+class SURF;
+}
+namespace cuda {
+class FastFeatureDetector;
+class ORB;
+class SURF_CUDA;
+class CornersDetector;
+}
+}
+#if (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
+typedef cv::xfeatures2d::SIFT CV_SIFT;
+#else
+typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.4.0 / 3.4.11
+#endif
+typedef cv::xfeatures2d::SURF CV_SURF;
+typedef cv::FastFeatureDetector CV_FAST;
+typedef cv::xfeatures2d::FREAK CV_FREAK;
+typedef cv::xfeatures2d::DAISY CV_DAISY;
+typedef cv::GFTTDetector CV_GFTT;
+typedef cv::xfeatures2d::BriefDescriptorExtractor CV_BRIEF;
+typedef cv::BRISK CV_BRISK;
+typedef cv::ORB CV_ORB;
+typedef cv::cuda::SURF_CUDA CV_SURF_GPU;
+typedef cv::cuda::ORB CV_ORB_GPU;
+typedef cv::cuda::FastFeatureDetector CV_FAST_GPU;
+typedef cv::cuda::CornersDetector CV_GFTT_GPU;
+#endif
+
+// CudaSift fork: https://github.com/matlabbe/CudaSift
+class SiftData;
+
+namespace rtabmap {
+
+class ORBextractor;
+class SPDetector;
+
+class Stereo;
+#if CV_MAJOR_VERSION < 3
+class CV_ORB;
+#endif
+
+// Feature2D
+class RTABMAP_CORE_EXPORT Feature2D {
+public:
+	enum Type {kFeatureUndef=-1,
+		kFeatureSurf=0,
+		kFeatureSift=1,
+		kFeatureOrb=2,
+		kFeatureFastFreak=3,
+		kFeatureFastBrief=4,
+		kFeatureGfttFreak=5,
+		kFeatureGfttBrief=6,
+		kFeatureBrisk=7,
+		kFeatureGfttOrb=8,  //new 0.10.11
+		kFeatureKaze=9,     //new 0.13.2
+		kFeatureOrbOctree=10, //new 0.19.2
+		kFeatureSuperPointTorch=11, //new 0.19.7
+		kFeatureSurfFreak=12, //new 0.20.4
+		kFeatureGfttDaisy=13, //new 0.20.6
+		kFeatureSurfDaisy=14,  //new 0.20.6
+		kFeaturePyDetector=15}; //new 0.20.8
+
+	static std::string typeName(Type type)
+	{
+		switch(type){
+		case kFeatureSurf:
+			return "SURF";
+		case kFeatureSift:
+			return "SIFT";
+		case kFeatureOrb:
+			return "ORB";
+		case kFeatureFastFreak:
+			return "FAST+FREAK";
+		case kFeatureFastBrief:
+			return "FAST+BRIEF";
+		case kFeatureGfttFreak:
+			return "GFTT+Freak";
+		case kFeatureGfttBrief:
+			return "GFTT+Brief";
+		case kFeatureBrisk:
+			return "BRISK";
+		case kFeatureGfttOrb:
+			return "GFTT+ORB";
+		case kFeatureKaze:
+			return "KAZE";
+		case kFeatureOrbOctree:
+			return "ORB-OCTREE";
+		case kFeatureSuperPointTorch:
+			return "SUPERPOINT";
+		case kFeatureSurfFreak:
+			return "SURF+Freak";
+		case kFeatureGfttDaisy:
+			return "GFTT+Daisy";
+		case kFeatureSurfDaisy:
+			return "SURF+Daisy";
+		default:
+			return "Unknown";
+		}
+	}
+
+	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
+	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
+
+	static void filterKeypointsByDepth(
+				std::vector<cv::KeyPoint> & keypoints,
+				const cv::Mat & depth,
+				float minDepth,
+				float maxDepth);
+	static void filterKeypointsByDepth(
+			std::vector<cv::KeyPoint> & keypoints,
+			cv::Mat & descriptors,
+			const cv::Mat & depth,
+			float minDepth,
+			float maxDepth);
+	static void filterKeypointsByDepth(
+			std::vector<cv::KeyPoint> & keypoints,
+			cv::Mat & descriptors,
+			std::vector<cv::Point3f> & keypoints3D,
+			float minDepth,
+			float maxDepth);
+
+	static void filterKeypointsByDisparity(
+				std::vector<cv::KeyPoint> & keypoints,
+				const cv::Mat & disparity,
+				float minDisparity);
+	static void filterKeypointsByDisparity(
+			std::vector<cv::KeyPoint> & keypoints,
+			cv::Mat & descriptors,
+			const cv::Mat & disparity,
+			float minDisparity);
+
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols, bool ssc = false);
+
+	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
+	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
+
+	int getMaxFeatures() const {return maxFeatures_;}
+	bool getSSC() const {return SSC_;}
+	float getMinDepth() const {return _minDepth;}
+	float getMaxDepth() const {return _maxDepth;}
+	int getGridRows() const {return gridRows_;}
+	int getGridCols() const {return gridCols_;}
+
+public:
+	virtual ~Feature2D();
+
+	std::vector<cv::KeyPoint> generateKeypoints(
+			const cv::Mat & image,
+			const cv::Mat & mask = cv::Mat());
+	cv::Mat generateDescriptors(
+			const cv::Mat & image,
+			std::vector<cv::KeyPoint> & keypoints) const;
+	std::vector<cv::Point3f> generateKeypoints3D(
+			const SensorData & data,
+			const std::vector<cv::KeyPoint> & keypoints) const;
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual const ParametersMap & getParameters() const {return parameters_;}
+	virtual Feature2D::Type getType() const = 0;
+
+protected:
+	Feature2D(const ParametersMap & parameters = ParametersMap());
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) = 0;
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
+
+private:
+	ParametersMap parameters_;
+	int maxFeatures_;
+	bool SSC_;
+	float _maxDepth; // 0=inf
+	float _minDepth;
+	std::vector<float> _roiRatios; // size 4
+	int _subPixWinSize;
+	int _subPixIterations;
+	double _subPixEps;
+	int gridRows_;
+	int gridCols_;
+	// Stereo stuff
+	Stereo * _stereo;
+};
+
+//SURF
+class RTABMAP_CORE_EXPORT SURF : public Feature2D
+{
+public:
+	SURF(const ParametersMap & parameters = ParametersMap());
+	virtual ~SURF();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureSurf;}
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	double hessianThreshold_;
+	int nOctaves_;
+	int nOctaveLayers_;
+	bool extended_;
+	bool upright_;
+	float gpuKeypointsRatio_;
+	bool gpuVersion_;
+
+	cv::Ptr<CV_SURF> _surf;
+	cv::Ptr<CV_SURF_GPU> _gpuSurf;
+};
+
+//SIFT
+class RTABMAP_CORE_EXPORT SIFT : public Feature2D
+{
+public:
+	SIFT(const ParametersMap & parameters = ParametersMap());
+	virtual ~SIFT();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureSift;}
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	int nOctaveLayers_;
+	double contrastThreshold_;
+	double edgeThreshold_;
+	double sigma_;
+	bool preciseUpscale_;
+	bool rootSIFT_;
+	bool gpu_;
+	float guaussianThreshold_;
+	bool upscale_;
+
+	cv::Ptr<CV_SIFT> sift_;
+	SiftData * cudaSiftData_;
+	float * cudaSiftMemory_;
+	cv::Size cudaSiftMemorySize_;
+	cv::Mat cudaSiftDescriptors_;
+	bool cudaSiftUpscaling_;
+};
+
+//ORB
+class RTABMAP_CORE_EXPORT ORB : public Feature2D
+{
+public:
+	ORB(const ParametersMap & parameters = ParametersMap());
+	virtual ~ORB();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureOrb;}
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	float scaleFactor_;
+	int nLevels_;
+	int edgeThreshold_;
+	int firstLevel_;
+	int WTA_K_;
+	int scoreType_;
+	int patchSize_;
+	bool gpu_;
+
+	int fastThreshold_;
+	bool nonmaxSuppresion_;
+
+	cv::Ptr<CV_ORB> _orb;
+	cv::Ptr<CV_ORB_GPU> _gpuOrb;
+};
+
+//FAST
+class RTABMAP_CORE_EXPORT FAST : public Feature2D
+{
+public:
+	FAST(const ParametersMap & parameters = ParametersMap());
+	virtual ~FAST();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureUndef;}
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat &, std::vector<cv::KeyPoint> &) const {return cv::Mat();}
+
+private:
+	int threshold_;
+	bool nonmaxSuppression_;
+	bool gpu_;
+	double gpuKeypointsRatio_;
+	int minThreshold_;
+	int maxThreshold_;
+	int gridRows_;
+	int gridCols_;
+	int fastCV_;
+
+	bool fastCVinit_;
+	int fastCVMaxFeatures_;
+	int fastCVLastImageHeight_;
+	uint32_t* fastCVCorners_= NULL;
+	uint32_t* fastCVCornerScores_ = NULL;
+	void* fastCVTempBuf_ = NULL;
+
+	cv::Ptr<cv::FeatureDetector> _fast;
+	cv::Ptr<CV_FAST_GPU> _gpuFast;
+};
+
+//FAST_BRIEF
+class RTABMAP_CORE_EXPORT FAST_BRIEF : public FAST
+{
+public:
+	FAST_BRIEF(const ParametersMap & parameters = ParametersMap());
+	virtual ~FAST_BRIEF();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureFastBrief;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	int bytes_;
+
+	cv::Ptr<CV_BRIEF> _brief;
+};
+
+//FAST_FREAK
+class RTABMAP_CORE_EXPORT FAST_FREAK : public FAST
+{
+public:
+	FAST_FREAK(const ParametersMap & parameters = ParametersMap());
+	virtual ~FAST_FREAK();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureFastFreak;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+	cv::Ptr<CV_FREAK> _freak;
+};
+
+//GFTT
+class RTABMAP_CORE_EXPORT GFTT : public Feature2D
+{
+public:
+	GFTT(const ParametersMap & parameters = ParametersMap());
+	virtual ~GFTT();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+
+private:
+	double _qualityLevel;
+	double _minDistance;
+	int _blockSize;
+	bool _useHarrisDetector;
+	double _k;
+	bool _gpu;
+
+	cv::Ptr<CV_GFTT> _gftt;
+	cv::Ptr<CV_GFTT_GPU> _gpuGftt;
+};
+
+//GFTT_BRIEF
+class RTABMAP_CORE_EXPORT GFTT_BRIEF : public GFTT
+{
+public:
+	GFTT_BRIEF(const ParametersMap & parameters = ParametersMap());
+	virtual ~GFTT_BRIEF();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureGfttBrief;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	int bytes_;
+
+	cv::Ptr<CV_BRIEF> _brief;
+};
+
+//GFTT_FREAK
+class RTABMAP_CORE_EXPORT GFTT_FREAK : public GFTT
+{
+public:
+	GFTT_FREAK(const ParametersMap & parameters = ParametersMap());
+	virtual ~GFTT_FREAK();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureGfttFreak;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+	cv::Ptr<CV_FREAK> _freak;
+};
+
+//SURF_FREAK
+class RTABMAP_CORE_EXPORT SURF_FREAK : public SURF
+{
+public:
+	SURF_FREAK(const ParametersMap & parameters = ParametersMap());
+	virtual ~SURF_FREAK();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureSurfFreak;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+	cv::Ptr<CV_FREAK> _freak;
+};
+
+//GFTT_ORB
+class RTABMAP_CORE_EXPORT GFTT_ORB : public GFTT
+{
+public:
+	GFTT_ORB(const ParametersMap & parameters = ParametersMap());
+	virtual ~GFTT_ORB();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureGfttOrb;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	ORB _orb;
+};
+
+//BRISK
+class RTABMAP_CORE_EXPORT BRISK : public Feature2D
+{
+public:
+	BRISK(const ParametersMap & parameters = ParametersMap());
+	virtual ~BRISK();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureBrisk;}
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	int thresh_;
+	int octaves_;
+	float patternScale_;
+
+	cv::Ptr<CV_BRISK> brisk_;
+};
+
+//KAZE
+class RTABMAP_CORE_EXPORT KAZE : public Feature2D
+{
+public:
+	KAZE(const ParametersMap & parameters = ParametersMap());
+	virtual ~KAZE();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const { return kFeatureKaze; }
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool extended_;
+	bool upright_;
+	float threshold_;
+	int nOctaves_;
+	int nOctaveLayers_;
+	int diffusivity_;
+
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<cv::KAZE> kaze_;
+#endif
+};
+
+//ORB OCTREE
+class RTABMAP_CORE_EXPORT ORBOctree : public Feature2D
+{
+public:
+	ORBOctree(const ParametersMap & parameters = ParametersMap());
+	virtual ~ORBOctree();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureOrbOctree;}
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	float scaleFactor_;
+	int nLevels_;
+	int patchSize_;
+	int edgeThreshold_;
+	int fastThreshold_;
+	int fastMinThreshold_;
+
+	cv::Ptr<ORBextractor> _orb;
+	cv::Mat descriptors_;
+};
+
+//SuperPointTorch
+class RTABMAP_CORE_EXPORT SuperPointTorch : public Feature2D
+{
+public:
+	SuperPointTorch(const ParametersMap & parameters = ParametersMap());
+	virtual ~SuperPointTorch();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const { return kFeatureSuperPointTorch; }
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+	cv::Ptr<SPDetector> superPoint_;
+
+	std::string path_;
+	float threshold_;
+	bool nms_;
+	int minDistance_;
+	bool cuda_;
+};
+
+//GFTT_DAISY
+class RTABMAP_CORE_EXPORT GFTT_DAISY : public GFTT
+{
+public:
+	GFTT_DAISY(const ParametersMap & parameters = ParametersMap());
+	virtual ~GFTT_DAISY();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureGfttDaisy;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<CV_DAISY> _daisy;
+#endif
+};
+
+//SURF_DAISY
+class RTABMAP_CORE_EXPORT SURF_DAISY : public SURF
+{
+public:
+	SURF_DAISY(const ParametersMap & parameters = ParametersMap());
+	virtual ~SURF_DAISY();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureSurfDaisy;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	bool orientationNormalized_;
+	bool scaleNormalized_;
+	float patternScale_;
+	int nOctaves_;
+
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<CV_DAISY> _daisy;
+#endif
+};
+
+}
+
+#endif /* FEATURES2D_H_ */

rtabmap/corelib/include/rtabmap/core/FlannIndex.h

@@ -0,0 +1,119 @@
+/*
+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.
+*/
+
+#ifndef CORELIB_SRC_FLANNINDEX_H_
+#define CORELIB_SRC_FLANNINDEX_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+#include <list>
+#include <opencv2/opencv.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT FlannIndex
+{
+public:
+	FlannIndex();
+	virtual ~FlannIndex();
+
+	void release();
+	size_t indexedFeatures() const;
+
+	// return Bytes
+	size_t memoryUsed() const;
+
+	// Note that useDistanceL1 doesn't have any effect if LSH is used
+	void buildLinearIndex(
+			const cv::Mat & features,
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
+	void buildKDTreeIndex(
+			const cv::Mat & features,
+			int trees = 4,
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
+	void buildKDTreeSingleIndex(
+			const cv::Mat & features,
+			int leafMaxSize = 10,
+			bool reorder = true,
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
+	void buildLSHIndex(
+			const cv::Mat & features,
+			unsigned int table_number = 12,
+			unsigned int key_size = 20,
+			unsigned int multi_probe_level = 2,
+			float rebalancingFactor = 2.0f);
+
+	bool isBuilt();
+
+	int featuresType() const {return featuresType_;}
+	int featuresDim() const {return featuresDim_;}
+
+	std::vector<unsigned int> addPoints(const cv::Mat & features);
+
+	void removePoint(unsigned int index);
+
+	// return squared distances (indices should be casted in size_t)
+	void knnSearch(
+			const cv::Mat & query,
+			cv::Mat & indices,
+			cv::Mat & dists,
+	        int knn,
+			int checks = 32,
+			float eps = 0.0,
+			bool sorted = true) const;
+
+	// return squared distances
+	void radiusSearch(
+			const cv::Mat & query,
+			std::vector<std::vector<size_t> > & indices,
+			std::vector<std::vector<float> > & dists,
+			float radius,
+			int maxNeighbors = 0,
+			int checks = 32,
+			float eps = 0.0,
+			bool sorted = true) const;
+
+private:
+	void * index_;
+	unsigned int nextIndex_;
+	int featuresType_;
+	int featuresDim_;
+	bool isLSH_;
+	bool useDistanceL1_; // true=EUCLEDIAN_L2 false=MANHATTAN_L1
+	float rebalancingFactor_;
+
+	// keep feature in memory until the tree is rebuilt
+	// (in case the word is deleted when removed from the VWDictionary)
+	std::map<int, cv::Mat> addedDescriptors_;
+	std::list<int> removedIndexes_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* CORELIB_SRC_FLANNINDEX_H_ */

rtabmap/corelib/include/rtabmap/core/GPS.h

@@ -0,0 +1,78 @@
+/*
+Copyright (c) 2010-2018, 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.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_
+
+#include <rtabmap/core/GeodeticCoords.h>
+
+namespace rtabmap {
+
+class GPS
+{
+public:
+	GPS():
+		stamp_(0.0),
+		longitude_(0.0),
+		latitude_(0.0),
+		altitude_(0.0),
+		error_(0.0),
+		bearing_(0.0)
+	{}
+	GPS(const double & stamp,
+		const double & longitude,
+		const double & latitude,
+		const double & altitude,
+		const double & error,
+		const double & bearing):
+			stamp_(stamp),
+			longitude_(longitude),
+			latitude_(latitude),
+			altitude_(altitude),
+			error_(error),
+			bearing_(bearing)
+	{}
+	const double & stamp() const {return stamp_;}
+	const double & longitude() const {return longitude_;}
+	const double & latitude() const {return latitude_;}
+	const double & altitude() const {return altitude_;}
+	const double & error() const {return error_;}
+	const double & bearing() const {return bearing_;}
+
+	GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
+private:
+	double stamp_;     // in sec
+	double longitude_; // DD
+	double latitude_;  // DD
+	double altitude_;  // m
+	double error_;     // m
+	double bearing_;   // deg (North 0->360 clockwise)
+};
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_ */

rtabmap/corelib/include/rtabmap/core/GainCompensator.h

@@ -0,0 +1,108 @@
+/*
+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.
+*/
+
+#ifndef CORELIB_SRC_GAINCOMPENSATOR_H_
+#define CORELIB_SRC_GAINCOMPENSATOR_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/pcl_base.h>
+#include <opencv2/opencv.hpp>
+#include <rtabmap/core/Link.h>
+
+namespace rtabmap {
+
+/**
+ * Works like cv::GainCompensator but with point clouds
+ */
+class RTABMAP_CORE_EXPORT GainCompensator {
+public:
+	GainCompensator(double maxCorrespondenceDistance = 0.02, double minOverlap = 0.0, double alpha = 0.01, double beta = 10);
+	virtual ~GainCompensator();
+
+	void feed(
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudA, // should not contain NaNs
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudB, // should not contain NaNs
+			const Transform & transformB);
+	void feed(
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudA,
+			const pcl::IndicesPtr & indicesA,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloudB,
+			const pcl::IndicesPtr & indicesB,
+			const Transform & transformB);
+	void feed(
+			const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds, // should not contain NaNs
+			const std::multimap<int, Link> & links);
+	void feed(
+			const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds,
+			const std::map<int, pcl::IndicesPtr> & indices,
+			const std::multimap<int, Link> & links);
+	void feed(
+			const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
+			const std::map<int, pcl::IndicesPtr> & indices,
+			const std::multimap<int, Link> & links);
+	void feed(
+			const std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::IndicesPtr> > & clouds,
+			const std::multimap<int, Link> & links);
+
+	void apply(
+			int id,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+			bool rgb = true) const;
+	void apply(
+			int id,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+			const pcl::IndicesPtr & indices,
+			bool rgb = true) const;
+	void apply(
+			int id,
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+			const pcl::IndicesPtr & indices,
+			bool rgb = true) const;
+	void apply(
+			int id,
+			cv::Mat & image,
+			bool rgb = true) const;
+
+	double getGain(int id, double * r=0, double * g=0, double * b=0) const;
+	int getIndex(int id) const;
+
+private:
+	cv::Mat_<double> gains_;
+	std::map<int, int> idToIndex_;
+	double maxCorrespondenceDistance_;
+	double minOverlap_;
+	double alpha_;
+	double beta_;
+
+};
+
+} /* namespace rtabmap */
+
+#endif /* CORELIB_SRC_GAINCOMPENSATOR_H_ */

rtabmap/corelib/include/rtabmap/core/GeodeticCoords.h

@@ -0,0 +1,86 @@
+/*
+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.
+*/
+
+/*
+ * The methods in this file were modified from the originals of the MRPT toolkit (see notice below):
+ * https://github.com/MRPT/mrpt/blob/master/libs/topography/src/conversions.cpp
+ */
+
+/* +---------------------------------------------------------------------------+
+   |                     Mobile Robot Programming Toolkit (MRPT)               |
+   |                          http://www.mrpt.org/                             |
+   |                                                                           |
+   | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
+   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
+   | Released under BSD License. See details in http://www.mrpt.org/License    |
+   +---------------------------------------------------------------------------+ */
+
+
+#ifndef GEODETICCOORDS_H_
+#define GEODETICCOORDS_H_
+
+#include <rtabmap/core/rtabmap_core_export.h>
+
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT GeodeticCoords
+{
+public:
+	GeodeticCoords();
+	GeodeticCoords(double latitude, double longitude, double altitude);
+
+	const double & latitude() const {return latitude_;}
+	const double & longitude() const {return longitude_;}
+	const double & altitude() const {return altitude_;}
+
+	void setLatitude(const double & value) {latitude_ = value;}
+	void setLongitude(const double & value) {longitude_ = value;}
+	void setAltitude(const double & value) {altitude_ = value;}
+
+	cv::Point3d toGeocentric_WGS84() const;
+	cv::Point3d toENU_WGS84(const GeodeticCoords & origin) const; // East=X, North=Y
+
+	void fromGeocentric_WGS84(const cv::Point3d& geocentric);
+	void fromENU_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
+
+	static cv::Point3d ENU_WGS84ToGeocentric_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
+	static cv::Point3d Geocentric_WGS84ToENU_WGS84(
+			const cv::Point3d & geocentric_WGS84,
+			const cv::Point3d & origin_geocentric_WGS84,
+			const GeodeticCoords & origin);
+
+private:
+	double latitude_;  // deg
+	double longitude_; // deg
+	double altitude_;  // m
+};
+
+}
+
+#endif /* GEODETICCOORDS_H_ */

rtabmap/corelib/include/rtabmap/core/GlobalDescriptor.h

@@ -0,0 +1,59 @@
+/*
+Copyright (c) 2010-2024, 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.
+*/
+
+#pragma once
+
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+class GlobalDescriptor
+{
+
+public:
+	GlobalDescriptor(int type, const cv::Mat & data, const cv::Mat & info = cv::Mat()) :
+		type_(type),
+		info_(info),
+		data_(data)
+	{}
+	GlobalDescriptor() :
+		type_(-1) // Not set
+	{}
+	virtual ~GlobalDescriptor() {}
+
+	int type() const {return type_;}
+	const cv::Mat info() const {return info_;}
+	const cv::Mat data() const {return data_;}
+
+private:
+	int type_;
+	cv::Mat info_;
+	cv::Mat data_;
+};
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/GlobalDescriptorExtractor.h

@@ -0,0 +1,73 @@
+/*
+Copyright (c) 2010-2024, 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.
+*/
+
+#ifndef GLOBAL_DESCRIPTOR_EXTRACTOR_H_
+#define GLOBAL_DESCRIPTOR_EXTRACTOR_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/SensorData.h"
+
+
+namespace rtabmap {
+
+// Feature2D
+class RTABMAP_CORE_EXPORT GlobalDescriptorExtractor {
+public:
+	enum Type {
+		kUndef=0,
+		kPyDescriptor=1};
+
+	static std::string typeName(Type type)
+	{
+		switch(type){
+		case kPyDescriptor:
+			return "PyDescriptor";
+		default:
+			return "Unknown";
+		}
+	}
+
+	static GlobalDescriptorExtractor * create(const ParametersMap & parameters = ParametersMap());
+	static GlobalDescriptorExtractor * create(GlobalDescriptorExtractor::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
+
+public:
+	virtual ~GlobalDescriptorExtractor();
+
+	virtual GlobalDescriptor extract(const SensorData & data) const = 0;
+
+	virtual void parseParameters(const ParametersMap & parameters) {}
+	virtual GlobalDescriptorExtractor::Type getType() const = 0;
+
+protected:
+	GlobalDescriptorExtractor(const ParametersMap & parameters = ParametersMap());
+};
+
+}
+
+#endif /* GLOBAL_DESCRIPTOR_EXTRACTOR_H_ */

rtabmap/corelib/include/rtabmap/core/GlobalMap.h

@@ -0,0 +1,102 @@
+/*
+Copyright (c) 2010-2023, 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.
+*/
+
+#ifndef SRC_MAP_H_
+#define SRC_MAP_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/LocalGrid.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Transform.h>
+#include <list>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT GlobalMap
+{
+public:
+	inline static float logodds(double probability)
+	{
+		return (float) log(probability/(1-probability));
+	}
+
+	inline static double probability(double logodds)
+	{
+		return 1. - ( 1. / (1. + exp(logodds)));
+	}
+
+public:
+	virtual ~GlobalMap();
+
+	bool update(const std::map<int, Transform> & poses); // return true if map has changed
+
+	virtual void clear();
+
+	float getCellSize() const {return cellSize_;}
+	float getUpdateError() const {return updateError_;}
+	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
+
+	void getGridMin(double & x, double & y) const {x=minValues_[0];y=minValues_[1];}
+	void getGridMax(double & x, double & y) const {x=maxValues_[0];y=maxValues_[1];}
+	void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];}
+	void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];}
+
+	virtual unsigned long getMemoryUsed() const;
+
+protected:
+	GlobalMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap());
+
+	virtual void assemble(const std::list<std::pair<int, Transform> > & newPoses) = 0;
+
+	const std::map<int, LocalGrid> & cache() const {return cache_->localGrids();}
+
+	const std::map<int, Transform> & assembledNodes() const {return addedNodes_;}
+	bool isNodeAssembled(int id) {return addedNodes_.find(id) != addedNodes_.end();}
+	void addAssembledNode(int id, const Transform & pose);
+
+protected:
+	float cellSize_;
+	float updateError_;
+
+	float occupancyThr_;
+	float logOddsHit_;
+	float logOddsMiss_;
+	float logOddsClampingMin_;
+	float logOddsClampingMax_;
+
+	double minValues_[3];
+	double maxValues_[3];
+
+private:
+	const LocalGridCache * cache_;
+	std::map<int, Transform> addedNodes_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* SRC_MAP_H_ */

rtabmap/corelib/include/rtabmap/core/Graph.h

@@ -0,0 +1,358 @@
+/*
+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.
+*/
+
+#ifndef GRAPH_H_
+#define GRAPH_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <map>
+#include <list>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Link.h>
+#include <rtabmap/core/GPS.h>
+#include <rtabmap/core/CameraModel.h>
+
+namespace rtabmap {
+class Memory;
+
+namespace graph {
+
+////////////////////////////////////////////
+// Graph utilities
+////////////////////////////////////////////
+
+bool RTABMAP_CORE_EXPORT exportPoses(
+		const std::string & filePath,
+		int format, // 0=Raw (*.txt), 1=RGBD-SLAM motion capture (*.txt) (10=without change of coordinate frame, 11=10+ID), 2=KITTI (*.txt), 3=TORO (*.graph), 4=g2o (*.g2o)
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & constraints = std::multimap<int, Link>(), // required for formats 3 and 4
+		const std::map<int, double> & stamps = std::map<int, double>(),  // required for format 1
+		const ParametersMap & parameters = ParametersMap()); // optional for formats 3 and 4
+
+bool RTABMAP_CORE_EXPORT importPoses(
+		const std::string & filePath,
+		int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame, 11=10+ID), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
+		std::map<int, Transform> & poses,
+		std::multimap<int, Link> * constraints = 0, // optional for formats 3 and 4
+		std::map<int, double> * stamps = 0); // optional for format 1 and 9
+
+bool RTABMAP_CORE_EXPORT exportGPS(
+		const std::string & filePath,
+		const std::map<int, GPS> & gpsValues,
+		unsigned int rgba = 0xFFFFFFFF);
+
+/**
+ * Compute translation and rotation errors for KITTI datasets.
+ * See http://www.cvlibs.net/datasets/kitti/eval_odometry.php.
+ * @param poses_gt, Ground Truth poses
+ * @param poses_result, Estimated poses
+ * @param t_err, Output translation error (%)
+ * @param r_err, Output rotation error (deg/m)
+ */
+void RTABMAP_CORE_EXPORT calcKittiSequenceErrors(
+		const std::vector<Transform> &poses_gt,
+		const std::vector<Transform> &poses_result,
+		float & t_err,
+		float & r_err);
+
+/**
+ * Compute average of translation and rotation errors between each poses.
+ * @param poses_gt, Ground Truth poses
+ * @param poses_result, Estimated poses
+ * @param t_err, Output translation error (m)
+ * @param r_err, Output rotation error (deg)
+ */
+void RTABMAP_CORE_EXPORT calcRelativeErrors (
+		const std::vector<Transform> &poses_gt,
+		const std::vector<Transform> &poses_result,
+		float & t_err,
+		float & r_err);
+
+/**
+ * Compute root-mean-square error (RMSE) like the TUM RGBD
+ * dataset's evaluation tool (absolute trajectory error).
+ * See https://vision.in.tum.de/data/datasets/rgbd-dataset
+ * @param groundTruth, Ground Truth poses
+ * @param poses, Estimated poses
+ * @return Gt to Map transform
+ */
+Transform RTABMAP_CORE_EXPORT calcRMSE(
+		const std::map<int, Transform> &groundTruth,
+		const std::map<int, Transform> &poses,
+		float & translational_rmse,
+		float & translational_mean,
+		float & translational_median,
+		float & translational_std,
+		float & translational_min,
+		float & translational_max,
+		float & rotational_rmse,
+		float & rotational_mean,
+		float & rotational_median,
+		float & rotational_std,
+		float & rotational_min,
+		float & rotational_max,
+		bool align2D = false);
+
+void RTABMAP_CORE_EXPORT computeMaxGraphErrors(
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		float & maxLinearErrorRatio,
+		float & maxAngularErrorRatio,
+		float & maxLinearError,
+		float & maxAngularError,
+		const Link ** maxLinearErrorLink = 0,
+		const Link ** maxAngularErrorLink = 0,
+		bool for3DoF = false);
+
+std::vector<double> RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap<int, Link> & links);
+
+std::multimap<int, Link>::iterator RTABMAP_CORE_EXPORT findLink(
+		std::multimap<int, Link> & links,
+		int from,
+		int to,
+		bool checkBothWays = true,
+		Link::Type type = Link::kUndef);
+std::multimap<int, std::pair<int, Link::Type> >::iterator RTABMAP_CORE_EXPORT findLink(
+		std::multimap<int, std::pair<int, Link::Type> > & links,
+		int from,
+		int to,
+		bool checkBothWays = true,
+		Link::Type type = Link::kUndef);
+std::multimap<int, int>::iterator RTABMAP_CORE_EXPORT findLink(
+		std::multimap<int, int> & links,
+		int from,
+		int to,
+		bool checkBothWays = true);
+std::multimap<int, Link>::const_iterator RTABMAP_CORE_EXPORT findLink(
+		const std::multimap<int, Link> & links,
+		int from,
+		int to,
+		bool checkBothWays = true,
+		Link::Type type = Link::kUndef);
+std::multimap<int, std::pair<int, Link::Type> >::const_iterator RTABMAP_CORE_EXPORT findLink(
+		const std::multimap<int, std::pair<int, Link::Type> > & links,
+		int from,
+		int to,
+		bool checkBothWays = true,
+		Link::Type type = Link::kUndef);
+std::multimap<int, int>::const_iterator RTABMAP_CORE_EXPORT findLink(
+		const std::multimap<int, int> & links,
+		int from,
+		int to,
+		bool checkBothWays = true);
+std::list<Link> RTABMAP_CORE_EXPORT findLinks(
+		const std::multimap<int, Link> & links,
+		int from);
+
+std::multimap<int, Link> RTABMAP_CORE_EXPORT filterDuplicateLinks(
+		const std::multimap<int, Link> & links);
+/**
+ * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
+ */
+std::multimap<int, Link> RTABMAP_CORE_EXPORT filterLinks(
+		const std::multimap<int, Link> & links,
+		Link::Type filteredType,
+		bool inverted = false);
+/**
+ * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
+ */
+std::map<int, Link> RTABMAP_CORE_EXPORT filterLinks(
+		const std::map<int, Link> & links,
+		Link::Type filteredType,
+		bool inverted = false);
+
+//Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
+std::map<int, Transform> RTABMAP_CORE_EXPORT frustumPosesFiltering(
+		const std::map<int, Transform> & poses,
+		const Transform & cameraPose,
+		float horizontalFOV = 45.0f, // in degrees, xfov = atan((image_width/2)/fx)*2
+		float verticalFOV = 45.0f,   // in degrees, yfov = atan((image_height/2)/fy)*2
+		float nearClipPlaneDistance = 0.1f,
+		float farClipPlaneDistance = 100.0f,
+		bool negative = false);
+
+/**
+ * Get only the the most recent or older poses in the defined radius.
+ * @param poses The poses
+ * @param radius Radius (m) of the search for near neighbors
+ * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
+ * @param keepLatest keep the latest node if true, otherwise the oldest node is kept
+ * @return A map containing only most recent or older poses in the the defined radius
+ */
+std::map<int, Transform> RTABMAP_CORE_EXPORT radiusPosesFiltering(
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle,
+		bool keepLatest = true);
+
+/**
+ * Get all neighbor nodes in a fixed radius around each pose.
+ * @param poses The poses
+ * @param radius Radius (m) of the search for near neighbors
+ * @param angle Maximum angle (rad, [0,PI]) of accepted neighbor nodes in the radius (0 means ignore angle)
+ * @return A map between each pose id and its neighbors found in the radius
+ */
+std::multimap<int, int> RTABMAP_CORE_EXPORT radiusPosesClustering(
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle);
+
+void reduceGraph(
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		std::multimap<int, int> & hyperNodes, //<parent ID, child ID>
+		std::multimap<int, Link> & hyperLinks);
+
+/**
+ * Perform A* path planning in the graph.
+ * @param poses The graph's poses
+ * @param links The graph's links (from node id -> to node id)
+ * @param from initial node
+ * @param to final node
+ * @param updateNewCosts Keep up-to-date costs while traversing the graph.
+ * @return the path ids from id "from" to id "to" including initial and final nodes.
+ */
+std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
+			const std::map<int, rtabmap::Transform> & poses,
+			const std::multimap<int, int> & links,
+			int from,
+			int to,
+			bool updateNewCosts = false);
+
+/**
+ * Perform Dijkstra path planning in the graph.
+ * @param poses The graph's poses
+ * @param links The graph's links (from node id -> to node id)
+ * @param from initial node
+ * @param to final node
+ * @param updateNewCosts Keep up-to-date costs while traversing the graph.
+ * @param useSameCostForAllLinks Ignore distance between nodes
+ * @return the path ids from id "from" to id "to" including initial and final nodes.
+ */
+std::list<int> RTABMAP_CORE_EXPORT computePath(
+			const std::multimap<int, Link> & links,
+			int from,
+			int to,
+			bool updateNewCosts = false,
+			bool useSameCostForAllLinks = false);
+
+/**
+ * Perform Dijkstra path planning in the graph.
+ * @param fromId initial node
+ * @param toId final node
+ * @param memory The graph's memory
+ * @param lookInDatabase check links in database
+ * @param updateNewCosts Keep up-to-date costs while traversing the graph.
+ * @return the path ids from id "fromId" to id "toId" including initial and final nodes (Identity pose for the first node).
+ */
+std::list<std::pair<int, Transform> > RTABMAP_CORE_EXPORT computePath(
+		int fromId,
+		int toId,
+		const Memory * memory,
+		bool lookInDatabase = true,
+		bool updateNewCosts = false,
+		float linearVelocity = 0.0f,   // m/sec
+		float angularVelocity = 0.0f); // rad/sec
+
+/**
+ * Find the nearest node of the target pose
+ * @param nodes the nodes to search for
+ * @param targetPose the target pose to search around
+ * @param distance squared distance of the nearest node found (optional)
+ * @return the node id.
+ */
+int RTABMAP_CORE_EXPORT findNearestNode(
+		const std::map<int, rtabmap::Transform> & poses,
+		const rtabmap::Transform & targetPose,
+		float * distance = 0);
+
+/**
+ * Find the nearest nodes of the query pose or node
+ * @param nodeId the query id
+ * @param nodes the nodes to search for
+ * @param radius radius to search for (m), if 0, k should be > 0.
+ * @param k max nearest neighbors (0=all inside the radius)
+ * @return the nodes with squared distance to query node.
+ */
+std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
+		int nodeId,
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle = 0.0f,
+		int k=0);
+std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(
+		const Transform & targetPose,
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle = 0.0f,
+		int k=0);
+std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
+		int nodeId,
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle = 0.0f,
+		int k=0);
+std::map<int, Transform> RTABMAP_CORE_EXPORT findNearestPoses(
+		const Transform & targetPose,
+		const std::map<int, Transform> & poses,
+		float radius,
+		float angle = 0.0f,
+		int k=0);
+
+// Use new findNearestNodes() interface with radius=0, angle=0.
+RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT findNearestNodes(const std::map<int, rtabmap::Transform> & nodes, const rtabmap::Transform & targetPose, int k);
+// Renamed to findNearestNodes()
+RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius);
+// Renamed to findNearestNodes()
+RTABMAP_DEPRECATED std::map<int, float> RTABMAP_CORE_EXPORT getNodesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius);
+// Renamed to findNearestNodes()
+RTABMAP_DEPRECATED std::map<int, Transform> RTABMAP_CORE_EXPORT getPosesInRadius(int nodeId, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f);
+// Renamed to findNearestNodes()
+RTABMAP_DEPRECATED std::map<int, Transform> RTABMAP_CORE_EXPORT getPosesInRadius(const Transform & targetPose, const std::map<int, Transform> & nodes, float radius, float angle = 0.0f);
+
+float RTABMAP_CORE_EXPORT computePathLength(
+		const std::vector<std::pair<int, Transform> > & path,
+		unsigned int fromIndex = 0,
+		unsigned int toIndex = 0);
+
+// assuming they are all linked in map order
+float RTABMAP_CORE_EXPORT computePathLength(
+		const std::map<int, Transform> & path);
+
+std::list<std::map<int, Transform> > RTABMAP_CORE_EXPORT getPaths(
+		std::map<int, Transform> poses,
+		const std::multimap<int, Link> & links);
+
+void RTABMAP_CORE_EXPORT computeMinMax(const std::map<int, Transform> & poses,
+		cv::Vec3f & min,
+		cv::Vec3f & max);
+
+} /* namespace graph */
+
+} /* namespace rtabmap */
+#endif /* GRAPH_H_ */

rtabmap/corelib/include/rtabmap/core/IMU.h

@@ -0,0 +1,108 @@
+/*
+ * IMU.h
+ *
+ *  Created on: 2018-03-05
+ *      Author: mathieu
+ */
+
+#ifndef IMU_H_
+#define IMU_H_
+
+#include <opencv2/core/core.hpp>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/core/Transform.h>
+
+namespace rtabmap {
+
+
+// Correspondence class to sensor_msgs/IMU
+class IMU
+{
+public:
+	IMU() {}
+	IMU(const cv::Vec4d & orientation, // qx qy qz qw
+		const cv::Mat & orientationCovariance,
+		const cv::Vec3d & angularVelocity,
+		const cv::Mat & angularVelocityCovariance,
+		const cv::Vec3d & linearAcceleration,
+		const cv::Mat & linearAccelerationCovariance,
+		const Transform & localTransform = Transform::getIdentity()) :
+			orientation_(orientation),
+			orientationCovariance_(orientationCovariance),
+			angularVelocity_(angularVelocity),
+			angularVelocityCovariance_(angularVelocityCovariance),
+			linearAcceleration_(linearAcceleration),
+			linearAccelerationCovariance_(linearAccelerationCovariance),
+			localTransform_(localTransform)
+	{
+	}
+	IMU(const cv::Vec3d & angularVelocity,
+		const cv::Mat & angularVelocityCovariance,
+		const cv::Vec3d & linearAcceleration,
+		const cv::Mat & linearAccelerationCovariance,
+		const Transform & localTransform = Transform::getIdentity()) :
+			angularVelocity_(angularVelocity),
+			angularVelocityCovariance_(angularVelocityCovariance),
+			linearAcceleration_(linearAcceleration),
+			linearAccelerationCovariance_(linearAccelerationCovariance),
+			localTransform_(localTransform)
+	{
+	}
+
+	// qx qy qz qw
+	const cv::Vec4d & orientation() const {return orientation_;}
+	const cv::Mat & orientationCovariance() const {return orientationCovariance_;} // 3x3 double Row major about x, y, z axes, empty if orientation is not set
+
+	const cv::Vec3d & angularVelocity() const {return angularVelocity_;}
+	const cv::Mat & angularVelocityCovariance() const {return angularVelocityCovariance_;} // 3x3 double Row major about x, y, z axes, empty if angularVelocity is not set
+
+	const cv::Vec3d linearAcceleration() const {return linearAcceleration_;}
+	const cv::Mat & linearAccelerationCovariance() const {return linearAccelerationCovariance_;} // 3x3 double Row major x, y z, empty if linearAcceleration is not set
+
+	const Transform & localTransform() const {return localTransform_;}
+
+	// apply local transform rotation to data, and set Identity rotation for local transform
+	void convertToBaseFrame();
+
+	bool empty() const
+	{
+		return localTransform_.isNull();
+	}
+
+private:
+	cv::Vec4d orientation_;
+	cv::Mat orientationCovariance_; // 3x3 double Row major about x, y, z axes, empty if orientation is not set
+
+	cv::Vec3d angularVelocity_;
+	cv::Mat angularVelocityCovariance_; // 3x3 double Row major about x, y, z axes, empty if angularVelocity is not set
+
+	cv::Vec3d linearAcceleration_;
+	cv::Mat linearAccelerationCovariance_; // 3x3 double Row major x, y z, empty if linearAcceleration is not set
+
+	Transform localTransform_;
+};
+
+class IMUEvent : public UEvent
+{
+public:
+	IMUEvent() :
+		stamp_(0.0)
+	{}
+	IMUEvent(const IMU & data, double stamp) :
+		data_(data),
+		stamp_(stamp)
+	{
+	}
+	virtual std::string getClassName() const {return "IMUEvent";}
+	const IMU & getData() const {return data_;}
+	double getStamp() const {return stamp_;}
+
+private:
+	IMU data_;
+	double stamp_;
+};
+
+}
+
+
+#endif /* IMU_H_ */

rtabmap/corelib/include/rtabmap/core/IMUFilter.h

@@ -0,0 +1,80 @@
+/*
+Copyright (c) 2010-2019, 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.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+#include <rtabmap/core/Parameters.h>
+#include <Eigen/Geometry>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT IMUFilter
+{
+public:
+	enum Type {
+		kMadgwick=0,
+		kComplementaryFilter=1};
+public:
+	static IMUFilter * create(const ParametersMap & parameters = ParametersMap());
+	static IMUFilter * create(IMUFilter::Type type, const ParametersMap & parameters = ParametersMap());
+
+public:
+	virtual void parseParameters(const ParametersMap & parameters) {}
+	virtual ~IMUFilter(){}
+
+	void update(
+			double gx, double gy, double gz,
+			double ax, double ay, double az,
+			double stamp);
+
+	virtual IMUFilter::Type type() const = 0;
+	virtual void getOrientation(double & qx, double & qy, double & qz, double & qw) const = 0;
+	virtual void reset(double qx = 0.0, double qy = 0.0, double qz = 0.0, double qw = 1.0) = 0;
+
+protected:
+	IMUFilter(const ParametersMap & parameters = ParametersMap()) : previousStamp_(0) {}
+
+private:
+	// Update from accelerometer and gyroscope data.
+	// [gx, gy, gz]: Angular veloctiy, in rad / s.
+	// [ax, ay, az]: Normalized gravity vector.
+	// dt: time delta, in seconds.
+	virtual void updateImpl(
+			double gx, double gy, double gz,
+			double ax, double ay, double az,
+			double dt) = 0;
+
+private:
+	double previousStamp_;
+};
+
+}
+
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_ */

rtabmap/corelib/include/rtabmap/core/IMUThread.h

@@ -0,0 +1,77 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#include <fstream>
+
+namespace rtabmap
+{
+
+class IMUFilter;
+
+/**
+ * Class IMUThread
+ *
+ */
+class RTABMAP_CORE_EXPORT IMUThread :
+	public UThread,
+	public UEventsSender
+{
+public:
+	IMUThread(int rate, const Transform & localTransform);
+	virtual ~IMUThread();
+
+	bool init(const std::string & path);
+	void setRate(int rate);
+	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
+	void disableIMUFiltering();
+
+private:
+	virtual void mainLoopBegin();
+	virtual void mainLoop();
+
+private:
+	int rate_;
+	Transform localTransform_;
+	std::ifstream imuFile_;
+	UTimer frameRateTimer_;
+	double captureDelay_;
+	double previousStamp_;
+	IMUFilter * _imuFilter;
+	bool _imuBaseFrameConversion;
+};
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/Image.h

@@ -0,0 +1,38 @@
+/*
+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.
+*/
+
+#ifndef IMAGE_H_
+#define IMAGE_H_
+
+#include <rtabmap/core/SensorData.h>
+
+namespace rtabmap {
+RTABMAP_DEPRECATED(typedef SensorData Image, "rtabmap::Image class is renamed to rtabmap::SensorData, use the last one instead.");
+}
+
+
+#endif /* IMAGE_H_ */

rtabmap/corelib/include/rtabmap/core/LASWriter.h

@@ -0,0 +1,43 @@
+/*
+Copyright (c) 2010-2024, 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.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace rtabmap {
+
+int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud,          const std::vector<int> & cameraIds = std::vector<int>());
+int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud,       const std::vector<int> & cameraIds = std::vector<int>(), const std::vector<float> & intensities = std::vector<float>());
+int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud,         const std::vector<int> & cameraIds = std::vector<int>());
+
+}
+
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_ */

rtabmap/corelib/include/rtabmap/core/Landmark.h

@@ -0,0 +1,100 @@
+/*
+Copyright (c) 2010-2018, 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.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_
+
+#include <rtabmap/core/rtabmap_core_export.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>
+
+namespace rtabmap {
+
+class Landmark
+{
+public:
+	Landmark() :
+		id_(0),
+        size_(0.0f)
+	{}
+    Landmark(const int & id, const float & size, const Transform & pose, const cv::Mat & covariance) :
+        id_(id),
+        size_(size),
+        pose_(pose),
+        covariance_(covariance)
+    {
+        UASSERT(id_>0);
+        UASSERT(!pose_.isNull());
+        UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
+        UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
+        UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
+        UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
+        UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
+        UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
+        UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
+    }
+    // Use constructor with size=0 instead.
+    RTABMAP_DEPRECATED Landmark(const int & id, const Transform & pose, const cv::Mat & covariance);
+
+	virtual ~Landmark() {}
+
+	const int & id() const {return id_;}
+    const float & size() const {return size_;}
+	const Transform & pose() const {return pose_;}
+	const cv::Mat & covariance() const {return covariance_;}
+
+private:
+	int id_;
+    float size_;
+	Transform pose_;
+	cv::Mat covariance_;
+};
+
+typedef std::map<int, Landmark> Landmarks;
+
+inline Landmark::Landmark(const int & id, const Transform & pose, const cv::Mat & covariance) :
+    id_(id),
+    size_(0.0f),
+    pose_(pose),
+    covariance_(covariance)
+{
+    UASSERT(id_>0);
+    UASSERT(!pose_.isNull());
+    UASSERT(covariance_.cols == 6 && covariance_.rows == 6 && covariance_.type() == CV_64FC1);
+    UASSERT_MSG(uIsFinite(covariance_.at<double>(0,0)) && covariance_.at<double>(0,0)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(0,0)).c_str());
+    UASSERT_MSG(uIsFinite(covariance_.at<double>(1,1)) && covariance_.at<double>(1,1)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(1,1)).c_str());
+    UASSERT_MSG(uIsFinite(covariance_.at<double>(2,2)) && covariance_.at<double>(2,2)>0, uFormat("Linear covariance should not be null! Value=%f.", covariance_.at<double>(2,2)).c_str());
+    UASSERT_MSG(uIsFinite(covariance_.at<double>(3,3)) && covariance_.at<double>(3,3)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(3,3)).c_str());
+    UASSERT_MSG(uIsFinite(covariance_.at<double>(4,4)) && covariance_.at<double>(4,4)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(4,4)).c_str());
+    UASSERT_MSG(uIsFinite(covariance_.at<double>(5,5)) && covariance_.at<double>(5,5)>0, uFormat("Angular covariance should not be null! Value=%f (set to 9999 if unknown).", covariance_.at<double>(5,5)).c_str());
+}
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LANDMARK_H_ */

rtabmap/corelib/include/rtabmap/core/LaserScan.h

@@ -0,0 +1,186 @@
+/*
+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.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Transform.h>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT LaserScan
+{
+public:
+	enum Format{kUnknown=0,
+		kXY=1,
+		kXYI=2,
+		kXYNormal=3,
+		kXYINormal=4,
+		kXYZ=5,
+		kXYZI=6,
+		kXYZRGB=7,
+		kXYZNormal=8,
+		kXYZINormal=9,
+		kXYZRGBNormal=10,
+		kXYZIT=11};
+
+	static std::string formatName(const Format & format);
+	static int channels(const Format & format);
+	static bool isScan2d(const Format & format);
+	static bool isScanHasNormals(const Format & format);
+	static bool isScanHasRGB(const Format & format);
+	static bool isScanHasIntensity(const Format & format);
+	static bool isScanHasTime(const Format & format);
+	static LaserScan backwardCompatibility(
+			const cv::Mat & oldScanFormat,
+			int maxPoints = 0,
+			int maxRange = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	static LaserScan backwardCompatibility(
+			const cv::Mat & oldScanFormat,
+			float minRange,
+			float maxRange,
+			float angleMin,
+			float angleMax,
+			float angleInc,
+			const Transform & localTransform = Transform::getIdentity());
+
+public:
+	LaserScan();
+	LaserScan(const LaserScan & data,
+			int maxPoints,
+			float maxRange,
+			const Transform & localTransform = Transform::getIdentity());
+	// Use version without \"format\" argument.
+	RTABMAP_DEPRECATED LaserScan(const LaserScan & data,
+			int maxPoints,
+			float maxRange,
+			Format format,
+			const Transform & localTransform = Transform::getIdentity());
+	LaserScan(const cv::Mat & data,
+			int maxPoints,
+			float maxRange,
+			Format format,
+			const Transform & localTransform = Transform::getIdentity());
+	// Use version without \"format\" argument.
+	RTABMAP_DEPRECATED LaserScan(const LaserScan & data,
+			Format format,
+			float minRange,
+			float maxRange,
+			float angleMin,
+			float angleMax,
+			float angleIncrement,
+			const Transform & localTransform = Transform::getIdentity());
+	LaserScan(const LaserScan & data,
+			float minRange,
+			float maxRange,
+			float angleMin,
+			float angleMax,
+			float angleIncrement,
+			const Transform & localTransform = Transform::getIdentity());
+	LaserScan(const cv::Mat & data,
+			Format format,
+			float minRange,
+			float maxRange,
+			float angleMin,
+			float angleMax,
+			float angleIncrement,
+			const Transform & localTransform = Transform::getIdentity());
+
+	const cv::Mat & data() const {return data_;}
+	Format format() const {return format_;}
+	std::string formatName() const {return formatName(format_);}
+	int channels() const {return data_.channels();}
+	int maxPoints() const {return maxPoints_;}
+	float rangeMin() const {return rangeMin_;}
+	float rangeMax() const {return rangeMax_;}
+	float angleMin() const {return angleMin_;}
+	float angleMax() const {return angleMax_;}
+	float angleIncrement() const {return angleIncrement_;}
+	void setLocalTransform(const Transform & t) {localTransform_ = t;}
+	Transform localTransform() const {return localTransform_;}
+
+	bool empty() const {return data_.empty();}
+	bool isEmpty() const {return data_.empty();}
+	int size() const {return data_.total();}
+	int dataType() const {return data_.type();}
+	bool is2d() const {return isScan2d(format_);}
+	bool hasNormals() const {return isScanHasNormals(format_);}
+	bool hasRGB() const {return isScanHasRGB(format_);}
+	bool hasIntensity() const {return isScanHasIntensity(format_);}
+	bool hasTime() const {return isScanHasTime(format_);}
+	bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
+	bool isOrganized() const {return data_.rows > 1;}
+	LaserScan clone() const;
+	LaserScan densify() const;
+
+	int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
+	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
+	int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
+	int getTimeOffset() const {return hasTime()?4:-1;}
+
+	float & field(unsigned int pointIndex, unsigned int channelOffset);
+
+	void clear() {data_ = cv::Mat();}
+
+	/**
+	 * Concatenate scan's data, localTransform is ignored.
+	 */
+	LaserScan & operator+=(const LaserScan &);
+	/**
+	 * Concatenate scan's data, localTransform is ignored.
+	 */
+	LaserScan operator+(const LaserScan &);
+
+private:
+	void init(const cv::Mat & data,
+			Format format,
+			float minRange,
+			float maxRange,
+			float angleMin,
+			float angleMax,
+			float angleIncrement,
+			int maxPoints,
+			const Transform & localTransform = Transform::getIdentity());
+
+private:
+	cv::Mat data_;
+	Format format_;
+	int maxPoints_;
+	float rangeMin_;
+	float rangeMax_;
+	float angleMin_;
+	float angleMax_;
+	float angleIncrement_;
+	Transform localTransform_;
+};
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASERSCAN_H_ */

rtabmap/corelib/include/rtabmap/core/Lidar.h

@@ -0,0 +1,57 @@
+/*
+Copyright (c) 2010-2022, 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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+#include <rtabmap/core/SensorCapture.h>
+
+namespace rtabmap
+{
+
+/**
+ * Class Lidar
+ *
+ */
+class RTABMAP_CORE_EXPORT Lidar : public SensorCapture
+{
+public:
+	virtual ~Lidar() {}
+
+protected:
+	/**
+	 * Constructor
+	 *
+	 * @param lidarRate the frame rate (Hz), 0 for fast as the lidar can
+	 * @param localTransform the transform from base frame to lidar frame
+	 */
+	Lidar(float lidarRate = 0, const Transform & localTransform = Transform::getIdentity()) :
+		SensorCapture(lidarRate, localTransform) {}
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/Link.h

@@ -0,0 +1,107 @@
+/*
+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.
+*/
+
+#ifndef LINK_H_
+#define LINK_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Transform.h>
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT Link
+{
+public:
+	enum Type {
+		kNeighbor,
+		kGlobalClosure,
+		kLocalSpaceClosure,
+		kLocalTimeClosure,
+		kUserClosure,
+		kVirtualClosure,
+		kNeighborMerged,
+		kPosePrior, // Absolute pose in /world frame, From == To
+		kLandmark,  // Transform /base_link -­­> /landmark, "From" is node observing the landmark "To" (landmark is negative id)
+		kGravity,  // Orientation of the base frame accordingly to gravity (From == To)
+		kEnd,
+		kSelfRefLink = 97, // Include kPosePrior and kGravity (all links where From=To)
+		kAllWithLandmarks = 98,
+		kAllWithoutLandmarks = 99,
+		kUndef = 99};
+	static std::string typeName(Type type);
+
+	Link();
+	Link(int from,
+			int to,
+			Type type,
+			const Transform & transform,
+			const cv::Mat & infMatrix = cv::Mat::eye(6,6,CV_64FC1), // information matrix: inverse of covariance matrix
+			const cv::Mat & userData = cv::Mat());
+
+	bool isValid() const {return from_ != 0 && to_ != 0 && !transform_.isNull() && type_!=kUndef;}
+
+	int from() const {return from_;}
+	int to() const {return to_;}
+	const Transform & transform() const {return transform_;}
+	Type type() const {return type_;}
+	std::string typeName() const {return typeName(type_);}
+	const cv::Mat & infMatrix() const {return infMatrix_;}
+	double rotVariance(bool minimum = true) const;
+	double transVariance(bool minimum = true) const;
+
+	void setFrom(int from) {from_ = from;}
+	void setTo(int to) {to_ = to;}
+	void setTransform(const Transform & transform) {transform_ = transform;}
+	void setType(Type type) {type_ = type;}
+    void setInfMatrix(const cv::Mat & infMatrix);
+
+	const cv::Mat & userDataRaw() const {return _userDataRaw;}
+	const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
+	void uncompressUserData();
+	cv::Mat uncompressUserDataConst() const;
+
+	Link merge(const Link & link, Type outputType) const;
+	Link inverse() const;
+
+private:
+	int from_;
+	int to_;
+	Transform transform_;
+	Type type_;
+	cv::Mat infMatrix_; // Information matrix = covariance matrix ^ -1
+
+	// user data
+	cv::Mat _userDataCompressed;      // compressed data
+	cv::Mat _userDataRaw;
+};
+
+}
+
+
+#endif /* LINK_H_ */

rtabmap/corelib/include/rtabmap/core/LocalGrid.h

@@ -0,0 +1,90 @@
+/*
+Copyright (c) 2010-2023, 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.
+*/
+
+#ifndef SRC_LOCALGRID_H_
+#define SRC_LOCALGRID_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <opencv2/core.hpp>
+#include <map>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT LocalGrid
+{
+public:
+	LocalGrid(const cv::Mat & ground,
+		 const cv::Mat & obstacles,
+		 const cv::Mat & empty,
+		 float cellSize,
+		 const cv::Point3f & viewPoint = cv::Point3f(0,0,0));
+	virtual ~LocalGrid() {}
+	bool is3D() const;
+public:
+	cv::Mat groundCells;
+	cv::Mat obstacleCells;
+	cv::Mat emptyCells;
+	float cellSize;
+	cv::Point3f viewPoint;
+};
+
+class RTABMAP_CORE_EXPORT LocalGridCache
+{
+public:
+	LocalGridCache() {}
+	virtual ~LocalGridCache() {}
+
+	void add(int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Mat & empty,
+			float cellSize,
+			const cv::Point3f & viewPoint = cv::Point3f(0,0,0));
+
+	void add(int nodeId, const LocalGrid & localGrid);
+
+	bool shareTo(int nodeId, LocalGridCache & anotherCache) const;
+
+	unsigned long getMemoryUsed() const;
+	void clear(bool temporaryOnly = false);
+
+	size_t size() const {return localGrids_.size();}
+	bool empty() const {return localGrids_.empty();}
+	const std::map<int, LocalGrid> & localGrids() const {return localGrids_;}
+
+	std::map<int, LocalGrid>::const_iterator find(int nodeId) const {return localGrids_.find(nodeId);}
+	std::map<int, LocalGrid>::const_iterator begin() const {return localGrids_.begin();}
+	std::map<int, LocalGrid>::const_iterator end() const {return localGrids_.end();}
+
+private:
+	std::map<int, LocalGrid> localGrids_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* SRC_LOCALGRID_H_ */

rtabmap/corelib/include/rtabmap/core/LocalGridMaker.h

@@ -0,0 +1,115 @@
+/*
+Copyright (c) 2010-2023, 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.
+*/
+
+#ifndef SRC_LOCAL_MAP_H_
+#define SRC_LOCAL_MAP_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Signature.h>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT LocalGridMaker
+{
+public:
+	LocalGridMaker(const ParametersMap & parameters = ParametersMap());
+	virtual ~LocalGridMaker();
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	float getCellSize() const {return cellSize_;}
+	bool isGridFromDepth() const {return occupancySensor_;}
+	bool isMapFrameProjection() const {return projMapFrame_;}
+
+	template<typename PointT>
+	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
+			const typename pcl::PointCloud<PointT>::Ptr & cloud,
+			const pcl::IndicesPtr & indices,
+			const Transform & pose,
+			const cv::Point3f & viewPoint,
+			pcl::IndicesPtr & groundIndices,        // output cloud indices
+			pcl::IndicesPtr & obstaclesIndices,     // output cloud indices
+			pcl::IndicesPtr * flatObstacles = 0) const; // output cloud indices
+
+	void createLocalMap(
+			const Signature & node,
+			cv::Mat & groundCells,
+			cv::Mat & obstacleCells,
+			cv::Mat & emptyCells,
+			cv::Point3f & viewPoint);
+
+	void createLocalMap(
+			const LaserScan & cloud,
+			const Transform & pose,
+			cv::Mat & groundCells,
+			cv::Mat & obstacleCells,
+			cv::Mat & emptyCells,
+			cv::Point3f & viewPointInOut) const;
+
+protected:
+	ParametersMap parameters_;
+
+	unsigned int cloudDecimation_;
+	float rangeMax_;
+	float rangeMin_;
+	std::vector<float> roiRatios_;
+	float footprintLength_;
+	float footprintWidth_;
+	float footprintHeight_;
+	int scanDecimation_;
+	float cellSize_;
+	bool preVoxelFiltering_;
+	int occupancySensor_;
+	bool projMapFrame_;
+	float maxObstacleHeight_;
+	int normalKSearch_;
+	float groundNormalsUp_;
+	float maxGroundAngle_;
+	float clusterRadius_;
+	int minClusterSize_;
+	bool flatObstaclesDetected_;
+	float minGroundHeight_;
+	float maxGroundHeight_;
+	bool normalsSegmentation_;
+	bool grid3D_;
+	bool groundIsObstacle_;
+	float noiseFilteringRadius_;
+	int noiseFilteringMinNeighbors_;
+	bool scan2dUnknownSpaceFilled_;
+	bool rayTracing_;
+};
+
+} /* namespace rtabmap */
+
+#include <rtabmap/core/impl/LocalMapMaker.hpp>
+
+#endif /* SRC_MAP_H_ */

rtabmap/corelib/include/rtabmap/core/MarkerDetector.h

@@ -0,0 +1,100 @@
+/*
+Copyright (c) 2010-2019, 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.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_MARKERDETECTOR_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_MARKERDETECTOR_H_
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/CameraModel.h>
+#include <opencv2/opencv_modules.hpp>
+
+#ifdef HAVE_OPENCV_ARUCO
+#include <opencv2/aruco.hpp>
+#endif
+
+namespace rtabmap {
+
+typedef std::map<int, Transform> MapIdPose;
+
+class MarkerInfo {
+public:
+    MarkerInfo(int id, float length, Transform pose) :
+        id_(id),
+        length_(length),
+        pose_(pose)
+    {}
+    int id() const {return id_;}
+    float length() const {return length_;}
+    const Transform & pose() const {return pose_;}
+private:
+    int id_;
+    float length_;
+    Transform pose_;
+};
+
+class RTABMAP_CORE_EXPORT MarkerDetector {
+    
+public:
+	MarkerDetector(const ParametersMap & parameters = ParametersMap());
+	virtual ~MarkerDetector();
+	void parseParameters(const ParametersMap & parameters);
+
+	// Use the other detect(), in which the returned map contains the length of each marker detected.
+    RTABMAP_DEPRECATED
+    MapIdPose detect(const cv::Mat & image,
+			const CameraModel & model,
+			const cv::Mat & depth = cv::Mat(),
+			float * estimatedMarkerLength = 0,
+			cv::Mat * imageWithDetections = 0);
+
+    std::map<int, MarkerInfo> detect(const cv::Mat & image,
+		    const std::vector<CameraModel> & models,
+		    const cv::Mat & depth = cv::Mat(),
+		    const std::map<int, float> & markerLengths = std::map<int, float>(),
+		    cv::Mat * imageWithDetections = 0);
+
+    std::map<int, MarkerInfo> detect(const cv::Mat & image,
+		    const CameraModel & model,
+		    const cv::Mat & depth = cv::Mat(),
+		    const std::map<int, float> & markerLengths = std::map<int, float>(),
+		    cv::Mat * imageWithDetections = 0);
+
+private:
+#ifdef HAVE_OPENCV_ARUCO
+	cv::Ptr<cv::aruco::DetectorParameters> detectorParams_;
+	float markerLength_;
+	float maxDepthError_;
+	float maxRange_;
+	float minRange_;
+	int dictionaryId_;
+	cv::Ptr<cv::aruco::Dictionary> dictionary_;
+#endif
+};
+
+} /* namespace rtabmap */
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_MARKERDETECTOR_H_ */

rtabmap/corelib/include/rtabmap/core/Memory.h

@@ -0,0 +1,389 @@
+/*
+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.
+*/
+
+#ifndef MEMORY_H_
+#define MEMORY_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include "rtabmap/utilite/UEventsHandler.h"
+#include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/SensorData.h"
+#include "rtabmap/core/Link.h"
+#include "rtabmap/core/Features2d.h"
+#include <typeinfo>
+#include <list>
+#include <map>
+#include <set>
+#include "rtabmap/utilite/UStl.h"
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <pcl/pcl_config.h>
+
+namespace rtabmap {
+
+class Signature;
+class DBDriver;
+class VWDictionary;
+class VisualWord;
+class Feature2D;
+class Statistics;
+class Registration;
+class RegistrationInfo;
+class RegistrationIcp;
+class RegistrationVis;
+class Stereo;
+class LocalGridMaker;
+class MarkerDetector;
+class GlobalDescriptorExtractor;
+
+class RTABMAP_CORE_EXPORT Memory
+{
+public:
+	static const int kIdStart;
+	static const int kIdVirtual;
+	static const int kIdInvalid;
+
+public:
+	Memory(const ParametersMap & parameters = ParametersMap());
+	virtual ~Memory();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual const ParametersMap & getParameters() const {return parameters_;}
+	bool update(const SensorData & data,
+			Statistics * stats = 0);
+	bool update(const SensorData & data,
+			const Transform & pose,
+			const cv::Mat & covariance,
+			const std::vector<float> & velocity = std::vector<float>(), // vx,vy,vz,vroll,vpitch,vyaw
+			Statistics * stats = 0);
+	bool init(const std::string & dbUrl,
+			bool dbOverwritten = false,
+			const ParametersMap & parameters = ParametersMap(),
+			bool postInitClosingEvents = false);
+	void close(bool databaseSaved = true, bool postInitClosingEvents = false, const std::string & ouputDatabasePath = "");
+	std::map<int, float> computeLikelihood(const Signature * signature,
+			const std::list<int> & ids);
+	int incrementMapId(std::map<int, int> * reducedIds = 0);
+	void updateAge(int signatureId);
+
+	std::list<int> forget(const std::set<int> & ignoredIds = std::set<int>());
+	std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
+
+	int cleanup();
+	void saveStatistics(const Statistics & statistics, bool saveWMState);
+	void savePreviewImage(const cv::Mat & image) const;
+	cv::Mat loadPreviewImage() const;
+	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
+	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
+	void saveOptimizedMesh(
+			const cv::Mat & cloud,
+			const std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > & polygons = std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > >(),      // Textures -> polygons -> vertices
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
+#endif
+			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
+	cv::Mat loadOptimizedMesh(
+			std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons = 0,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
+#endif
+			cv::Mat * textures = 0) const;
+	void emptyTrash();
+	void joinTrashThread();
+	bool addLink(const Link & link, bool addInDatabase = false);
+	void updateLink(const Link & link, bool updateInDatabase = false);
+	void removeAllVirtualLinks();
+	void removeVirtualLinks(int signatureId);
+	std::map<int, int> getNeighborsId(
+			int signatureId,
+			int maxGraphDepth,
+			int maxCheckedInDatabase = -1,
+			bool incrementMarginOnLoop = false,
+			bool ignoreLoopIds = false,
+			bool ignoreIntermediateNodes = false,
+			bool ignoreLocalSpaceLoopIds = false,
+			const std::set<int> & nodesSet = std::set<int>(),
+			double * dbAccessTime = 0) const;
+	std::map<int, float> getNeighborsIdRadius(
+			int signatureId,
+			float radius,
+			const std::map<int, Transform> & optimizedPoses,
+			int maxGraphDepth) const;
+	void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
+	void saveLocationData(int locationId);
+	void removeLink(int idA, int idB);
+	void removeRawData(int id, bool image = true, bool scan = true, bool userData = true);
+
+	//getters
+	const std::map<int, double> & getWorkingMem() const {return _workingMem;}
+	const std::set<int> & getStMem() const {return _stMem;}
+	int getMaxStMemSize() const {return _maxStMemSize;}
+	std::multimap<int, Link> getNeighborLinks(int signatureId,
+			bool lookInDatabase = false) const;
+	std::multimap<int, Link> getLoopClosureLinks(int signatureId,
+			bool lookInDatabase = false) const;
+	std::multimap<int, Link> getLinks(int signatureId, // can be also used to get links from landmarks
+			bool lookInDatabase = false,
+			bool withLandmarks = false) const;
+	std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true, bool withLandmarks = false) const;
+	bool isBinDataKept() const {return _binDataKept;}
+	float getSimilarityThreshold() const {return _similarityThreshold;}
+	std::map<int, int> getWeights() const;
+	int getLastSignatureId() const;
+	const Signature * getLastWorkingSignature() const;
+	std::map<int, Link> getNodesObservingLandmark(int landmarkId, bool lookInDatabase) const;
+	int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
+	bool labelSignature(int id, const std::string & label);
+	const std::map<int, std::string> & getAllLabels() const {return _labels;}
+	const std::map<int, std::set<int> > & getLandmarksIndex() const {return _landmarksIndex;}
+	bool allNodesInWM() const {return _allNodesInWM;}
+
+	/**
+	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
+	 * compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * If you have one dimension unsigned 8 bits raw data, make sure to transpose it
+	 * (to have multiple rows instead of multiple columns) in order to be detected as
+	 * not compressed.
+	 */
+	bool setUserData(int id, const cv::Mat & data);
+	int getDatabaseMemoryUsed() const; // in bytes
+	std::string getDatabaseVersion() const;
+	std::string getDatabaseUrl() const;
+	double getDbSavingTime() const;
+	int getMapId(int id, bool lookInDatabase = false) const;
+	Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
+	Transform getGroundTruthPose(int signatureId, bool lookInDatabase = false) const;
+	const std::map<int, Transform> & getGroundTruths() const {return _groundTruths;} // only those in working+STM memory
+	void getGPS(int id, GPS & gps, Transform & offsetENU, bool lookInDatabase, int maxGraphDepth = 0) const;
+	bool getNodeInfo(int signatureId,
+			Transform & odomPose,
+			int & mapId,
+			int & weight,
+			std::string & label,
+			double & stamp,
+			Transform & groundTruth,
+			std::vector<float> & velocity,
+			GPS & gps,
+			EnvSensors & sensors,
+			bool lookInDatabase = false) const;
+	cv::Mat getImageCompressed(int signatureId) const;
+	SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const;
+	void getNodeWordsAndGlobalDescriptors(int nodeId,
+			std::multimap<int, int> & words,
+			std::vector<cv::KeyPoint> & wordsKpts,
+			std::vector<cv::Point3f> & words3,
+			cv::Mat & wordsDescriptors,
+			std::vector<GlobalDescriptor> & globalDescriptors) const;
+	void getNodeCalibration(int nodeId,
+			std::vector<CameraModel> & models,
+			std::vector<StereoCameraModel> & stereoModels) const;
+	std::set<int> getAllSignatureIds(bool ignoreChildren = true) const;
+	bool memoryChanged() const {return _memoryChanged;}
+	bool isIncremental() const {return _incrementalMemory;}
+	bool isLocalizationDataSaved() const {return _localizationDataSaved;}
+	const Signature * getSignature(int id) const;
+	bool isInSTM(int signatureId) const {return _stMem.find(signatureId) != _stMem.end();}
+	bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
+	bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
+	bool isIDsGenerated() const {return _generateIds;}
+	int getLastGlobalLoopClosureId() const {return _lastGlobalLoopClosureId;}
+	const Feature2D * getFeature2D() const {return _feature2D;}
+	bool isGraphReduced() const {return _reduceGraph;}
+	const std::vector<double> & getOdomMaxInf() const {return _odomMaxInf;}
+	bool isOdomGravityUsed() const {return _useOdometryGravity;}
+
+	void dumpMemoryTree(const char * fileNameTree) const;
+	virtual void dumpMemory(std::string directory) const;
+	virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
+	void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
+	unsigned long getMemoryUsed() const; //Bytes
+
+	void generateGraph(const std::string & fileName, const std::set<int> & ids = std::set<int>());
+	int cleanupLocalGrids(
+			const std::map<int, Transform> & poses,
+			const cv::Mat & map,
+			float xMin,
+			float yMin,
+			float cellSize,
+			int cropRadius = 1,
+			bool filterScans = false);
+
+	//keypoint stuff
+	const VWDictionary * getVWDictionary() const;
+
+	// RGB-D stuff
+	void getMetricConstraints(
+			const std::set<int> & ids,
+			std::map<int, Transform> & poses,
+			std::multimap<int, Link> & links,
+			bool lookInDatabase = false,
+			bool landmarksAdded = false);
+
+	Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
+	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
+	Transform computeIcpTransform(const Signature & fromS, const Signature & toS, Transform guess, RegistrationInfo * info = 0) const;
+	Transform computeIcpTransformMulti(
+			int newId,
+			int oldId,
+			const std::map<int, Transform> & poses,
+			RegistrationInfo * info = 0);
+
+private:
+	void preUpdate();
+	void addSignatureToStm(Signature * signature, const cv::Mat & covariance);
+	void clear();
+	void loadDataFromDb(bool postInitClosingEvents);
+	void moveToTrash(Signature * s, bool keepLinkedToGraph = true, std::list<int> * deletedWords = 0);
+
+	void moveSignatureToWMFromSTM(int id, int * reducedTo = 0);
+	void addSignatureToWmFromLTM(Signature * signature);
+	Signature * _getSignature(int id) const;
+	std::list<Signature *> getRemovableSignatures(int count,
+			const std::set<int> & ignoredIds = std::set<int>());
+	int getNextId();
+	void initCountId();
+	void rehearsal(Signature * signature, Statistics * stats = 0);
+	bool rehearsalMerge(int oldId, int newId);
+
+	const std::map<int, Signature*> & getSignatures() const {return _signatures;}
+
+	void copyData(const Signature * from, Signature * to);
+	Signature * createSignature(
+			const SensorData & data,
+			const Transform & pose,
+			Statistics * stats = 0);
+
+	//keypoint stuff
+	void disableWordsRef(int signatureId);
+	void enableWordsRef(const std::list<int> & signatureIds);
+	void cleanUnusedWords();
+	int getNi(int signatureId) const;
+
+protected:
+	DBDriver * _dbDriver;
+
+private:
+	// parameters
+	ParametersMap parameters_;
+	float _similarityThreshold;
+	bool _binDataKept;
+	bool _rawDescriptorsKept;
+	bool _saveDepth16Format;
+	bool _notLinkedNodesKeptInDb;
+	bool _saveIntermediateNodeData;
+	std::string _rgbCompressionFormat;
+	std::string _depthCompressionFormat;
+	bool _incrementalMemory;
+	bool _localizationDataSaved;
+	bool _reduceGraph;
+	int _maxStMemSize;
+	float _recentWmRatio;
+	bool _transferSortingByWeightId;
+	bool _idUpdatedToNewOneRehearsal;
+	bool _generateIds;
+	bool _badSignaturesIgnored;
+	bool _mapLabelsAdded;
+	bool _depthAsMask;
+	float _maskFloorThreshold;
+	bool _stereoFromMotion;
+	unsigned int _imagePreDecimation;
+	unsigned int _imagePostDecimation;
+	bool _compressionParallelized;
+	float _laserScanDownsampleStepSize;
+	float _laserScanVoxelSize;
+	int _laserScanNormalK;
+	float _laserScanNormalRadius;
+	float _laserScanGroundNormalsUp;
+	bool _reextractLoopClosureFeatures;
+	bool _localBundleOnLoopClosure;
+	bool _invertedReg;
+	float _rehearsalMaxDistance;
+	float _rehearsalMaxAngle;
+	bool _rehearsalWeightIgnoredWhileMoving;
+	bool _useOdometryFeatures;
+	bool _useOdometryGravity;
+	bool _rotateImagesUpsideUp;
+	bool _createOccupancyGrid;
+	int _visMaxFeatures;
+	bool _visSSC;
+	bool _imagesAlreadyRectified;
+	bool _rectifyOnlyFeatures;
+	bool _covOffDiagonalIgnored;
+	bool _detectMarkers;
+	float _markerLinVariance;
+	float _markerAngVariance;
+	bool _markerOrientationIgnored;
+
+	int _idCount;
+	int _idMapCount;
+	Signature * _lastSignature;
+	int _lastGlobalLoopClosureId;
+	bool _memoryChanged; // False by default, become true only when Memory::update() is called.
+	bool _linksChanged; // False by default, become true when links are modified.
+	int _signaturesAdded;
+	bool _allNodesInWM;
+	GPS _gpsOrigin;
+	std::vector<CameraModel> _rectCameraModels;
+	std::vector<StereoCameraModel> _rectStereoCameraModels;
+	std::vector<double> _odomMaxInf;
+
+	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
+	std::set<int> _stMem; // id
+	std::map<int, double> _workingMem; // id,age
+	std::map<int, Transform> _groundTruths;
+	std::map<int, std::string> _labels;
+	std::map<int, std::set<int> > _landmarksIndex; // < -landmarkId, nodeIds >
+    std::map<int, float> _landmarksSize;           // +landmarkId
+
+	//Keypoint stuff
+	VWDictionary * _vwd;
+	Feature2D * _feature2D;
+	float _badSignRatio;
+	bool _tfIdfLikelihoodUsed;
+	bool _parallelized;
+
+	Registration * _registrationPipeline;
+	RegistrationIcp * _registrationIcpMulti;
+	RegistrationVis * _registrationVis;
+
+	LocalGridMaker * _localMapMaker;
+
+	MarkerDetector * _markerDetector;
+
+	GlobalDescriptorExtractor * _globalDescriptorExtractor;
+};
+
+} // namespace rtabmap
+
+#endif /* MEMORY_H_ */

rtabmap/corelib/include/rtabmap/core/OccupancyGrid.h

@@ -0,0 +1,38 @@
+/*
+Copyright (c) 2010-2023, 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.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_OCCUPANCYGRID_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_OCCUPANCYGRID_H_
+
+/*
+ * Deprecated header, use the one below directly!
+ */
+
+#include <rtabmap/core/global_map/OccupancyGrid.h>
+
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_OCCUPANCYGRID_H_ */

rtabmap/corelib/include/rtabmap/core/OctoMap.h

@@ -0,0 +1,38 @@
+/*
+Copyright (c) 2010-2023, 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.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_OCTOMAP_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_OCTOMAP_H_
+
+/*
+ * Deprecated header, use the one below directly!
+ */
+
+#include <rtabmap/core/global_map/OctoMap.h>
+
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_OCTOMAP_H_ */

rtabmap/corelib/include/rtabmap/core/Odometry.h

@@ -0,0 +1,136 @@
+/*
+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.
+*/
+
+#ifndef ODOMETRY_H_
+#define ODOMETRY_H_
+
+#include <rtabmap/core/rtabmap_core_export.h>
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Parameters.h>
+
+namespace rtabmap {
+
+class OdometryInfo;
+class ParticleFilter;
+
+class RTABMAP_CORE_EXPORT Odometry
+{
+public:
+	enum Type {
+		kTypeUndef = -1,
+		kTypeF2M = 0,
+		kTypeF2F = 1,
+		kTypeFovis = 2,
+		kTypeViso2 = 3,
+		kTypeDVO = 4,
+		kTypeORBSLAM = 5,
+		kTypeOkvis = 6,
+		kTypeLOAM = 7,
+		kTypeMSCKF = 8,
+		kTypeVINS = 9,
+		kTypeOpenVINS = 10,
+		kTypeFLOAM = 11,
+		kTypeOpen3D = 12
+	};
+
+public:
+	static Odometry * create(const ParametersMap & parameters = ParametersMap());
+	static Odometry * create(Type & type, const ParametersMap & parameters = ParametersMap());
+
+public:
+	virtual ~Odometry();
+	Transform process(SensorData & data, OdometryInfo * info = 0);
+	Transform process(SensorData & data, const Transform & guess, OdometryInfo * info = 0);
+	virtual void reset(const Transform & initialPose = Transform::getIdentity());
+	virtual Odometry::Type getType() = 0;
+	virtual bool canProcessRawImages() const {return false;}
+	virtual bool canProcessAsyncIMU() const {return false;}
+
+	//getters
+	const Transform & getPose() const {return _pose;}
+	bool isInfoDataFilled() const {return _fillInfoData;}
+	// Use getVelocityGuess() instead.
+	RTABMAP_DEPRECATED const Transform & previousVelocityTransform() const;
+	const Transform & getVelocityGuess() const {return velocityGuess_;}
+	double previousStamp() const {return previousStamp_;}
+	unsigned int framesProcessed() const {return framesProcessed_;}
+	bool imagesAlreadyRectified() const {return _imagesAlreadyRectified;}
+
+protected:
+	const std::map<double, Transform> & imus() const {return imus_;}
+
+private:
+	virtual Transform computeTransform(SensorData & data, const Transform & guess = Transform(), OdometryInfo * info = 0) = 0;
+
+	void initKalmanFilter(const Transform & initialPose = Transform::getIdentity(), float vx=0.0f, float vy=0.0f, float vz=0.0f, float vroll=0.0f, float vpitch=0.0f, float vyaw=0.0f);
+	void predictKalmanFilter(float dt, float * vx=0, float * vy=0, float * vz=0, float * vroll=0, float * vpitch=0, float * vyaw=0);
+	void updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw);
+
+private:
+	int _resetCountdown;
+	bool _force3DoF;
+	bool _holonomic;
+	bool guessFromMotion_;
+	float guessSmoothingDelay_;
+	int _filteringStrategy;
+	int _particleSize;
+	float _particleNoiseT;
+	float _particleLambdaT;
+	float _particleNoiseR;
+	float _particleLambdaR;
+	bool _fillInfoData;
+	float _kalmanProcessNoise;
+	float _kalmanMeasurementNoise;
+	unsigned int _imageDecimation;
+	bool _alignWithGround;
+	bool _publishRAMUsage;
+	bool _imagesAlreadyRectified;
+	bool _deskewing;
+	Transform _pose;
+	int _resetCurrentCount;
+	double previousStamp_;
+	std::list<std::pair<std::vector<float>, double> > previousVelocities_;
+	Transform velocityGuess_;
+	Transform imuLastTransform_;
+	Transform previousGroundTruthPose_;
+	float distanceTravelled_;
+	unsigned int framesProcessed_;
+
+	std::vector<ParticleFilter *> particleFilters_;
+	cv::KalmanFilter kalmanFilter_;
+	std::vector<StereoCameraModel> stereoModels_;
+	std::vector<CameraModel> models_;
+	std::map<double, Transform> imus_;
+
+protected:
+	Odometry(const rtabmap::ParametersMap & parameters);
+};
+
+} /* namespace rtabmap */
+#endif /* ODOMETRY_H_ */

rtabmap/corelib/include/rtabmap/core/OdometryEvent.h

@@ -0,0 +1,111 @@
+/*
+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.
+*/
+
+#ifndef ODOMETRYEVENT_H_
+#define ODOMETRYEVENT_H_
+
+#include "rtabmap/utilite/UEvent.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UMath.h"
+#include "rtabmap/core/SensorData.h"
+#include "rtabmap/core/OdometryInfo.h"
+
+namespace rtabmap {
+
+class OdometryEvent : public UEvent
+{
+public:
+	OdometryEvent()
+	{
+		_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
+	}
+	OdometryEvent(
+		const SensorData & data,
+		const Transform & pose,
+		const OdometryInfo & info = OdometryInfo()) :
+			_data(data),
+			_pose(pose),
+			_info(info)
+	{
+		if(_info.reg.covariance.empty())
+		{
+			_info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1);
+		}
+		UASSERT(_info.reg.covariance.cols == 6 && _info.reg.covariance.rows == 6 && _info.reg.covariance.type() == CV_64FC1);
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(0,0)) && _info.reg.covariance.at<double>(0,0)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(1,1)) && _info.reg.covariance.at<double>(1,1)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(2,2)) && _info.reg.covariance.at<double>(2,2)>0, "Transitional variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(3,3)) && _info.reg.covariance.at<double>(3,3)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(4,4)) && _info.reg.covariance.at<double>(4,4)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+		UASSERT_MSG(uIsFinite(_info.reg.covariance.at<double>(5,5)) && _info.reg.covariance.at<double>(5,5)>0, "Rotational variance should not be null! (set to 1 if unknown)");
+	}
+	virtual ~OdometryEvent() {}
+	virtual std::string getClassName() const {return "OdometryEvent";}
+
+	SensorData & data() {return _data;}
+	const SensorData & data() const {return _data;}
+	const Transform & pose() const {return _pose;}
+	const cv::Mat & covariance() const {return _info.reg.covariance;}
+	std::vector<float> velocity() const {
+		if(_info.interval>0.0)
+		{
+			std::vector<float> velocity(6,0);
+			float x,y,z,roll,pitch,yaw;
+			_info.transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+			velocity[0] = x/_info.interval;
+			velocity[1] = y/_info.interval;
+			velocity[2] = z/_info.interval;
+			velocity[3] = roll/_info.interval;
+			velocity[4] = pitch/_info.interval;
+			velocity[5] = yaw/_info.interval;
+			return velocity;
+		}
+		return std::vector<float>();
+	}
+	const OdometryInfo & info() const {return _info;}
+
+private:
+	SensorData _data;
+	Transform _pose;
+	OdometryInfo _info;
+};
+
+class OdometryResetEvent : public UEvent
+{
+public:
+	OdometryResetEvent(const Transform & pose = Transform::getIdentity()){_pose = pose;}
+	virtual ~OdometryResetEvent() {}
+	virtual std::string getClassName() const {return "OdometryResetEvent";}
+	const Transform & getPose() const {return _pose;}
+private:
+	Transform _pose;
+};
+
+}
+
+
+#endif /* ODOMETRYEVENT_H_ */

rtabmap/corelib/include/rtabmap/core/OdometryInfo.h

@@ -0,0 +1,93 @@
+/*
+Copyright (c) 2010-2025, 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.
+*/
+
+#ifndef ODOMETRYINFO_H_
+#define ODOMETRYINFO_H_
+
+#include <rtabmap/core/rtabmap_core_export.h>
+#include <map>
+#include "rtabmap/core/Transform.h"
+#include "rtabmap/core/RegistrationInfo.h"
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/LaserScan.h"
+#include <opencv2/features2d/features2d.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT OdometryInfo
+{
+public:
+	OdometryInfo();
+	OdometryInfo copyWithoutData() const;
+	std::map<std::string, float> statistics(const Transform & pose = Transform());
+
+	bool lost;
+	RegistrationInfo reg;
+	int features;
+	int localMapSize;
+	int localScanMapSize;
+	int localKeyFrames;
+	int localBundleOutliers;
+	int localBundleConstraints;
+	float localBundleTime;
+	std::map<int, Transform> localBundlePoses;
+	std::map<int, std::vector<CameraModel> > localBundleModels;
+	float localBundleAvgInlierDistance;
+	int localBundleMaxKeyFramesForInlier;
+	std::vector<int> localBundleOutliersPerCam;
+	bool keyFrameAdded;
+	float timeDeskewing;
+	float timeEstimation;
+	float timeParticleFiltering;
+	double stamp;
+	double interval;
+	Transform transform;
+	Transform transformFiltered;
+	Transform transformGroundTruth;
+	Transform guessVelocity; // deprecated, will be removed. Use guess and interval instead.
+	Transform guess;
+	float distanceTravelled;
+	int memoryUsage; //MB
+	double gravityRollError;
+	double gravityPitchError;
+
+	int type;
+
+	// F2M
+	std::multimap<int, cv::KeyPoint> words;
+	std::map<int, cv::Point3f> localMap;
+	LaserScan localScanMap;
+
+	// F2F
+	std::vector<cv::Point2f> refCorners;
+	std::vector<cv::Point2f> newCorners;
+	std::vector<int> cornerInliers;
+};
+
+}
+
+#endif /* ODOMETRYINFO_H_ */

rtabmap/corelib/include/rtabmap/core/OdometryThread.h

@@ -0,0 +1,77 @@
+/*
+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.
+*/
+
+#ifndef ODOMETRYTHREAD_H_
+#define ODOMETRYTHREAD_H_
+
+#include <rtabmap/core/rtabmap_core_export.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <list>
+
+namespace rtabmap {
+
+class Odometry;
+
+class RTABMAP_CORE_EXPORT OdometryThread : public UThread, public UEventsHandler {
+public:
+	// take ownership of Odometry
+	OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize = 1);
+	virtual ~OdometryThread();
+
+protected:
+	virtual bool handleEvent(UEvent * event);
+
+private:
+	virtual void mainLoopBegin();
+	virtual void mainLoopKill();
+
+	//============================================================
+	// MAIN LOOP
+	//============================================================
+	virtual void mainLoop();
+	void addData(const SensorData & data);
+	bool getData(SensorData & data);
+
+private:
+	USemaphore _dataAdded;
+	UMutex _dataMutex;
+	std::list<SensorData> _dataBuffer;
+	std::list<SensorData> _imuBuffer;
+	Odometry * _odometry;
+	unsigned int _dataBufferMaxSize;
+	bool _resetOdometry;
+	Transform _resetPose;
+	double _oldestAsyncImuStamp;
+	double _newestAsyncImuStamp;
+};
+
+} // namespace rtabmap
+
+
+#endif /* ODOMETRYTHREAD_H_ */

rtabmap/corelib/include/rtabmap/core/Optimizer.h

@@ -0,0 +1,204 @@
+/*
+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.
+*/
+
+#ifndef OPTIMIZER_H_
+#define OPTIMIZER_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <map>
+#include <list>
+#include <rtabmap/core/Link.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Signature.h>
+
+namespace rtabmap {
+
+class FeatureBA
+{
+public:
+	FeatureBA(const cv::KeyPoint & kptIn, const float & depthIn = 0.0f, const cv::Mat & descriptorIn = cv::Mat(), int cameraIndexIn = 0):
+		kpt(kptIn),
+		depth(depthIn),
+		descriptor(descriptorIn),
+		cameraIndex(cameraIndexIn)
+	{
+		//UDEBUG("kpt=(%f,%f) depth=%f, camIndex=%d", kpt.pt.x, kpt.pt.y, depth, cameraIndex);
+	}
+	cv::KeyPoint kpt;
+	float depth;
+	cv::Mat descriptor;
+	int cameraIndex;
+};
+
+////////////////////////////////////////////
+// Graph optimizers
+////////////////////////////////////////////
+class RTABMAP_CORE_EXPORT Optimizer
+{
+public:
+	enum Type {
+		kTypeUndef = -1,
+		kTypeTORO = 0,
+		kTypeG2O = 1,
+		kTypeGTSAM = 2,
+		kTypeCeres = 3,
+		kTypeCVSBA = 4
+	};
+	static bool isAvailable(Optimizer::Type type);
+	static Optimizer * create(const ParametersMap & parameters);
+	static Optimizer * create(Optimizer::Type type, const ParametersMap & parameters = ParametersMap());
+
+	// Get connected poses and constraints from a set of links
+	void getConnectedGraph(
+			int fromId,
+			const std::map<int, Transform> & posesIn,
+			const std::multimap<int, Link> & linksIn,
+			std::map<int, Transform> & posesOut,
+			std::multimap<int, Link> & linksOut) const;
+
+public:
+	virtual ~Optimizer() {}
+
+	virtual Type type() const = 0;
+
+	// getters
+	int iterations() const {return iterations_;}
+	bool isSlam2d() const {return slam2d_;}
+	bool isCovarianceIgnored() const {return covarianceIgnored_;}
+	double epsilon() const {return epsilon_;}
+	bool isRobust() const {return robust_;}
+	bool priorsIgnored() const {return priorsIgnored_;}
+	bool landmarksIgnored() const {return landmarksIgnored_;}
+	float gravitySigma() const {return gravitySigma_;}
+
+	// setters
+	void setIterations(int iterations) {iterations_ = iterations;}
+	void setSlam2d(bool enabled) {slam2d_ = enabled;}
+	void setCovarianceIgnored(bool enabled) {covarianceIgnored_ = enabled;}
+	void setEpsilon(double epsilon) {epsilon_ = epsilon;}
+	void setRobust(bool enabled) {robust_ = enabled;}
+	void setPriorsIgnored(bool enabled) {priorsIgnored_ = enabled;}
+	void setLandmarksIgnored(bool enabled) {landmarksIgnored_ = enabled;}
+	void setGravitySigma(float value) {gravitySigma_ = value;}
+
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	std::map<int, Transform> optimizeIncremental(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & constraints,
+			std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+			double * finalError = 0,
+			int * iterationsDone = 0);
+
+	std::map<int, Transform> optimize(
+				int rootId,
+				const std::map<int, Transform> & poses,
+				const std::multimap<int, Link> & constraints,
+				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+				double * finalError = 0,
+				int * iterationsDone = 0);
+
+	// inherited classes should implement one of these methods
+	virtual std::map<int, Transform> optimize(
+				int rootId,
+				const std::map<int, Transform> & poses,
+				const std::multimap<int, Link> & constraints,
+				cv::Mat & outputCovariance,
+				std::list<std::map<int, Transform> > * intermediateGraphes = 0,
+				double * finalError = 0,
+				int * iterationsDone = 0);
+	virtual std::map<int, Transform> optimizeBA(
+			int rootId, // if negative, all other poses are fixed
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
+			std::map<int, cv::Point3f> & points3DMap,
+			const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
+			std::set<int> * outliers = 0);
+
+	std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures,
+			std::map<int, cv::Point3f> & points3DMap,
+			std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
+			bool rematchFeatures = false,
+			const ParametersMap & registrationParameters = ParametersMap());
+
+	std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures,
+			bool rematchFeatures = false,
+			const ParametersMap & registrationParameters = ParametersMap());
+
+	Transform optimizeBA(
+			const Link & link,
+			const CameraModel & model,
+			std::map<int, cv::Point3f> & points3DMap,
+			const std::map<int, std::map<int, FeatureBA> > & wordReferences,
+			std::set<int> * outliers = 0);
+
+	void computeBACorrespondences(
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, Signature> & signatures,
+			std::map<int, cv::Point3f> & points3DMap,
+			std::map<int, std::map<int, FeatureBA > > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
+			bool rematchFeatures = false,
+			bool useLinkTransformAsGuess = false,
+			ParametersMap registrationParameters = ParametersMap());
+
+protected:
+	Optimizer(
+			int iterations         = Parameters::defaultOptimizerIterations(),
+			bool slam2d            = Parameters::defaultRegForce3DoF(),
+			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
+			double epsilon         = Parameters::defaultOptimizerEpsilon(),
+			bool robust            = Parameters::defaultOptimizerRobust(),
+			bool priorsIgnored     = Parameters::defaultOptimizerPriorsIgnored(),
+			bool landmarksIgnored  = Parameters::defaultOptimizerLandmarksIgnored(),
+			float gravitySigma     = Parameters::defaultOptimizerGravitySigma());
+	Optimizer(const ParametersMap & parameters);
+
+private:
+	int iterations_;
+	bool slam2d_;
+	bool covarianceIgnored_;
+	double epsilon_;
+	bool robust_;
+	bool priorsIgnored_;
+	bool landmarksIgnored_;
+	float gravitySigma_;
+};
+
+} /* namespace rtabmap */
+#endif /* OPTIMIZER_H_ */

rtabmap/corelib/include/rtabmap/core/PDALWriter.h

@@ -0,0 +1,47 @@
+/*
+Copyright (c) 2010-2021, 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.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_PDALWRITER_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_PDALWRITER_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace rtabmap {
+
+std::string getPDALSupportedWriters();
+
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud,          const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud,       const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false, const std::vector<float> & intensities = std::vector<float>());
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false, const std::vector<float> & intensities = std::vector<float>());
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud,         const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud,   const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+
+}
+
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_PDALWRITER_H_ */

rtabmap/corelib/include/rtabmap/core/ParamEvent.h

@@ -0,0 +1,61 @@
+/*
+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.
+*/
+
+#ifndef PARAMEVENT_H_
+#define PARAMEVENT_H_
+
+#include "rtabmap/core/Parameters.h"
+#include <rtabmap/utilite/UEvent.h>
+
+namespace rtabmap
+{
+
+/**
+ * The parameters event. This event is used to send
+ * parameters across the threads.
+ */
+class ParamEvent : public UEvent
+{
+public:
+	ParamEvent(const ParametersMap & parameters) : UEvent(0), parameters_(parameters) {}
+	ParamEvent(const std::string & parameterKey, const std::string & parameterValue) : UEvent(0)
+	{
+		parameters_.insert(std::pair<std::string, std::string>(parameterKey, parameterValue));
+	}
+	~ParamEvent() {}
+	virtual std::string getClassName() const {return "ParamEvent";}
+
+	const ParametersMap & getParameters() const {return parameters_;}
+
+private:
+	ParametersMap parameters_; /**< The parameters map (key,value). */
+};
+
+}
+
+#endif /* PARAMEVENT_H_ */
+

rtabmap/corelib/include/rtabmap/core/Parameters.h

@@ -0,0 +1,986 @@
+/*
+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.
+*/
+
+#ifndef PARAMETERS_H_
+#define PARAMETERS_H_
+
+// default parameters
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+#include "rtabmap/core/Version.h" // DLL export/import defines
+#include <rtabmap/utilite/UConversion.h>
+#include <opencv2/core/version.hpp>
+#include <opencv2/opencv_modules.hpp>
+#include <string>
+#include <map>
+
+namespace rtabmap
+{
+
+typedef std::map<std::string, std::string> ParametersMap; // Key, value
+typedef std::pair<std::string, std::string> ParametersPair;
+
+/**
+ * Macro used to create parameter's key and default value.
+ * This macro must be used only in the Parameters class definition (in this file).
+ * They are automatically added to the default parameters map of the class Parameters.
+ * Example:
+ * @code
+ *         //for PARAM(Video, ImageWidth, int, 640), the output will be :
+ *         public:
+ *             static std::string kVideoImageWidth() {return std::string("Video/ImageWidth");}
+ *             static int defaultVideoImageWidth() {return 640;}
+ *         private:
+ *             class DummyVideoImageWidth {
+ *             public:
+ *                 DummyVideoImageWidth() {parameters_.insert(ParametersPair("Video/ImageWidth", "640"));}
+ *             };
+ *             DummyVideoImageWidth dummyVideoImageWidth;
+ * @endcode
+ */
+#define RTABMAP_PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE, DESCRIPTION) \
+    public: \
+        static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
+        static TYPE default##PREFIX##NAME() {return (TYPE)DEFAULT_VALUE;} \
+        static std::string type##PREFIX##NAME() {return std::string(#TYPE);} \
+    private: \
+        class Dummy##PREFIX##NAME { \
+        public: \
+            Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, #DEFAULT_VALUE)); \
+                                   parametersType_.insert(ParametersPair(#PREFIX "/" #NAME, #TYPE)); \
+                                   descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
+        }; \
+        Dummy##PREFIX##NAME dummy##PREFIX##NAME
+// end define PARAM
+
+/**
+ * It's the same as the macro PARAM but it should be used for string parameters.
+ * Macro used to create parameter's key and default value.
+ * This macro must be used only in the Parameters class definition (in this file).
+ * They are automatically added to the default parameters map of the class Parameters.
+ * Example:
+ * @code
+ *         //for PARAM_STR(Video, TextFileName, "Hello_world"), the output will be :
+ *         public:
+ *             static std::string kVideoFileName() {return std::string("Video/FileName");}
+ *             static std::string defaultVideoFileName() {return "Hello_world";}
+ *         private:
+ *             class DummyVideoFileName {
+ *             public:
+ *                 DummyVideoFileName() {parameters_.insert(ParametersPair("Video/FileName", "Hello_world"));}
+ *             };
+ *             DummyVideoFileName dummyVideoFileName;
+ * @endcode
+ */
+#define RTABMAP_PARAM_STR(PREFIX, NAME, DEFAULT_VALUE, DESCRIPTION) \
+    public: \
+        static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
+        static std::string default##PREFIX##NAME() {return DEFAULT_VALUE;} \
+        static std::string type##PREFIX##NAME() {return std::string("string");} \
+    private: \
+        class Dummy##PREFIX##NAME { \
+        public: \
+            Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, DEFAULT_VALUE)); \
+                                   parametersType_.insert(ParametersPair(#PREFIX "/" #NAME, "string")); \
+                                   descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
+        }; \
+        Dummy##PREFIX##NAME dummy##PREFIX##NAME
+// end define PARAM
+
+/**
+ * Macro used to create parameter's key and default value.
+ * This macro must be used only in the Parameters class definition (in this file).
+ * They are automatically added to the default parameters map of the class Parameters.
+ * Example:
+ * @code
+ *         //for PARAM(Video, ImageWidth, int, 640), the output will be :
+ *         public:
+ *             static std::string kVideoImageWidth() {return std::string("Video/ImageWidth");}
+ *             static int defaultVideoImageWidth() {return 640;}
+ *         private:
+ *             class DummyVideoImageWidth {
+ *             public:
+ *                 DummyVideoImageWidth() {parameters_.insert(ParametersPair("Video/ImageWidth", "640"));}
+ *             };
+ *             DummyVideoImageWidth dummyVideoImageWidth;
+ * @endcode
+ */
+#define RTABMAP_PARAM_COND(PREFIX, NAME, TYPE, COND, DEFAULT_VALUE1, DEFAULT_VALUE2, DESCRIPTION) \
+    public: \
+        static std::string k##PREFIX##NAME() {return std::string(#PREFIX "/" #NAME);} \
+        static TYPE default##PREFIX##NAME() {return COND?DEFAULT_VALUE1:DEFAULT_VALUE2;} \
+        static std::string type##PREFIX##NAME() {return std::string(#TYPE);} \
+    private: \
+        class Dummy##PREFIX##NAME { \
+        public: \
+            Dummy##PREFIX##NAME() {parameters_.insert(ParametersPair(#PREFIX "/" #NAME, COND?#DEFAULT_VALUE1:#DEFAULT_VALUE2)); \
+                                   parametersType_.insert(ParametersPair(#PREFIX "/" #NAME, #TYPE)); \
+                                   descriptions_.insert(ParametersPair(#PREFIX "/" #NAME, DESCRIPTION));} \
+        }; \
+        Dummy##PREFIX##NAME dummy##PREFIX##NAME
+// end define PARAM
+
+/**
+ * Class Parameters.
+ * This class is used to manage all custom parameters
+ * we want in the application. It was designed to be very easy to add
+ * a new parameter (just by adding one line of code).
+ * The macro PARAM(PREFIX, NAME, TYPE, DEFAULT_VALUE) is
+ * used to create a parameter in this class. A parameter can be accessed after by
+ * Parameters::defaultPARAMETERNAME() for the default value, Parameters::kPARAMETERNAME for his key (parameter name).
+ * The class provides also a general map containing all the parameter's key and
+ * default value. This map can be accessed anywhere in the application by
+ * Parameters::getDefaultParameters();
+ * Example:
+ * @code
+ *         //Defining a parameter in this class with the macro PARAM:
+ *         PARAM(Video, ImageWidth, int, 640);
+ *
+ *         // Now from anywhere in the application (Parameters is a singleton)
+ *         int width = Parameters::defaultVideoImageWidth(); // theDefaultValue = 640
+ *         std::string theKey = Parameters::kVideoImageWidth(); // theKey = "Video/ImageWidth"
+ *         std::string strValue = Util::value(Parameters::getDefaultParameters(), theKey); // strValue = "640"
+ * @endcode
+ * @see getDefaultParameters()
+ * TODO Add a detailed example with simple classes
+ */
+class RTABMAP_CORE_EXPORT Parameters
+{
+    // Rtabmap parameters
+    RTABMAP_PARAM(Rtabmap, PublishStats,                 bool, true,  "Publishing statistics.");
+    RTABMAP_PARAM(Rtabmap, PublishLastSignature,         bool, true,  "Publishing last signature.");
+    RTABMAP_PARAM(Rtabmap, PublishPdf,                   bool, true,  "Publishing pdf.");
+    RTABMAP_PARAM(Rtabmap, PublishLikelihood,            bool, true,  "Publishing likelihood.");
+    RTABMAP_PARAM(Rtabmap, PublishRAMUsage,              bool, false, "Publishing RAM usage in statistics (may add a small overhead to get info from the system).");
+    RTABMAP_PARAM(Rtabmap, ComputeRMSE,                  bool, true,  "Compute root mean square error (RMSE) and publish it in statistics, if ground truth is provided.");
+    RTABMAP_PARAM(Rtabmap, SaveWMState,                  bool, false, "Save working memory state after each update in statistics.");
+    RTABMAP_PARAM(Rtabmap, TimeThr,                      float, 0,    "Maximum time allowed for map update (ms) (0 means infinity). When map update time exceeds this fixed time threshold, some nodes in Working Memory (WM) are transferred to Long-Term Memory to limit the size of the WM and decrease the update time.");
+    RTABMAP_PARAM(Rtabmap, MemoryThr,                    int, 0,      uFormat("Maximum nodes in the Working Memory (0 means infinity). Similar to \"%s\", when the number of nodes in Working Memory (WM) exceeds this treshold, some nodes are transferred to Long-Term Memory to keep WM size fixed.", kRtabmapTimeThr().c_str()));
+    RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1,    "Detection rate (Hz). RTAB-Map will filter input images to satisfy this rate.");
+    RTABMAP_PARAM(Rtabmap, ImageBufferSize,          unsigned int, 1, "Data buffer size (0 min inf).");
+    RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
+    RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory,         "",          "Working directory.");
+    RTABMAP_PARAM(Rtabmap, MaxRetrieved,             unsigned int, 2, "Maximum nodes retrieved at the same time from LTM.");
+    RTABMAP_PARAM(Rtabmap, MaxRepublished,           unsigned int, 2, uFormat("Maximum nodes republished when requesting missing data. When %s=false, only loop closure data is republished, otherwise the closest nodes from the current localization are republished first. Ignored if %s=false.", kRGBDEnabled().c_str(), kRtabmapPublishLastSignature().c_str()));
+    RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true,  "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
+    RTABMAP_PARAM(Rtabmap, StatisticLogged,              bool, false, "Logging enabled.");
+    RTABMAP_PARAM(Rtabmap, StatisticLoggedHeaders,       bool, true,  "Add column header description to log files.");
+    RTABMAP_PARAM(Rtabmap, StartNewMapOnLoopClosure,     bool, false, "Start a new map only if there is a global loop closure with a previous map.");
+    RTABMAP_PARAM(Rtabmap, StartNewMapOnGoodSignature,   bool, false, uFormat("Start a new map only if the first signature is not bad (i.e., has enough features, see %s).", kKpBadSignRatio().c_str()));
+    RTABMAP_PARAM(Rtabmap, ImagesAlreadyRectified,       bool, true,  "Images are already rectified. By default RTAB-Map assumes that received images are rectified. If they are not, they can be rectified by RTAB-Map if this parameter is false.");
+    RTABMAP_PARAM(Rtabmap, RectifyOnlyFeatures,          bool, false,  uFormat("If \"%s\" is false and this parameter is true, the whole RGB image will not be rectified, only the features. Warning: As projection of RGB-D image to point cloud is assuming that images are rectified, the generated point cloud map will have wrong colors if this parameter is true.", kRtabmapImagesAlreadyRectified().c_str()));
+
+    // Hypotheses selection
+    RTABMAP_PARAM(Rtabmap, LoopThr,           float, 0.11,      "Loop closing threshold.");
+    RTABMAP_PARAM(Rtabmap, LoopRatio,         float, 0,         "The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
+    RTABMAP_PARAM(Rtabmap, LoopGPS,           bool,  true,      uFormat("Use GPS to filter likelihood (if GPS is recorded). Only locations inside the local radius \"%s\" of the current GPS location are considered for loop closure detection.", kRGBDLocalRadius().c_str()));
+    RTABMAP_PARAM(Rtabmap, VirtualPlaceLikelihoodRatio, int, 0,  "Likelihood ratio for virtual place (for no loop closure hypothesis): 0=Mean / StdDev, 1=StdDev / (Max-Mean)");
+
+    // Memory
+    RTABMAP_PARAM(Mem, RehearsalSimilarity,         float, 0.6,     "Rehearsal similarity.");
+    RTABMAP_PARAM(Mem, ImageKept,                   bool, false,    "Keep raw images in RAM.");
+    RTABMAP_PARAM(Mem, BinDataKept,                 bool, true,     "Keep binary data in db.");
+    RTABMAP_PARAM(Mem, RawDescriptorsKept,          bool, true,     "Raw descriptors kept in memory.");
+    RTABMAP_PARAM(Mem, MapLabelsAdded,              bool, true,     "Create map labels. The first node of a map will be labeled as \"map#\" where # is the map ID.");
+    RTABMAP_PARAM(Mem, SaveDepth16Format,           bool, false,    "Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
+    RTABMAP_PARAM(Mem, NotLinkedNodesKept,          bool, true,     "Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
+    RTABMAP_PARAM(Mem, IntermediateNodeDataKept,    bool, false,    "Keep intermediate node data in db.");
+    RTABMAP_PARAM_STR(Mem, ImageCompressionFormat,   ".jpg",        "RGB image compression format. It should be \".jpg\" or \".png\".");
+    RTABMAP_PARAM_STR(Mem, DepthCompressionFormat,   ".rvl",        "Depth image compression format for 16UC1 depth type. It should be \".png\" or \".rvl\". If depth type is 32FC1, \".png\" is used.");
+    RTABMAP_PARAM(Mem, STMSize,                   unsigned int, 10, "Short-term memory size.");
+    RTABMAP_PARAM(Mem, IncrementalMemory,           bool, true,     "SLAM mode, otherwise it is Localization mode.");
+    RTABMAP_PARAM(Mem, LocalizationDataSaved,       bool, false,     uFormat("Save localization data during localization session (when %s=false). When enabled, the database will then also grow in localization mode. This mode would be used only for debugging purpose.", kMemIncrementalMemory().c_str()).c_str());
+    RTABMAP_PARAM(Mem, ReduceGraph,                 bool, false,    "Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set).");
+    RTABMAP_PARAM(Mem, RecentWmRatio,               float, 0.2,     "Ratio of locations after the last loop closure in WM that cannot be transferred.");
+    RTABMAP_PARAM(Mem, TransferSortingByWeightId,   bool, false,    "On transfer, signatures are sorted by weight->ID only (i.e. the oldest of the lowest weighted signatures are transferred first). If false, the signatures are sorted by weight->Age->ID (i.e. the oldest inserted in WM of the lowest weighted signatures are transferred first). Note that retrieval updates the age, not the ID.");
+    RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne,  bool, false,    "On merge, update to new id. When false, no copy.");
+    RTABMAP_PARAM(Mem, RehearsalWeightIgnoredWhileMoving, bool, false, "When the robot is moving, weights are not updated on rehearsal.");
+    RTABMAP_PARAM(Mem, GenerateIds,                 bool, true,     "True=Generate location IDs, False=use input image IDs.");
+    RTABMAP_PARAM(Mem, BadSignaturesIgnored,        bool, false,    "Bad signatures are ignored.");
+    RTABMAP_PARAM(Mem, InitWMWithAllNodes,          bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.");
+    RTABMAP_PARAM(Mem, DepthAsMask,                 bool, true,     "Use depth image as mask when extracting features for vocabulary.");
+    RTABMAP_PARAM(Mem, DepthMaskFloorThr,           float, 0.0,     uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kMemDepthAsMask().c_str()));
+    RTABMAP_PARAM(Mem, StereoFromMotion,            bool, false,    uFormat("Triangulate features without depth using stereo from motion (odometry). It would be ignored if %s is true and the feature detector used supports masking.", kMemDepthAsMask().c_str()));
+    RTABMAP_PARAM(Mem, ImagePreDecimation,          unsigned int, 1, uFormat("Decimation of the RGB image before visual feature detection. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.",kMemDepthAsMask().c_str()));
+    RTABMAP_PARAM(Mem, ImagePostDecimation,         unsigned int, 1, uFormat("Decimation of the RGB image before saving it to database. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. Decimation is done from the original image. If set to same value than %s, data already decimated is saved (no need to re-decimate the image).", kMemImagePreDecimation().c_str()));
+    RTABMAP_PARAM(Mem, CompressionParallelized,     bool, true,     "Compression of sensor data is multi-threaded.");
+    RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,         "If > 1, downsample the laser scans when creating a signature.");
+    RTABMAP_PARAM(Mem, LaserScanVoxelSize,          float, 0.0,     uFormat("If > 0 m, voxel filtering is done on laser scans when creating a signature. If the laser scan had normals, they will be removed. To recompute the normals, make sure to use \"%s\" or \"%s\" parameters.", kMemLaserScanNormalK().c_str(), kMemLaserScanNormalRadius().c_str()));
+    RTABMAP_PARAM(Mem, LaserScanNormalK,            int, 0,         "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.");
+    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       float, 0.0,     "If > 0 m and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
+    RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, true,     "Use odometry features instead of regenerating them.");
+    RTABMAP_PARAM(Mem, UseOdomGravity,              bool, false,    uFormat("Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if \"%s\" is not zero.", kOptimizerGravitySigma().c_str()));
+    RTABMAP_PARAM(Mem, CovOffDiagIgnored,           bool, true,     "Ignore off diagonal values of the covariance matrix.");
+    RTABMAP_PARAM(Mem, GlobalDescriptorStrategy,    int, 0,        "Extract global descriptor from sensor data. 0=disabled, 1=PyDescriptor");
+    RTABMAP_PARAM(Mem, RotateImagesUpsideUp,        bool, false,    "Rotate images so that upside is up if they are not already. This can be useful in case the robots don't have all same camera orientation but are using the same map, so that not rotation-invariant visual features can still be used across the fleet.");
+
+    // KeypointMemory (Keypoint-based)
+    RTABMAP_PARAM(Kp, NNStrategy,               int, 1,       "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
+    RTABMAP_PARAM(Kp, IncrementalDictionary,    bool, true,   "");
+    RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor \"%s\" in size).", kKpFlannRebalancingFactor().c_str()));
+    RTABMAP_PARAM(Kp, FlannRebalancingFactor,   float, 2.0,   uFormat("Factor used when rebuilding the incremental FLANN index (see \"%s\"). Set <=1 to disable.", kKpIncrementalFlann().c_str()));
+    RTABMAP_PARAM(Kp, ByteToFloat,              bool, false,  uFormat("For %s=1, binary descriptors are converted to float by converting each byte to float instead of converting each bit to float. When converting bytes instead of bits, less memory is used and search is faster at the cost of slightly less accurate matching.", kKpNNStrategy().c_str()));
+    RTABMAP_PARAM(Kp, MaxDepth,                 float, 0,     "Filter extracted keypoints by depth (0=inf).");
+    RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");
+    RTABMAP_PARAM(Kp, MaxFeatures,              int, 500,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
+    RTABMAP_PARAM(Kp, SSC,                      bool, false,  "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
+    RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
+    RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
+#if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
+    // OpenCV>2 without xFeatures2D module doesn't have BRIEF
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+#else
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+#endif
+    RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,      bool, true,   "Use of the td-idf strategy to compute the likelihood.");
+    RTABMAP_PARAM(Kp, Parallelized,             bool, true,   "If the dictionary update and signature creation were parallelized.");
+    RTABMAP_PARAM_STR(Kp, RoiRatios,       "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
+    RTABMAP_PARAM_STR(Kp, DictionaryPath,       "",           "Path of the pre-computed dictionary");
+    RTABMAP_PARAM(Kp, NewWordsComparedTogether, bool, true,   "When adding new words to dictionary, they are compared also with each other (to detect same words in the same signature).");
+    RTABMAP_PARAM(Kp, SubPixWinSize,            int, 3,       "See cv::cornerSubPix().");
+    RTABMAP_PARAM(Kp, SubPixIterations,         int, 0,       "See cv::cornerSubPix(). 0 disables sub pixel refining.");
+    RTABMAP_PARAM(Kp, SubPixEps,                double, 0.02, "See cv::cornerSubPix().");
+    RTABMAP_PARAM(Kp, GridRows,                 int, 1,       uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kKpMaxFeatures().c_str()));
+    RTABMAP_PARAM(Kp, GridCols,                 int, 1,       uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kKpMaxFeatures().c_str()));
+
+    //Database
+    RTABMAP_PARAM(DbSqlite3, InMemory,     bool, false,      "Using database in the memory instead of a file on the hard disk.");
+    RTABMAP_PARAM(DbSqlite3, CacheSize, unsigned int, 10000, "Sqlite cache size (default is 2000).");
+    RTABMAP_PARAM(DbSqlite3, JournalMode,  int, 3,           "0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : \"PRAGMA journal_mode\")");
+    RTABMAP_PARAM(DbSqlite3, Synchronous,  int, 0,           "0=OFF, 1=NORMAL, 2=FULL (see sqlite3 doc : \"PRAGMA synchronous\")");
+    RTABMAP_PARAM(DbSqlite3, TempStore,    int, 2,           "0=DEFAULT, 1=FILE, 2=MEMORY (see sqlite3 doc : \"PRAGMA temp_store\")");
+    RTABMAP_PARAM_STR(Db, TargetVersion,   "",               "Target database version for backward compatibility purpose. Only Major and minor versions are used and should be set (e.g., 0.19 vs 0.20 or 1.0 vs 2.0). Patch version is ignored (e.g., 0.20.1 and 0.20.3 will generate a 0.20 database).");
+
+    // Keypoints descriptors/detectors
+    RTABMAP_PARAM(SURF, Extended,          bool, false,  "Extended descriptor flag (true - use extended 128-element descriptors; false - use 64-element descriptors).");
+    RTABMAP_PARAM(SURF, HessianThreshold,  float, 500,   "Threshold for hessian keypoint detector used in SURF.");
+    RTABMAP_PARAM(SURF, Octaves,           int, 4,       "Number of pyramid octaves the keypoint detector will use.");
+    RTABMAP_PARAM(SURF, OctaveLayers,      int, 2,       "Number of octave layers within each octave.");
+    RTABMAP_PARAM(SURF, Upright,           bool, false,  "Up-right or rotated features flag (true - do not compute orientation of features; false - compute orientation).");
+    RTABMAP_PARAM(SURF, GpuVersion,        bool, false,  "GPU-SURF: Use GPU version of SURF. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
+    RTABMAP_PARAM(SURF, GpuKeypointsRatio,  float, 0.01, "Used with SURF GPU.");
+
+    RTABMAP_PARAM(SIFT, NOctaveLayers,     int, 3,       "The number of layers in each octave. 3 is the value used in D. Lowe paper. The number of octaves is computed automatically from the image resolution. Not used by CudaSift, the number of octaves is still computed automatically.");
+    RTABMAP_PARAM(SIFT, ContrastThreshold, double, 0.04, uFormat("The contrast threshold used to filter out weak features in semi-uniform (low-contrast) regions. The larger the threshold, the less features are produced by the detector. Not used by CudaSift (see %s instead).", kSIFTGaussianThreshold().c_str()));
+    RTABMAP_PARAM(SIFT, EdgeThreshold,     double, 10,   "The threshold used to filter out edge-like features. Note that the its meaning is different from the contrastThreshold, i.e. the larger the edgeThreshold, the less features are filtered out (more features are retained).");
+    RTABMAP_PARAM(SIFT, Sigma,             double, 1.6,  "The sigma of the Gaussian applied to the input image at the octave #0. If your image is captured with a weak camera with soft lenses, you might want to reduce the number.");
+    RTABMAP_PARAM(SIFT, PreciseUpscale,    bool, false,  "Whether to enable precise upscaling in the scale pyramid (OpenCV >= 4.8).");
+    RTABMAP_PARAM(SIFT, RootSIFT,          bool, false,  "Apply RootSIFT normalization of the descriptors.");
+    RTABMAP_PARAM(SIFT, Gpu,               bool, false,  "CudaSift: Use GPU version of SIFT. This option is enabled only if RTAB-Map is built with CudaSift dependency and GPUs are detected.");
+    RTABMAP_PARAM(SIFT, GaussianThreshold, float, 2.0,   "CudaSift: Threshold on difference of Gaussians for feature pruning. The higher the threshold, the less features are produced by the detector.");
+    RTABMAP_PARAM(SIFT, Upscale,           bool, false,  "CudaSift: Whether to enable upscaling.");
+
+    RTABMAP_PARAM(BRIEF, Bytes,            int, 32,      "Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");
+
+    RTABMAP_PARAM(FAST, Threshold,          int, 20,      "Threshold on difference between intensity of the central pixel and pixels of a circle around this pixel.");
+    RTABMAP_PARAM(FAST, NonmaxSuppression,  bool, true,   "If true, non-maximum suppression is applied to detected corners (keypoints).");
+    RTABMAP_PARAM(FAST, Gpu,                bool, false,  "GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
+    RTABMAP_PARAM(FAST, GpuKeypointsRatio,  double, 0.05, "Used with FAST GPU.");
+    RTABMAP_PARAM(FAST, MinThreshold,       int, 7,       "Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
+    RTABMAP_PARAM(FAST, MaxThreshold,       int, 200,     "Maximum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
+    RTABMAP_PARAM(FAST, GridRows,           int, 0,       "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+    RTABMAP_PARAM(FAST, GridCols,           int, 0,       "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+    RTABMAP_PARAM(FAST, CV,                 int, 0,       "Enable FastCV implementation if non-zero (and RTAB-Map is built with FastCV support). Values should be 9 and 10.");
+
+    RTABMAP_PARAM(GFTT, QualityLevel,      double, 0.001, "");
+    RTABMAP_PARAM(GFTT, MinDistance,       double, 7,    "");
+    RTABMAP_PARAM(GFTT, BlockSize,         int, 3,       "");
+    RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false,  "");
+    RTABMAP_PARAM(GFTT, K,                 double, 0.04, "");
+    RTABMAP_PARAM(GFTT, Gpu,               bool, false,  "GPU-GFTT: Use GPU version of GFTT. This option is enabled only if OpenCV>=3 is built with CUDA and GPUs are detected.");
+
+    RTABMAP_PARAM(ORB, ScaleFactor,   float, 2,  "Pyramid decimation ratio, greater than 1. scaleFactor==2 means the classical pyramid, where each next level has 4x less pixels than the previous, but such a big scale factor will degrade feature matching scores dramatically. On the other hand, too close to 1 scale factor will mean that to cover certain scale range you will need more pyramid levels and so the speed will suffer.");
+    RTABMAP_PARAM(ORB, NLevels,       int, 3,      "The number of pyramid levels. The smallest level will have linear size equal to input_image_linear_size/pow(scaleFactor, nlevels).");
+    RTABMAP_PARAM(ORB, EdgeThreshold, int, 19,     "This is size of the border where the features are not detected. It should roughly match the patchSize parameter.");
+    RTABMAP_PARAM(ORB, FirstLevel,    int, 0,      "It should be 0 in the current implementation.");
+    RTABMAP_PARAM(ORB, WTA_K,         int, 2,      "The number of points that produce each element of the oriented BRIEF descriptor. The default value 2 means the BRIEF where we take a random point pair and compare their brightnesses, so we get 0/1 response. Other possible values are 3 and 4. For example, 3 means that we take 3 random points (of course, those point coordinates are random, but they are generated from the pre-defined seed, so each element of BRIEF descriptor is computed deterministically from the pixel rectangle), find point of maximum brightness and output index of the winner (0, 1 or 2). Such output will occupy 2 bits, and therefore it will need a special variant of Hamming distance, denoted as NORM_HAMMING2 (2 bits per bin). When WTA_K=4, we take 4 random points to compute each bin (that will also occupy 2 bits with possible values 0, 1, 2 or 3).");
+    RTABMAP_PARAM(ORB, ScoreType,     int, 0,      "The default HARRIS_SCORE=0 means that Harris algorithm is used to rank features (the score is written to KeyPoint::score and is used to retain best nfeatures features); FAST_SCORE=1 is alternative value of the parameter that produces slightly less stable keypoints, but it is a little faster to compute.");
+    RTABMAP_PARAM(ORB, PatchSize,     int, 31,     "size of the patch used by the oriented BRIEF descriptor. Of course, on smaller pyramid layers the perceived image area covered by a feature will be larger.");
+    RTABMAP_PARAM(ORB, Gpu,           bool, false, "GPU-ORB: Use GPU version of ORB. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
+
+    RTABMAP_PARAM(FREAK, OrientationNormalized, bool, true,   "Enable orientation normalization.");
+    RTABMAP_PARAM(FREAK, ScaleNormalized,       bool, true,   "Enable scale normalization.");
+    RTABMAP_PARAM(FREAK, PatternScale,          float, 22,    "Scaling of the description pattern.");
+    RTABMAP_PARAM(FREAK, NOctaves,              int, 4,       "Number of octaves covered by the detected keypoints.");
+
+    RTABMAP_PARAM(BRISK, Thresh,       int, 30, "FAST/AGAST detection threshold score.");
+    RTABMAP_PARAM(BRISK, Octaves,      int, 3,  "Detection octaves. Use 0 to do single scale.");
+    RTABMAP_PARAM(BRISK, PatternScale, float, 1,"Apply this scale to the pattern used for sampling the neighbourhood of a keypoint.");
+
+    RTABMAP_PARAM(KAZE, Extended,       bool, false,   "Set to enable extraction of extended (128-byte) descriptor.");
+    RTABMAP_PARAM(KAZE, Upright,        bool, false,   "Set to enable use of upright descriptors (non rotation-invariant).");
+    RTABMAP_PARAM(KAZE, Threshold,      float, 0.001,  "Detector response threshold to accept keypoint.");
+    RTABMAP_PARAM(KAZE, NOctaves,       int, 4,        "Maximum octave evolution of the image.");
+    RTABMAP_PARAM(KAZE, NOctaveLayers,  int, 4,        "Default number of sublevels per scale level.");
+    RTABMAP_PARAM(KAZE, Diffusivity,    int, 1,        "Diffusivity type: 0=DIFF_PM_G1, 1=DIFF_PM_G2, 2=DIFF_WEICKERT or 3=DIFF_CHARBONNIER.");
+
+    RTABMAP_PARAM_STR(SuperPoint, ModelPath, "",           "[Required] Path to pre-trained weights Torch file of SuperPoint (*.pt).");
+    RTABMAP_PARAM(SuperPoint, Threshold,     float, 0.010, "Detector response threshold to accept keypoint.");
+    RTABMAP_PARAM(SuperPoint, NMS,           bool,  true,  "If true, non-maximum suppression is applied to detected keypoints.");
+    RTABMAP_PARAM(SuperPoint, NMSRadius,     int,  4,      uFormat("[%s=true] Minimum distance (pixels) between keypoints.", kSuperPointNMS().c_str()));
+    RTABMAP_PARAM(SuperPoint, Cuda,          bool, true,   "Use Cuda device for Torch, otherwise CPU device is used by default.");
+
+    RTABMAP_PARAM_STR(PyDetector, Path,       "",           "Path to python script file (see available ones in rtabmap/corelib/src/python/*). See the header to see where the script should be copied.");
+	RTABMAP_PARAM(PyDetector, Cuda,           bool, true,   "Use cuda.");
+
+    // BayesFilter
+    RTABMAP_PARAM(Bayes, VirtualPlacePriorThr, float, 0.9,  "Virtual place prior");
+    RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.36 0.30 0.16 0.062 0.0151 0.00255 0.000324 2.5e-05 1.3e-06 4.8e-08 1.2e-09 1.9e-11 2.2e-13 1.7e-15 8.5e-18 2.9e-20 6.9e-23", "Prediction of loop closures (Gaussian-like, here with sigma=1.6) - Format: {VirtualPlaceProb, LoopClosureProb, NeighborLvl1, NeighborLvl2, ...}.");
+    RTABMAP_PARAM(Bayes, FullPredictionUpdate, bool, false, "Regenerate all the prediction matrix on each iteration (otherwise only removed/added ids are updated).");
+
+    // Verify hypotheses
+    RTABMAP_PARAM(VhEp, Enabled, bool, false,       uFormat("Verify visual loop closure hypothesis by computing a fundamental matrix. This is done prior to transformation computation when %s is enabled.", kRGBDEnabled().c_str()));
+    RTABMAP_PARAM(VhEp, MatchCountMin, int, 8,      "Minimum of matching visual words pairs to accept the loop hypothesis.");
+    RTABMAP_PARAM(VhEp, RansacParam1,  float, 3,    "Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier.");
+    RTABMAP_PARAM(VhEp, RansacParam2,  float, 0.99, "Fundamental matrix (see cvFindFundamentalMat()): Performance of RANSAC.");
+
+    // RGB-D SLAM
+    RTABMAP_PARAM(RGBD, Enabled,                  bool, true,  "Activate metric SLAM. If set to false, classic RTAB-Map loop closure detection is done using only images and without any metric information.");
+    RTABMAP_PARAM(RGBD, LinearUpdate,             float, 0.1,  "Minimum linear displacement (m) to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement (rad) to update the map. Rehearsal is done prior to this, so weights are still updated.");
+    RTABMAP_PARAM(RGBD, LinearSpeedUpdate,        float, 0.0,  "Maximum linear speed (m/s) to update the map (0 means not limit).");
+    RTABMAP_PARAM(RGBD, AngularSpeedUpdate,       float, 0.0,  "Maximum angular speed (rad/s) to update the map (0 means not limit).");
+    RTABMAP_PARAM(RGBD, AggressiveLoopThr,        float, 0.05, uFormat("Loop closure threshold used (overriding %s) when a new mapping session is not yet linked to a map of the highest loop closure hypothesis. In localization mode, this threshold is used when there are no loop closure constraints with any map in the cache (%s). In all cases, the goal is to aggressively loop on a previous map in the database. Only used when %s is enabled. Set 1 to disable.", kRtabmapLoopThr().c_str(), kRGBDMaxOdomCacheSize().c_str(), kRGBDEnabled().c_str()));
+    RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
+    RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
+    RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 3.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
+    RTABMAP_PARAM(RGBD, MaxLoopClosureDistance,   float, 0.0,   "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).");
+    RTABMAP_PARAM(RGBD, ForceOdom3DoF,            bool, true,  uFormat("Force odometry pose to be 3DoF if %s=true.", kRegForce3DoF().c_str()));
+    RTABMAP_PARAM(RGBD, StartAtOrigin,            bool, false, uFormat("If true, rtabmap will assume the robot is starting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode (%s=false).", kMemIncrementalMemory().c_str()));
+    RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
+    RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
+    RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0,    "Linear velocity (m/sec) used to compute path weights.");
+    RTABMAP_PARAM(RGBD, PlanAngularVelocity,      float, 0,    "Angular velocity (rad/sec) used to compute path weights.");
+    RTABMAP_PARAM(RGBD, GoalsSavedInUserData,     bool, false, "When a goal is received and processed with success, it is saved in user data of the location with this format: \"GOAL:#\".");
+    RTABMAP_PARAM(RGBD, MaxLocalRetrieved,        unsigned int, 2, "Maximum local locations retrieved (0=disabled) near the current pose in the local map or on the current planned path (those on the planned path have priority).");
+    RTABMAP_PARAM(RGBD, LocalRadius,              float, 10,   "Local radius (m) for nodes selection in the local map. This parameter is used in some approaches about the local map management.");
+    RTABMAP_PARAM(RGBD, LocalImmunizationRatio,   float, 0.25, "Ratio of working memory for which local nodes are immunized from transfer.");
+    RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true,    "Save scan matching IDs from one-to-many proximity detection in link's user data.");
+    RTABMAP_PARAM(RGBD, NeighborLinkRefining,         bool, false,  uFormat("When a new node is added to the graph, the transformation of its neighbor link to the previous node is refined using registration approach selected (%s).", kRegStrategy().c_str()));
+    RTABMAP_PARAM(RGBD, LoopClosureIdentityGuess,     bool, false,  uFormat("Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used, thus assuming that registration strategy selected (%s) can deal with transformation estimation without guess.", kRegStrategy().c_str()));
+    RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false,  "Extract features even if there are some already in the nodes. Raw features are not saved in database.");
+    RTABMAP_PARAM(RGBD, LocalBundleOnLoopClosure,     bool, false,  "Do local bundle adjustment with neighborhood of the loop closure.");
+    RTABMAP_PARAM(RGBD, InvertedReg,                  bool, false,  "On loop closure, do registration from the target to reference instead of reference to target.");
+    RTABMAP_PARAM(RGBD, CreateOccupancyGrid,          bool, false,  "Create local occupancy grid maps. See \"Grid\" group for parameters.");
+    RTABMAP_PARAM(RGBD, MarkerDetection,              bool, false,  "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters.");
+    RTABMAP_PARAM(RGBD, LoopCovLimited,               bool, false,  "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
+    RTABMAP_PARAM(RGBD, MaxOdomCacheSize,             int,  10,      uFormat("Maximum odometry cache size. Used only in localization mode (when %s=false). This is used to get smoother localizations and to verify localization transforms (when %s!=0) to make sure we don't teleport to a location very similar to one we previously localized on. Set 0 to disable caching.", kMemIncrementalMemory().c_str(), kRGBDOptimizeMaxError().c_str()));
+    RTABMAP_PARAM(RGBD, LocalizationSmoothing,        bool, true,    uFormat("Adjust localization constraints based on optimized odometry cache poses (when %s>0).",  kRGBDMaxOdomCacheSize().c_str()));
+    RTABMAP_PARAM(RGBD, LocalizationPriorError,       double, 0.001, uFormat("The corresponding variance (error x error) set to priors of the map's poses during localization (when %s>0).",  kRGBDMaxOdomCacheSize().c_str()));
+    RTABMAP_PARAM(RGBD, LocalizationSecondTryWithoutProximityLinks,  bool, true, uFormat("When localization is rejected by graph optimization validation, try a second time without proximity links if landmark or loop closure links are also present in odometry cache (see %s). If it succeeds, the proximity links are removed. This assumes that global loop closure and landmark links are more accurate than proximity links.", kRGBDMaxOdomCacheSize().c_str()));
+
+    // Local/Proximity loop closure detection
+    RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, "Detection over all locations in STM.");
+    RTABMAP_PARAM(RGBD, ProximityBySpace,             bool, true,  "Detection over locations (in Working Memory) near in space.");
+    RTABMAP_PARAM(RGBD, ProximityMaxGraphDepth,       int, 50,     "Maximum depth from the current/last loop closure location and the local loop closure hypotheses. Set 0 to ignore.");
+    RTABMAP_PARAM(RGBD, ProximityMaxPaths,            int, 3,      "Maximum paths compared (from the most recent) for proximity detection. 0 means no limit.");
+    RTABMAP_PARAM(RGBD, ProximityPathFilteringRadius, float, 1,    "Path filtering radius to reduce the number of nodes to compare in a path in one-to-many proximity detection. The nearest node in a path should be inside that radius to be considered for one-to-one proximity detection.");
+    RTABMAP_PARAM(RGBD, ProximityPathMaxNeighbors,    int, 0,      "Maximum neighbor nodes compared on each path for one-to-many proximity detection. Set to 0 to disable one-to-many proximity detection (by merging the laser scans).");
+    RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,  "When comparing to a local path for one-to-many proximity detection, merge the scans using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");
+    RTABMAP_PARAM(RGBD, ProximityAngle,               float, 45,   "Maximum angle (degrees) for one-to-one proximity detection.");
+    RTABMAP_PARAM(RGBD, ProximityOdomGuess,           bool, false, "Use odometry as motion guess for one-to-one proximity detection.");
+    RTABMAP_PARAM(RGBD, ProximityGlobalScanMap,       bool, false, uFormat("Create a global assembled map from laser scans for one-to-many proximity detection, replacing the original one-to-many proximity detection (i.e., detection against local paths). Only used in localization mode (%s=false), otherwise original one-to-many proximity detection is done. Note also that if graph is modified (i.e., memory management is enabled or robot jumps from one disjoint session to another in same database), the global scan map is cleared and one-to-many proximity detection is reverted to original approach.", kMemIncrementalMemory().c_str()));
+    RTABMAP_PARAM(RGBD, ProximityMergedScanCovFactor, double, 100.0, uFormat("Covariance factor for one-to-many proximity detection (when %s>0 and scans are used).", kRGBDProximityPathMaxNeighbors().c_str()));
+
+    // Graph optimization
+#ifdef RTABMAP_GTSAM
+    RTABMAP_PARAM(Optimizer, Strategy,        int, 2,          "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
+    RTABMAP_PARAM(Optimizer, Iterations,      int, 20,         "Optimization iterations.");
+    RTABMAP_PARAM(Optimizer, Epsilon,         double, 0.00001, "Stop optimizing when the error improvement is less than this value.");
+#else
+#ifdef RTABMAP_G2O
+    RTABMAP_PARAM(Optimizer, Strategy,        int, 1,          "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
+    RTABMAP_PARAM(Optimizer, Iterations,      int, 20,         "Optimization iterations.");
+    RTABMAP_PARAM(Optimizer, Epsilon,         double, 0.0,     "Stop optimizing when the error improvement is less than this value.");
+#else
+#ifdef RTABMAP_CERES
+    RTABMAP_PARAM(Optimizer, Strategy,        int, 3,          "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
+    RTABMAP_PARAM(Optimizer, Iterations,      int, 20,         "Optimization iterations.");
+    RTABMAP_PARAM(Optimizer, Epsilon,         double, 0.000001, "Stop optimizing when the error improvement is less than this value.");
+#else
+    RTABMAP_PARAM(Optimizer, Strategy,        int, 0,          "Graph optimization strategy: 0=TORO, 1=g2o, 2=GTSAM and 3=Ceres.");
+    RTABMAP_PARAM(Optimizer, Iterations,      int, 100,        "Optimization iterations.");
+    RTABMAP_PARAM(Optimizer, Epsilon,         double, 0.00001, "Stop optimizing when the error improvement is less than this value.");
+#endif
+#endif
+#endif
+    RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
+    RTABMAP_PARAM(Optimizer, Robust,          bool, false,     uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
+    RTABMAP_PARAM(Optimizer, PriorsIgnored,   bool, true,      "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this.");
+    RTABMAP_PARAM(Optimizer, LandmarksIgnored,   bool, false,  "Ignore landmark constraints while optimizing. Currently only g2o and gtsam optimization supports this.");
+#if defined(RTABMAP_G2O) || defined(RTABMAP_GTSAM)
+    RTABMAP_PARAM(Optimizer, GravitySigma,    float, 0.3,      uFormat("Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies (see %s).", kOptimizerStrategy().c_str()));
+#else
+    RTABMAP_PARAM(Optimizer, GravitySigma,    float, 0.0,      uFormat("Gravity sigma value (>=0, typically between 0.1 and 0.3). Optimization is done while preserving gravity orientation of the poses. This should be used only with visual/lidar inertial odometry approaches, for which we assume that all odometry poses are aligned with gravity. Set to 0 to disable gravity constraints. Currently supported only with g2o and GTSAM optimization strategies (see %s).", kOptimizerStrategy().c_str()));
+#endif
+
+#ifdef RTABMAP_ORB_SLAM
+    RTABMAP_PARAM(g2o, Solver,            int, 3,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
+#else
+    RTABMAP_PARAM(g2o, Solver,            int, 0,          "0=csparse 1=pcg 2=cholmod 3=Eigen");
+#endif
+    RTABMAP_PARAM(g2o, Optimizer,         int, 0,          "0=Levenberg 1=GaussNewton");
+    RTABMAP_PARAM(g2o, PixelVariance,     double, 1.0,     "Pixel variance used for bundle adjustment.");
+    RTABMAP_PARAM(g2o, RobustKernelDelta, double, 8,       "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
+    RTABMAP_PARAM(g2o, Baseline,          double, 0.075,   "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.");
+
+    RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
+    RTABMAP_PARAM(GTSAM, Incremental,     bool, false,     uFormat("Do graph optimization incrementally (iSAM2) to increase optimization speed on loop closures. Note that only GaussNewton and Dogleg optimization algorithms are supported (%s) in this mode.", kGTSAMOptimizer().c_str()));
+    RTABMAP_PARAM(GTSAM, IncRelinearizeThreshold,  double, 0.01, "Only relinearize variables whose linear delta magnitude is greater than this threshold. See GTSAM::ISAM2 doc for more info.");
+    RTABMAP_PARAM(GTSAM, IncRelinearizeSkip,       int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");
+
+    // Odometry
+    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D");
+    RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,       "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
+    RTABMAP_PARAM(Odom, Holonomic,              bool, true,   "If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).");
+    RTABMAP_PARAM(Odom, FillInfoData,           bool, true,   "Fill info with data (inliers/outliers features).");
+    RTABMAP_PARAM(Odom, ImageBufferSize,     unsigned int, 1, "Data buffer size (0 min inf).");
+    RTABMAP_PARAM(Odom, FilteringStrategy,      int, 0,       "0=No filtering 1=Kalman filtering 2=Particle filtering. This filter is used to smooth the odometry output.");
+    RTABMAP_PARAM(Odom, ParticleSize,      unsigned int, 400, "Number of particles of the filter.");
+    RTABMAP_PARAM(Odom, ParticleNoiseT,         float, 0.002, "Noise (m) of translation components (x,y,z).");
+    RTABMAP_PARAM(Odom, ParticleLambdaT,        float, 100,   "Lambda of translation components (x,y,z).");
+    RTABMAP_PARAM(Odom, ParticleNoiseR,         float, 0.002, "Noise (rad) of rotational components (roll,pitch,yaw).");
+    RTABMAP_PARAM(Odom, ParticleLambdaR,        float, 100,   "Lambda of rotational components (roll,pitch,yaw).");
+    RTABMAP_PARAM(Odom, KalmanProcessNoise,     float, 0.001, "Process noise covariance value.");
+    RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,  "Process measurement covariance value.");
+    RTABMAP_PARAM(Odom, GuessMotion,            bool, true,   "Guess next transformation from the last motion computed.");
+    RTABMAP_PARAM(Odom, GuessSmoothingDelay,    float, 0,     uFormat("Guess smoothing delay (s). Estimated velocity is averaged based on last transforms up to this maximum delay. This can help to get smoother velocity prediction. Last velocity computed is used directly if \"%s\" is set or the delay is below the odometry rate.", kOdomFilteringStrategy().c_str()));
+    RTABMAP_PARAM(Odom, KeyFrameThr,            float, 0.3,   "[Visual] Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
+    RTABMAP_PARAM(Odom, VisKeyFrameThr,         int, 150,     "[Visual] Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
+    RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.9,   "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
+    RTABMAP_PARAM(Odom, ImageDecimation,     unsigned int, 1, uFormat("Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.", kVisDepthAsMask().c_str()));
+    RTABMAP_PARAM(Odom, AlignWithGround,        bool, false,  "Align odometry with the ground on initialization.");
+    RTABMAP_PARAM(Odom, Deskewing,              bool, true,   "Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided).");
+
+    // Odometry Frame-to-Map
+    RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
+    RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,       "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
+    RTABMAP_PARAM(OdomF2M, InitDepthFactor,     float, 0.05,  "[Visual] Depth factor used to initialize depth of features without depth. Depth = Factor * fx.");
+    RTABMAP_PARAM(OdomF2M, FloorThreshold,      float, 0.0,   "[Visual] Only track features in 3D feature map that are over this threshold (height in base frame). Can be useful to ignore reflections on the floor. 0 means disabled.");
+    RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
+    RTABMAP_PARAM(OdomF2M, ScanSubtractRadius,  float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
+    RTABMAP_PARAM(OdomF2M, ScanSubtractAngle,   float, 45,    uFormat("[Geometry] Max angle (degrees) used to filter points of a new added scan to local map (when \"%s\">0). 0 means any angle.", kOdomF2MScanSubtractRadius().c_str()).c_str());
+    RTABMAP_PARAM(OdomF2M, ScanRange,           float, 0,     "[Geometry] Distance Range used to filter points of local map (when > 0). 0 means local map is updated using time and not range.");
+    RTABMAP_PARAM(OdomF2M, ValidDepthRatio,     float, 0.75,  "If a new frame has points without valid depth, they are added to local feature map only if points with valid depth on total points is over this ratio. Setting to 1 means no points without valid depth are added to local feature map.");
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
+    RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 1, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
+#else
+    RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
+#endif
+    RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 10, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
+    RTABMAP_PARAM(OdomF2M, BundleAdjustmentMinMotion, float, 0.0, "To create a new keyframe with bundle adjustment, a minimum motion (in pixels) can be required. The motion is computed by the average distance between inliers of the previous keyframe and new frame.");
+    RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxKeyFramesPerFeature, int, 0, "Maximum keyframes per feature for bundle adjustment. 0 means not limit.");
+    RTABMAP_PARAM(OdomF2M, BundleUpdateFeatureMapOnAllFrames, bool, false, uFormat("Update 3D local feature map on every frame with bundle adjustment. Recommended if %s=false and %s=true so that features without depth are better triangulated on every frame (not only on keyframes). If disabled, the feature map is updated only when a new keyframe is added (legacy approach).", kVisDepthAsMask().c_str(), kMemUseOdomFeatures().c_str()));
+
+    // Odometry Mono
+    RTABMAP_PARAM(OdomMono, InitMinFlow,        float, 100,  "Minimum optical flow required for the initialization step.");
+    RTABMAP_PARAM(OdomMono, InitMinTranslation, float, 0.1,  "Minimum translation required for the initialization step.");
+    RTABMAP_PARAM(OdomMono, MinTranslation,     float, 0.02, "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum.");
+    RTABMAP_PARAM(OdomMono, MaxVariance,        float, 0.01, "Maximum variance to add new points to local map.");
+
+    // Odometry Fovis
+    RTABMAP_PARAM(OdomFovis, FeatureWindowSize,               int, 9,        "The size of the n x n image patch surrounding each feature, used for keypoint matching.");
+    RTABMAP_PARAM(OdomFovis, MaxPyramidLevel,                 int, 3,        "The maximum Gaussian pyramid level to process the image at. Pyramid level 1 corresponds to the original image.");
+    RTABMAP_PARAM(OdomFovis, MinPyramidLevel,                 int, 0,        "The minimum pyramid level.");
+    RTABMAP_PARAM(OdomFovis, TargetPixelsPerFeature,          int, 250,      "Specifies the desired feature density as a ratio of input image pixels per feature detected.  This number is used to control the adaptive feature thresholding.");
+    RTABMAP_PARAM(OdomFovis, FastThreshold,                   int, 20,       "FAST threshold.");
+    RTABMAP_PARAM(OdomFovis, UseAdaptiveThreshold,            bool, true,    "Use FAST adaptive threshold.");
+    RTABMAP_PARAM(OdomFovis, FastThresholdAdaptiveGain,       double, 0.005, "FAST threshold adaptive gain.");
+    RTABMAP_PARAM(OdomFovis, UseHomographyInitialization,     bool, true,    "Use homography initialization.");
+
+    RTABMAP_PARAM(OdomFovis, UseBucketing,                    bool, true,  "");
+    RTABMAP_PARAM(OdomFovis, BucketWidth,                     int, 80,     "");
+    RTABMAP_PARAM(OdomFovis, BucketHeight,                    int, 80,     "");
+    RTABMAP_PARAM(OdomFovis, MaxKeypointsPerBucket,           int, 25,     "");
+    RTABMAP_PARAM(OdomFovis, UseImageNormalization,           bool, false, "");
+
+    RTABMAP_PARAM(OdomFovis, InlierMaxReprojectionError,      double, 1.5,  "The maximum image-space reprojection error (in pixels) a feature match is allowed to have and still be considered an inlier in the set of features used for motion estimation.");
+    RTABMAP_PARAM(OdomFovis, CliqueInlierThreshold,           double, 0.1,  "See Howard's greedy max-clique algorithm for determining the maximum set of mutually consisten feature matches. This specifies the compatibility threshold, in meters.");
+    RTABMAP_PARAM(OdomFovis, MinFeaturesForEstimate,          int, 20,      "Minimum number of features in the inlier set for the motion estimate to be considered valid.");
+    RTABMAP_PARAM(OdomFovis, MaxMeanReprojectionError,        double, 10.0, "Maximum mean reprojection error over the inlier feature matches for the motion estimate to be considered valid.");
+    RTABMAP_PARAM(OdomFovis, UseSubpixelRefinement,           bool, true,   "Specifies whether or not to refine feature matches to subpixel resolution.");
+    RTABMAP_PARAM(OdomFovis, FeatureSearchWindow,             int, 25,      "Specifies the size of the search window to apply when searching for feature matches across time frames.  The search is conducted around the feature location predicted by the initial rotation estimate.");
+    RTABMAP_PARAM(OdomFovis, UpdateTargetFeaturesWithRefined, bool, false,  "When subpixel refinement is enabled, the refined feature locations can be saved over the original feature locations.  This has a slightly negative impact on frame-to-frame visual odometry, but is likely better when using this library as part of a visual SLAM algorithm.");
+
+    RTABMAP_PARAM(OdomFovis, StereoRequireMutualMatch,        bool, true,  "");
+    RTABMAP_PARAM(OdomFovis, StereoMaxDistEpipolarLine,       double, 1.5, "");
+    RTABMAP_PARAM(OdomFovis, StereoMaxRefinementDisplacement, double, 1.0, "");
+    RTABMAP_PARAM(OdomFovis, StereoMaxDisparity,              int, 128,    "");
+
+    // Odometry viso2
+    RTABMAP_PARAM(OdomViso2, RansacIters,               int, 200,    "Number of RANSAC iterations.");
+    RTABMAP_PARAM(OdomViso2, InlierThreshold,           double, 2.0, "Fundamental matrix inlier threshold.");
+    RTABMAP_PARAM(OdomViso2, Reweighting,               bool, true,  "Lower border weights (more robust to calibration errors).");
+    RTABMAP_PARAM(OdomViso2, MatchNmsN,                 int, 3,      "Non-max-suppression: min. distance between maxima (in pixels).");
+    RTABMAP_PARAM(OdomViso2, MatchNmsTau,               int, 50,     "Non-max-suppression: interest point peakiness threshold.");
+    RTABMAP_PARAM(OdomViso2, MatchBinsize,              int, 50,     "Matching bin width/height (affects efficiency only).");
+    RTABMAP_PARAM(OdomViso2, MatchRadius,               int, 200,    "Matching radius (du/dv in pixels).");
+    RTABMAP_PARAM(OdomViso2, MatchDispTolerance,        int, 2,      "Disparity tolerance for stereo matches (in pixels).");
+    RTABMAP_PARAM(OdomViso2, MatchOutlierDispTolerance, int, 5,      "Outlier removal: disparity tolerance (in pixels).");
+    RTABMAP_PARAM(OdomViso2, MatchOutlierFlowTolerance, int, 5,      "Outlier removal: flow tolerance (in pixels).");
+    RTABMAP_PARAM(OdomViso2, MatchMultiStage,           bool, true,  "Multistage matching (denser and faster).");
+    RTABMAP_PARAM(OdomViso2, MatchHalfResolution,       bool, true,  "Match at half resolution, refine at full resolution.");
+    RTABMAP_PARAM(OdomViso2, MatchRefinement,           int, 1,      "Refinement (0=none,1=pixel,2=subpixel).");
+    RTABMAP_PARAM(OdomViso2, BucketMaxFeatures,         int, 2,      "Maximal number of features per bucket.");
+    RTABMAP_PARAM(OdomViso2, BucketWidth,               double, 50,  "Width of bucket.");
+    RTABMAP_PARAM(OdomViso2, BucketHeight,              double, 50,  "Height of bucket.");
+
+    // Odometry ORB_SLAM2
+    RTABMAP_PARAM_STR(OdomORBSLAM, VocPath,             "",       "Path to ORB vocabulary (*.txt).");
+    RTABMAP_PARAM(OdomORBSLAM, Bf,              double, 0.076,    "Fake IR projector baseline (m) used only when stereo is not used.");
+    RTABMAP_PARAM(OdomORBSLAM, ThDepth,         double, 40.0,     "Close/Far threshold. Baseline times.");
+    RTABMAP_PARAM(OdomORBSLAM, Fps,             float,  0.0,      "Camera FPS (0 to estimate from input data).");
+    RTABMAP_PARAM(OdomORBSLAM, MaxFeatures,     int,    1000,     "Maximum ORB features extracted per frame.");
+    RTABMAP_PARAM(OdomORBSLAM, MapSize,         int,    3000,     "Maximum size of the feature map (0 means infinite). Only supported with ORB_SLAM2.");
+    RTABMAP_PARAM(OdomORBSLAM, Inertial,        bool,   false,    "Enable IMU. Only supported with ORB_SLAM3.");
+    RTABMAP_PARAM(OdomORBSLAM, GyroNoise,       double, 0.01,     "IMU gyroscope \"white noise\".");
+    RTABMAP_PARAM(OdomORBSLAM, AccNoise,        double, 0.1,      "IMU accelerometer \"white noise\".");
+    RTABMAP_PARAM(OdomORBSLAM, GyroWalk,        double, 0.000001, "IMU gyroscope \"random walk\".");
+    RTABMAP_PARAM(OdomORBSLAM, AccWalk,         double, 0.0001,   "IMU accelerometer \"random walk\".");
+    RTABMAP_PARAM(OdomORBSLAM, SamplingRate,    double, 0,        "IMU sampling rate (0 to estimate from input data).");
+
+
+    // Odometry OKVIS
+    RTABMAP_PARAM_STR(OdomOKVIS, ConfigPath,     "",  "Path of OKVIS config file.");
+
+    // Odometry LOAM
+    RTABMAP_PARAM(OdomLOAM, Sensor,     int,    2,    "Velodyne sensor: 0=VLP-16, 1=HDL-32, 2=HDL-64E");
+    RTABMAP_PARAM(OdomLOAM, ScanPeriod, float,  0.1,  "Scan period (s)");
+    RTABMAP_PARAM(OdomLOAM, Resolution, float,  0.2,   "Map resolution");
+    RTABMAP_PARAM(OdomLOAM, LinVar,     float,  0.01,  "Linear output variance.");
+    RTABMAP_PARAM(OdomLOAM, AngVar,     float,  0.01,  "Angular output variance.");
+    RTABMAP_PARAM(OdomLOAM, LocalMapping, bool,  true,  "Local mapping. It adds more time to compute odometry, but accuracy is significantly improved.");
+
+    // Odometry MSCKF_VIO
+    RTABMAP_PARAM(OdomMSCKF, GridRow,           int,  4,  "");
+    RTABMAP_PARAM(OdomMSCKF, GridCol,           int,  5,  "");
+    RTABMAP_PARAM(OdomMSCKF, GridMinFeatureNum, int,  3,  "");
+    RTABMAP_PARAM(OdomMSCKF, GridMaxFeatureNum, int,  4,  "");
+    RTABMAP_PARAM(OdomMSCKF, PyramidLevels,     int,  3,  "");
+    RTABMAP_PARAM(OdomMSCKF, PatchSize,         int,  15,  "");
+    RTABMAP_PARAM(OdomMSCKF, FastThreshold,     int,  10,  "");
+    RTABMAP_PARAM(OdomMSCKF, MaxIteration,      int,  30,  "");
+    RTABMAP_PARAM(OdomMSCKF, TrackPrecision,    double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, RansacThreshold,   double,  3,  "");
+    RTABMAP_PARAM(OdomMSCKF, StereoThreshold,   double,  5,  "");
+    RTABMAP_PARAM(OdomMSCKF, PositionStdThreshold,    double,  8.0,  "");
+    RTABMAP_PARAM(OdomMSCKF, RotationThreshold,       double,  0.2618,  "");
+    RTABMAP_PARAM(OdomMSCKF, TranslationThreshold,    double,  0.4,  "");
+    RTABMAP_PARAM(OdomMSCKF, TrackingRateThreshold,   double,  0.5,  "");
+    RTABMAP_PARAM(OdomMSCKF, OptTranslationThreshold, double,  0,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseGyro,     double,  0.005,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseAcc,      double,  0.05,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseGyroBias, double,  0.001,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseAccBias,  double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, NoiseFeature,  double,  0.035,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovVel,    double,  0.25,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovGyroBias, double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovAccBias,  double,  0.01,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovExRot,    double,  0.00030462,  "");
+    RTABMAP_PARAM(OdomMSCKF, InitCovExTrans,  double,  0.000025,  "");
+    RTABMAP_PARAM(OdomMSCKF, MaxCamStateSize,  int,  20,  "");
+
+    // Odometry VINS
+    RTABMAP_PARAM_STR(OdomVINS, ConfigPath,     "",  "Path of VINS config file.");
+
+    // Odometry OpenVINS
+    RTABMAP_PARAM(OdomOpenVINS, UseStereo,                 bool,   true,   "If we have more than 1 camera, if we should try to track stereo constraints between pairs");
+    RTABMAP_PARAM(OdomOpenVINS, UseKLT,                    bool,   true,   "If true we will use KLT, otherwise use a ORB descriptor + robust matching");
+    RTABMAP_PARAM(OdomOpenVINS, NumPts,                    int,    200,    "Number of points (per camera) we will extract and try to track");
+    RTABMAP_PARAM(OdomOpenVINS, MinPxDist,                 int,    15,     "Eistance between features (features near each other provide less information)");
+    RTABMAP_PARAM(OdomOpenVINS, FiTriangulate1d,           bool,   false,  "If we should perform 1d triangulation instead of 3d");
+    RTABMAP_PARAM(OdomOpenVINS, FiRefineFeatures,          bool,   true,   "If we should perform Levenberg-Marquardt refinement");
+    RTABMAP_PARAM(OdomOpenVINS, FiMaxRuns,                 int,    5,      "Max runs for Levenberg-Marquardt");
+    RTABMAP_PARAM(OdomOpenVINS, FiMaxBaseline,             double, 40,     "Max baseline ratio to accept triangulated features");
+    RTABMAP_PARAM(OdomOpenVINS, FiMaxCondNumber,           double, 10000,  "Max condition number of linear triangulation matrix accept triangulated features");
+
+    RTABMAP_PARAM(OdomOpenVINS, UseFEJ,                    bool,   true,   "If first-estimate Jacobians should be used (enable for good consistency)");
+    RTABMAP_PARAM(OdomOpenVINS, Integration,               int,    1,      "0=discrete, 1=rk4, 2=analytical (if rk4 or analytical used then analytical covariance propagation is used)");
+    RTABMAP_PARAM(OdomOpenVINS, CalibCamExtrinsics,        bool,   false,  "Bool to determine whether or not to calibrate imu-to-camera pose");
+    RTABMAP_PARAM(OdomOpenVINS, CalibCamIntrinsics,        bool,   false,  "Bool to determine whether or not to calibrate camera intrinsics");
+    RTABMAP_PARAM(OdomOpenVINS, CalibCamTimeoffset,        bool,   false,  "Bool to determine whether or not to calibrate camera to IMU time offset");
+    RTABMAP_PARAM(OdomOpenVINS, CalibIMUIntrinsics,        bool,   false,  "Bool to determine whether or not to calibrate the IMU intrinsics");
+    RTABMAP_PARAM(OdomOpenVINS, CalibIMUGSensitivity,      bool,   false,  "Bool to determine whether or not to calibrate the Gravity sensitivity");
+    RTABMAP_PARAM(OdomOpenVINS, MaxClones,                 int,    11,     "Max clone size of sliding window");
+    RTABMAP_PARAM(OdomOpenVINS, MaxSLAM,                   int,    50,     "Max number of estimated SLAM features");
+    RTABMAP_PARAM(OdomOpenVINS, MaxSLAMInUpdate,           int,    25,     "Max number of SLAM features we allow to be included in a single EKF update.");
+    RTABMAP_PARAM(OdomOpenVINS, MaxMSCKFInUpdate,          int,    50,     "Max number of MSCKF features we will use at a given image timestep.");
+    RTABMAP_PARAM(OdomOpenVINS, FeatRepMSCKF,              int,    0,      "What representation our features are in (msckf features)");
+    RTABMAP_PARAM(OdomOpenVINS, FeatRepSLAM,               int,    4,      "What representation our features are in (slam features)");
+    RTABMAP_PARAM(OdomOpenVINS, DtSLAMDelay,               double, 0.0,    "Delay, in seconds, that we should wait from init before we start estimating SLAM features");
+    RTABMAP_PARAM(OdomOpenVINS, GravityMag,                double, 9.81,   "Gravity magnitude in the global frame (i.e. should be 9.81 typically)");
+    RTABMAP_PARAM_STR(OdomOpenVINS, LeftMaskPath,                  "",     "Mask for left image");
+    RTABMAP_PARAM_STR(OdomOpenVINS, RightMaskPath,                 "",     "Mask for right image");
+
+    RTABMAP_PARAM(OdomOpenVINS, InitWindowTime,            double, 2.0,    "Amount of time we will initialize over (seconds)");
+    RTABMAP_PARAM(OdomOpenVINS, InitIMUThresh,             double, 1.0,    "Variance threshold on our acceleration to be classified as moving");
+    RTABMAP_PARAM(OdomOpenVINS, InitMaxDisparity,          double, 10.0,   "Max disparity to consider the platform stationary (dependent on resolution)");
+    RTABMAP_PARAM(OdomOpenVINS, InitMaxFeatures,           int,    50,     "How many features to track during initialization (saves on computation)");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynUse,                bool,   false,  "If dynamic initialization should be used");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEOptCalib,        bool,   false,  "If we should optimize calibration during intialization (not recommended)");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxIter,         int,    50,     "How many iterations the MLE refinement should use (zero to skip the MLE)");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxTime,         double, 0.05,   "How many seconds the MLE should be completed in");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMLEMaxThreads,      int,    6,      "How many threads the MLE should use");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynNumPose,            int,    6,      "Number of poses to use within our window time (evenly spaced)");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMinDeg,             double, 10.0,   "Orientation change needed to try to init");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationOri,       double, 10.0,   "What to inflate the recovered q_GtoI covariance by");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationVel,       double, 100.0,  "What to inflate the recovered v_IinG covariance by");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBg,        double, 10.0,   "What to inflate the recovered bias_g covariance by");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynInflationBa,        double, 100.0,  "What to inflate the recovered bias_a covariance by");
+    RTABMAP_PARAM(OdomOpenVINS, InitDynMinRecCond,         double, 1e-15,  "Reciprocal condition number thresh for info inversion");
+
+    RTABMAP_PARAM(OdomOpenVINS, TryZUPT,                   bool,   true,   "If we should try to use zero velocity update");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTChi2Multiplier,        double, 0.0,    "Chi2 multiplier for zero velocity");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxVelodicy,           double, 0.1,    "Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt)");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTNoiseMultiplier,       double, 10.0,   "Multiplier of our zupt measurement IMU noise matrix (default should be 1.0)");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTMaxDisparity,          double, 0.5,    "Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt)");
+    RTABMAP_PARAM(OdomOpenVINS, ZUPTOnlyAtBeginning,       bool,   false,  "If we should only use the zupt at the very beginning static initialization phase");
+
+    RTABMAP_PARAM(OdomOpenVINS, AccelerometerNoiseDensity, double, 0.01,   "[m/s^2/sqrt(Hz)] (accel \"white noise\")");
+    RTABMAP_PARAM(OdomOpenVINS, AccelerometerRandomWalk,   double, 0.001,  "[m/s^3/sqrt(Hz)] (accel bias diffusion)");
+    RTABMAP_PARAM(OdomOpenVINS, GyroscopeNoiseDensity,     double, 0.001,  "[rad/s/sqrt(Hz)] (gyro \"white noise\")");
+    RTABMAP_PARAM(OdomOpenVINS, GyroscopeRandomWalk,       double, 0.0001, "[rad/s^2/sqrt(Hz)] (gyro bias diffusion)");
+    RTABMAP_PARAM(OdomOpenVINS, UpMSCKFSigmaPx,            double, 1.0,    "Pixel noise for MSCKF features");
+    RTABMAP_PARAM(OdomOpenVINS, UpMSCKFChi2Multiplier,     double, 1.0,    "Chi2 multiplier for MSCKF features");
+    RTABMAP_PARAM(OdomOpenVINS, UpSLAMSigmaPx,             double, 1.0,    "Pixel noise for SLAM features");
+    RTABMAP_PARAM(OdomOpenVINS, UpSLAMChi2Multiplier,      double, 1.0,    "Chi2 multiplier for SLAM features");
+
+    // Odometry Open3D
+    RTABMAP_PARAM(OdomOpen3D, MaxDepth,         float, 3.0,  "Maximum depth.");
+    RTABMAP_PARAM(OdomOpen3D, Method,           int, 0,  "Registration method: 0=PointToPlane, 1=Intensity, 2=Hybrid.");
+
+    // Common registration parameters
+    RTABMAP_PARAM(Reg, RepeatOnce,               bool, true,    "Do a second registration with the output of the first registration as guess. Only done if no guess was provided for the first registration (like on loop closure). It can be useful if the registration approach used can use a guess to get better matches.");
+    RTABMAP_PARAM(Reg, Strategy,                 int, 0,        "0=Vis, 1=Icp, 2=VisIcp");
+    RTABMAP_PARAM(Reg, Force3DoF,                bool, false,   "Force 3 degrees-of-freedom transform (3Dof: x,y and yaw). Parameters z, roll and pitch will be set to 0.");
+
+    // Visual registration parameters
+    RTABMAP_PARAM(Vis, EstimationType,           int,    1,     "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
+    RTABMAP_PARAM(Vis, InlierDistance,           float,  0.1,   uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str()));
+    RTABMAP_PARAM(Vis, RefineIterations,         int,    5,     uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
+    RTABMAP_PARAM(Vis, PnPReprojError,           float,  2,     uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
+    RTABMAP_PARAM(Vis, PnPFlags,                 int,    0,     uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
+    RTABMAP_PARAM(Vis, PnPRefineIterations,      int,    0,     uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
+#else
+    RTABMAP_PARAM(Vis, PnPRefineIterations,      int,    1,     uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
+#endif
+    RTABMAP_PARAM(Vis, PnPVarianceMedianRatio,   int,    4,     uFormat("[%s = 1] Ratio used to compute variance of the estimated transformation if 3D correspondences are provided (should be > 1). The higher it is, the smaller the covariance will be. With accurate depth estimation, this could be set to 2. For depth estimated by stereo, 4 or more maybe used to ignore large errors of very far points.", kVisEstimationType().c_str()));
+    RTABMAP_PARAM(Vis, PnPMaxVariance,           float,  0.0,   uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str()));
+    RTABMAP_PARAM(Vis, PnPSamplingPolicy, unsigned int,  1,     uFormat("[%s = 1] Multi-camera random sampling policy: 0=AUTO, 1=ANY, 2=HOMOGENEOUS. With HOMOGENEOUS policy, RANSAC will be done uniformly against all cameras, so at least 2 matches per camera are required. With ANY policy, RANSAC is not constraint to sample on all cameras at the same time. AUTO policy will use HOMOGENEOUS if there are at least 2 matches per camera, otherwise it will fallback to ANY policy.", kVisEstimationType().c_str()).c_str());
+    RTABMAP_PARAM(Vis, PnPSplitLinearCovComponents, bool,  false, uFormat("[%s = 1] Compute variance for each linear component instead of using the combined XYZ variance for all linear components.", kVisEstimationType().c_str()).c_str());
+
+    RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float,  0.1,   uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
+    RTABMAP_PARAM(Vis, MinInliers,               int,    20,    "Minimum feature correspondences to compute/accept the transformation.");
+    RTABMAP_PARAM(Vis, MeanInliersDistance,      float,  0.0,   "Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled.");
+    RTABMAP_PARAM(Vis, MinInliersDistribution,   float,  0.0,   "Minimum distribution value of the inliers in the image to accept the transformation. The distribution is the second eigen value of the PCA (Principal Component Analysis) on the keypoints of the normalized image [-0.5, 0.5]. The value would be between 0 and 0.5. 0 means disabled.");
+
+    RTABMAP_PARAM(Vis, Iterations,               int,    300,   "Maximum iterations to compute the transform.");
+#if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
+    // OpenCV>2 without xFeatures2D module doesn't have BRIEF
+    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+#else
+    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
+#endif
+    RTABMAP_PARAM(Vis, MaxFeatures,               int,   1000,  "0 no limits.");
+    RTABMAP_PARAM(Vis, SSC,                       bool,  false, "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
+    RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,     "Max depth of the features (0 means no limit).");
+    RTABMAP_PARAM(Vis, MinDepth,                  float, 0,     "Min depth of the features (0 means no limit).");
+    RTABMAP_PARAM(Vis, DepthAsMask,               bool,  true,  "Use depth image as mask when extracting features.");
+    RTABMAP_PARAM(Vis, DepthMaskFloorThr,         float, 0.0,    uFormat("Filter floor from depth mask below specified threshold (m) before extracting features. 0 means disabled, negative means remove all objects above the floor threshold instead. Ignored if %s is false.", kVisDepthAsMask().c_str()));
+    RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
+    RTABMAP_PARAM(Vis, SubPixWinSize,             int,   3,     "See cv::cornerSubPix().");
+    RTABMAP_PARAM(Vis, SubPixIterations,          int,   0,     "See cv::cornerSubPix(). 0 disables sub pixel refining.");
+    RTABMAP_PARAM(Vis, SubPixEps,                 float, 0.02,  "See cv::cornerSubPix().");
+    RTABMAP_PARAM(Vis, GridRows,                  int,   1,     uFormat("Number of rows of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
+    RTABMAP_PARAM(Vis, GridCols,                  int,   1,     uFormat("Number of columns of the grid used to extract uniformly \"%s / grid cells\" features from each cell.", kVisMaxFeatures().c_str()));
+    RTABMAP_PARAM(Vis, CorType,                   int,   0,     "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
+    RTABMAP_PARAM(Vis, CorNNType,                 int,   1,     uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4, BruteForceCrossCheck=5, SuperGlue=6, GMS=7. Used for features matching approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorNNDR,                   float, 0.8,   uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for knn features matching approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorGuessWinSize,           int,   40,    uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorGuessMatchToProjection, bool,  false, uFormat("[%s=0] Match frame's corners to source's projected points (when guess transform is provided) instead of projected points to frame's corners.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowWinSize,            int,   16,    uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowIterations,         int,   30,    uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowEps,                float, 0.01,  uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowMaxLevel,           int,   3,     uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+    RTABMAP_PARAM(Vis, CorFlowGpu,                bool,  false, uFormat("[%s=1] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kVisCorType().c_str()));
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM)
+    RTABMAP_PARAM(Vis, BundleAdjustment,          int,   1,     "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
+#else
+    RTABMAP_PARAM(Vis, BundleAdjustment,          int,   0,     "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba, 3=Ceres.");
+#endif
+
+    // Features matching approaches
+    RTABMAP_PARAM_STR(PyMatcher, Path,       "",           "Path to python script file (see available ones in rtabmap/corelib/src/python/*). See the header to see where the script should be copied.");
+	RTABMAP_PARAM(PyMatcher, Iterations,     int, 20,      "Sinkhorn iterations. Used by SuperGlue.");
+	RTABMAP_PARAM(PyMatcher, Threshold,      float, 0.2,   "Used by SuperGlue.");
+	RTABMAP_PARAM(PyMatcher, Cuda,           bool, true,   "Used by SuperGlue.");
+	RTABMAP_PARAM_STR(PyMatcher, Model,        "indoor",   "For SuperGlue, set only \"indoor\" or \"outdoor\". For OANet, set path to one of the pth file (e.g., \"OANet/model/gl3d/sift-4000/model_best.pth\").");
+
+	RTABMAP_PARAM(GMS, WithRotation,         bool, false,   "Take rotation transformation into account.");
+	RTABMAP_PARAM(GMS, WithScale,            bool, false,   "Take scale transformation into account.");
+	RTABMAP_PARAM(GMS, ThresholdFactor,      double, 6.0,   "The higher, the less matches.");
+
+	// Global descriptor approaches
+	RTABMAP_PARAM_STR(PyDescriptor, Path,    "",          "Path to python script file (see available ones in rtabmap/corelib/src/pydescriptor/*). See the header to see where the script should be used.");
+	RTABMAP_PARAM(PyDescriptor, Dim,         int, 4096,   "Descriptor dimension.");
+
+    // ICP registration parameters
+#ifdef RTABMAP_POINTMATCHER
+    RTABMAP_PARAM(Icp, Strategy,                  int, 1,       "ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare).");
+#else
+    RTABMAP_PARAM(Icp, Strategy,                  int, 0,       "ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare).");
+#endif
+    RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
+    RTABMAP_PARAM(Icp, MaxRotation,               float, 0.78,  "Maximum ICP rotation correction accepted (rad).");
+    RTABMAP_PARAM(Icp, VoxelSize,                 float, 0.05,  "Uniform sampling voxel size (0=disabled).");
+    RTABMAP_PARAM(Icp, DownsamplingStep,          int, 1,       "Downsampling step size (1=no sampling). This is done before uniform sampling.");
+    RTABMAP_PARAM(Icp, RangeMin,                  float, 0,     "Minimum range filtering (0=disabled).");
+    RTABMAP_PARAM(Icp, RangeMax,                  float, 0,     "Maximum range filtering (0=disabled).");
+#ifdef RTABMAP_POINTMATCHER
+    RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.1,   "Max distance for point correspondences.");
+#else
+    RTABMAP_PARAM(Icp, MaxCorrespondenceDistance, float, 0.05,  "Max distance for point correspondences.");
+#endif
+	RTABMAP_PARAM(Icp, ReciprocalCorrespondences, bool, true,   "To be a valid correspondence, the corresponding point in target cloud to point in source cloud should be both their closest closest correspondence.");
+	RTABMAP_PARAM(Icp, Iterations,                int, 30,      "Max iterations.");
+    RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
+    RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.1,   "Ratio of matching correspondences to accept the transform.");
+    RTABMAP_PARAM(Icp, Force4DoF,                 bool, false,   uFormat("Limit ICP to x, y, z and yaw DoF. Available if %s > 0.", kIcpStrategy().c_str()));
+    RTABMAP_PARAM(Icp, FiltersEnabled,            int, 3,       "Flag to enable filters: 1=\"from\" cloud only, 2=\"to\" cloud only, 3=both.");
+#ifdef RTABMAP_POINTMATCHER
+    RTABMAP_PARAM(Icp, PointToPlane,                bool, true,   "Use point to plane ICP.");
+#else
+    RTABMAP_PARAM(Icp, PointToPlane,                bool, false,  "Use point to plane ICP.");
+#endif
+    RTABMAP_PARAM(Icp, PointToPlaneK,               int, 5,       "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneRadius,          float, 0.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneGroundNormalsUp, float, 0.0,   "Invert normals on ground if they are pointing down (useful for ring-like 3D LiDARs). 0 means disabled, 1 means only normals perfectly aligned with -z axis. This is only done with 3D scans.");
+    RTABMAP_PARAM(Icp, PointToPlaneMinComplexity,   float, 0.02,  uFormat("Minimum structural complexity (0.0=low, 1.0=high) of the scan to do PointToPlane registration, otherwise PointToPoint registration is done instead and strategy from %s is used. This check is done only when %s=true.", kIcpPointToPlaneLowComplexityStrategy().c_str(), kIcpPointToPlane().c_str()));
+    RTABMAP_PARAM(Icp, PointToPlaneLowComplexityStrategy, int, 1, uFormat("If structural complexity is below %s: set to 0 to so that the transform is automatically rejected, set to 1 to limit ICP correction in axes with most constraints (e.g., for a corridor-like environment, the resulting transform will be limited in y and yaw, x will taken from the guess), set to 2 to accept \"as is\" the transform computed by PointToPoint.", kIcpPointToPlaneMinComplexity().c_str()));
+    RTABMAP_PARAM(Icp, OutlierRatio,                float, 0.85,   uFormat("Outlier ratio used with %s>0. For libpointmatcher, this parameter set TrimmedDistOutlierFilter/ratio for convenience when configuration file is not set. For CCCoreLib, this parameter set the \"finalOverlapRatio\". The value should be between 0 and 1.", kIcpStrategy().c_str()));
+    RTABMAP_PARAM_STR(Icp, DebugExportFormat,       "",           "Export scans used for ICP in the specified format (a warning on terminal will be shown with the file paths used). Supported formats are \"pcd\", \"ply\" or \"vtk\". If logger level is debug, from and to scans will stamped, so previous files won't be overwritten.");
+
+    // libpointmatcher
+    RTABMAP_PARAM_STR(Icp, PMConfig,             "",            uFormat("Configuration file (*.yaml) used by libpointmatcher. Note that data filters set for libpointmatcher are done after filtering done by rtabmap (i.e., %s, %s), so make sure to disable those in rtabmap if you want to use only those from libpointmatcher. Parameters %s, %s and %s are also ignored if configuration file is set.", kIcpVoxelSize().c_str(), kIcpDownsamplingStep().c_str(), kIcpIterations().c_str(), kIcpEpsilon().c_str(), kIcpMaxCorrespondenceDistance().c_str()).c_str());
+    RTABMAP_PARAM(Icp, PMMatcherKnn,             int, 1,        "KDTreeMatcher/knn: number of nearest neighbors to consider it the reference. For convenience when configuration file is not set.");
+    RTABMAP_PARAM(Icp, PMMatcherEpsilon,         float, 0.0,    "KDTreeMatcher/epsilon: approximation to use for the nearest-neighbor search. For convenience when configuration file is not set.");
+    RTABMAP_PARAM(Icp, PMMatcherIntensity,       bool, false,   uFormat("KDTreeMatcher:  among nearest neighbors, keep only the one with the most similar intensity. This only work with %s>1.", kIcpPMMatcherKnn().c_str()));
+
+    RTABMAP_PARAM(Icp, CCSamplingLimit,          unsigned int, 50000, "Maximum number of points per cloud (they are randomly resampled below this limit otherwise).");
+    RTABMAP_PARAM(Icp, CCFilterOutFarthestPoints, bool, false, "If true, the algorithm will automatically ignore farthest points from the reference, for better convergence.");
+    RTABMAP_PARAM(Icp, CCMaxFinalRMS,            float, 0.2,   "Maximum final RMS error.");
+
+    // Stereo disparity
+    RTABMAP_PARAM(Stereo, WinWidth,              int, 15,       "Window width.");
+    RTABMAP_PARAM(Stereo, WinHeight,             int, 3,        "Window height.");
+    RTABMAP_PARAM(Stereo, Iterations,            int, 30,       "Maximum iterations.");
+    RTABMAP_PARAM(Stereo, MaxLevel,              int, 5,        "Maximum pyramid level.");
+    RTABMAP_PARAM(Stereo, MinDisparity,          float, 0.5,    "Minimum disparity.");
+    RTABMAP_PARAM(Stereo, MaxDisparity,          float, 128.0,  "Maximum disparity.");
+    RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
+    RTABMAP_PARAM(Stereo, SSD,                   bool, true,    uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str()));
+    RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  uFormat("[%s=true] Epsilon stop criterion.", kStereoOpticalFlow().c_str()));
+    RTABMAP_PARAM(Stereo, Gpu,                   bool, false,   uFormat("[%s=true] Enable GPU version of the optical flow approach (only available if OpenCV is built with CUDA).", kStereoOpticalFlow().c_str()));
+
+    RTABMAP_PARAM(Stereo, DenseStrategy,         int, 0,  "0=cv::StereoBM, 1=cv::StereoSGBM");
+
+    RTABMAP_PARAM(StereoBM, BlockSize,           int, 15,       "See cv::StereoBM");
+    RTABMAP_PARAM(StereoBM, MinDisparity,        int, 0,        "See cv::StereoBM");
+    RTABMAP_PARAM(StereoBM, NumDisparities,      int, 128,      "See cv::StereoBM");
+    RTABMAP_PARAM(StereoBM, PreFilterSize,       int, 9,        "See cv::StereoBM");
+    RTABMAP_PARAM(StereoBM, PreFilterCap,        int, 31,       "See cv::StereoBM");
+    RTABMAP_PARAM(StereoBM, UniquenessRatio,     int, 15,       "See cv::StereoBM");
+    RTABMAP_PARAM(StereoBM, TextureThreshold,    int, 10,       "See cv::StereoBM");
+    RTABMAP_PARAM(StereoBM, SpeckleWindowSize,   int, 100,      "See cv::StereoBM");
+    RTABMAP_PARAM(StereoBM, SpeckleRange,        int, 4,        "See cv::StereoBM");
+    RTABMAP_PARAM(StereoBM, Disp12MaxDiff,       int, -1,       "See cv::StereoBM");
+
+    RTABMAP_PARAM(StereoSGBM, BlockSize,         int, 15,       "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, MinDisparity,      int, 0,        "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, NumDisparities,    int, 128,      "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, PreFilterCap,      int, 31,       "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, UniquenessRatio,   int, 20,       "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, SpeckleWindowSize, int, 100,      "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, SpeckleRange,      int, 4,        "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, Disp12MaxDiff,     int, 1,        "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, P1,                int, 2,        "See cv::StereoSGBM");
+	RTABMAP_PARAM(StereoSGBM, P2,                int, 5,        "See cv::StereoSGBM");
+#if CV_MAJOR_VERSION < 3
+	RTABMAP_PARAM(StereoSGBM, Mode,              int, 0,        "See cv::StereoSGBM");
+#else
+	RTABMAP_PARAM(StereoSGBM, Mode,              int, 2,        "See cv::StereoSGBM");
+#endif
+
+    // Occupancy Grid
+    RTABMAP_PARAM(Grid, Sensor,                  int,    1,       "Create occupancy grid from selected sensor: 0=laser scan, 1=depth image(s) or 2=both laser scan and depth image(s).");
+    RTABMAP_PARAM(Grid, DepthDecimation,         unsigned int,  4, uFormat("[%s=true] Decimation of the depth image before creating cloud.", kGridDepthDecimation().c_str()));
+    RTABMAP_PARAM(Grid, RangeMin,                float,  0.0,     "Minimum range from sensor.");
+    RTABMAP_PARAM(Grid, RangeMax,                float,  5.0,     "Maximum range from sensor. 0=inf.");
+    RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", uFormat("[%s>=1] Region of interest ratios [left, right, top, bottom].", kGridSensor().c_str()));
+    RTABMAP_PARAM(Grid, FootprintLength,         float,  0.0,     "Footprint length used to filter points over the footprint of the robot.");
+    RTABMAP_PARAM(Grid, FootprintWidth,          float,  0.0,     "Footprint width used to filter points over the footprint of the robot. Footprint length should be set.");
+    RTABMAP_PARAM(Grid, FootprintHeight,         float,  0.0,     "Footprint height used to filter points over the footprint of the robot. Footprint length and width should be set.");
+    RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=0 or 2] Decimation of the laser scan before creating cloud.", kGridSensor().c_str()));
+    RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "Resolution of the occupancy grid.");
+    RTABMAP_PARAM(Grid, PreVoxelFiltering,       bool,   true,    uFormat("Input cloud is downsampled by voxel filter (voxel size is \"%s\") before doing segmentation of obstacles and ground.", kGridCellSize().c_str()));
+    RTABMAP_PARAM(Grid, MapFrameProjection,      bool,   false,   "Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.");
+    RTABMAP_PARAM(Grid, NormalsSegmentation,     bool,   true,    "Segment ground from obstacles using point normals, otherwise a fast passthrough is used.");
+    RTABMAP_PARAM(Grid, MaxObstacleHeight,       float,  0.0,     "Maximum obstacles height (0=disabled).");
+    RTABMAP_PARAM(Grid, MinGroundHeight,         float,  0.0,     "Minimum ground height (0=disabled).");
+    RTABMAP_PARAM(Grid, MaxGroundHeight,         float,  0.0,     uFormat("Maximum ground height (0=disabled). Should be set if \"%s\" is false.", kGridNormalsSegmentation().c_str()));
+    RTABMAP_PARAM(Grid, MaxGroundAngle,          float,  45,      uFormat("[%s=true] Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.", kGridNormalsSegmentation().c_str()));
+    RTABMAP_PARAM(Grid, NormalK,                 int,    20,      uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()));
+    RTABMAP_PARAM(Grid, ClusterRadius,           float,  0.1,     uFormat("[%s=true] Cluster maximum radius.", kGridNormalsSegmentation().c_str()));
+    RTABMAP_PARAM(Grid, MinClusterSize,          int,    10,      uFormat("[%s=true] Minimum cluster size to project the points.", kGridNormalsSegmentation().c_str()));
+    RTABMAP_PARAM(Grid, FlatObstacleDetected,    bool,   true,    uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
+#ifdef RTABMAP_OCTOMAP
+    RTABMAP_PARAM(Grid, 3D,                      bool,   true,    uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is 0.", kGridSensor().c_str()));
+#else
+    RTABMAP_PARAM(Grid, 3D,                      bool,   false,   uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is 0.", kGridSensor().c_str()));
+#endif
+    RTABMAP_PARAM(Grid, GroundIsObstacle,           bool,   false,   uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
+    RTABMAP_PARAM(Grid, NoiseFilteringRadius,       float,   0.0,    "Noise filtering radius (0=disabled). Done after segmentation.");
+    RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int,     5,      "Noise filtering minimum neighbors.");
+    RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,   bool,    false,  uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str()));
+    RTABMAP_PARAM(Grid, RayTracing,                 bool,   false,   uFormat("Ray tracing is done for each occupied cell, filling unknown space between the sensor and occupied cells. If %s=true, RTAB-Map should be built with OctoMap support, otherwise 3D ray tracing is ignored.", kGrid3D().c_str()));
+    RTABMAP_PARAM(GridGlobal, UpdateError,          float,  0.01,    "Graph changed detection error (m). Update map only if poses in new optimized graph have moved more than this value.");
+    RTABMAP_PARAM(GridGlobal, FootprintRadius,      float,  0.0,     "Footprint radius (m) used to clear all obstacles under the graph.");
+    RTABMAP_PARAM(GridGlobal, MinSize,              float,  0.0,     "Minimum map size (m).");
+    RTABMAP_PARAM(GridGlobal, Eroded,               bool,   false,   "Erode obstacle cells.");
+    RTABMAP_PARAM(GridGlobal, MaxNodes,             int,    0,       "Maximum nodes assembled in the map starting from the last node (0=unlimited).");
+    RTABMAP_PARAM(GridGlobal, AltitudeDelta,        float,  0,       "Assemble only nodes that have the same altitude of +-delta meters of the current pose (0=disabled). This is used to generate 2D occupancy grid based on the current altitude (e.g., multi-floor building).");
+    RTABMAP_PARAM(GridGlobal, OccupancyThr,         float,  0.5,     "Occupancy threshold (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbHit,              float,  0.7,     "Probability of a hit (value between 0.5 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbMiss,             float,  0.4,     "Probability of a miss (value between 0 and 0.5).");
+    RTABMAP_PARAM(GridGlobal, ProbClampingMin,      float,  0.1192,  "Probability clamping minimum (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbClampingMax,      float,  0.971,   "Probability clamping maximum (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, FloodFillDepth,       unsigned int, 0, "Flood fill filter (0=disabled), used to remove empty cells outside the map. The flood fill is done at the specified depth (between 1 and 16) of the OctoMap.");
+
+    RTABMAP_PARAM(Marker, Dictionary,             int,   0,     "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
+    RTABMAP_PARAM(Marker, Length,                 float, 0,     "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).");
+    RTABMAP_PARAM(Marker, MaxDepthError,          float, 0.01,  uFormat("Maximum depth error between all corners of a marker when estimating the marker length (when %s is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.", kMarkerLength().c_str()));
+    RTABMAP_PARAM(Marker, VarianceLinear,         float, 0.001, uFormat("Linear variance to set on marker detections. If %s is enabled and %s=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing.", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str()));
+    RTABMAP_PARAM(Marker, VarianceAngular,        float, 0.01,  uFormat("Angular variance to set on marker detections. If %s is enabled, it is ignored with %s=1 (g2o) and it corresponds to bearing variance with %s=2 (GTSAM).", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str(), kOptimizerStrategy().c_str()));
+    RTABMAP_PARAM(Marker, VarianceOrientationIgnored, bool, false, uFormat("When this setting is false, the landmark's orientation is optimized during graph optimization. When this setting is true, only the position of the landmark is optimized. This can be useful when the landmark's orientation estimation is not reliable. Note that for %s=1 (g2o), only %s needs be set if we ignore orientation. For %s=2 (GTSAM), instead of optimizing the landmark's position directly, a bearing/range factor is used, with %s as the variance of the range factor (with 9999 to optimize the position with only a bearing factor) and %s as the variance of the bearing factor (pitch/yaw).", kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kOptimizerStrategy().c_str(), kMarkerVarianceLinear().c_str(), kMarkerVarianceAngular().c_str()));
+    RTABMAP_PARAM(Marker, CornerRefinementMethod, int,   0,     "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
+    RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,   "Maximum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM(Marker, MinRange,               float, 0.0,   "Miniminum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM_STR(Marker, Priors,             "",           "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example:  \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).");
+    RTABMAP_PARAM(Marker, PriorsVarianceLinear,   float, 0.001, "Linear variance to set on marker priors.");
+    RTABMAP_PARAM(Marker, PriorsVarianceAngular,  float, 0.001, "Angular variance to set on marker priors.");
+
+    RTABMAP_PARAM(ImuFilter, MadgwickGain,                  double, 0.1,  "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal, belongs in [0, 1].");
+    RTABMAP_PARAM(ImuFilter, MadgwickZeta,                  double, 0.0,  "Gyro drift gain (approx. rad/s), belongs in [-1, 1].");
+
+    RTABMAP_PARAM(ImuFilter, ComplementaryGainAcc,          double, 0.01, "Gain parameter for the complementary filter, belongs in [0, 1].");
+    RTABMAP_PARAM(ImuFilter, ComplementaryBiasAlpha,        double, 0.01, "Bias estimation gain parameter, belongs in [0, 1].");
+    RTABMAP_PARAM(ImuFilter, ComplementaryDoBiasEstimation, bool,   true, "Parameter whether to do bias estimation or not.");
+    RTABMAP_PARAM(ImuFilter, ComplementaryDoAdpativeGain,   bool,   true, "Parameter whether to do adaptive gain or not.");
+
+public:
+    virtual ~Parameters();
+
+    /**
+     * Get default parameters
+     *
+     */
+    static const ParametersMap & getDefaultParameters()
+    {
+        return parameters_;
+    }
+
+    /**
+     * Get parameter type
+     *
+     */
+    static std::string getType(const std::string & paramKey);
+
+    /**
+     * Get parameter description
+     *
+     */
+    static std::string getDescription(const std::string & paramKey);
+
+    static bool parse(const ParametersMap & parameters, const std::string & key, bool & value);
+    static bool parse(const ParametersMap & parameters, const std::string & key, int & value);
+    static bool parse(const ParametersMap & parameters, const std::string & key, unsigned int & value);
+    static bool parse(const ParametersMap & parameters, const std::string & key, float & value);
+    static bool parse(const ParametersMap & parameters, const std::string & key, double & value);
+    static bool parse(const ParametersMap & parameters, const std::string & key, std::string & value);
+    static void parse(const ParametersMap & parameters, ParametersMap & parametersOut);
+
+    static const char * showUsage();
+    static ParametersMap parseArguments(int argc, char * argv[], bool onlyParameters = false);
+
+    static std::string getVersion();
+    static std::string getDefaultDatabaseName();
+
+    static std::string serialize(const ParametersMap & parameters);
+    static ParametersMap deserialize(const std::string & parameters);
+
+    static bool isFeatureParameter(const std::string & param);
+    static ParametersMap getDefaultOdometryParameters(bool stereo = false, bool vis = true, bool icp = false);
+    static ParametersMap getDefaultParameters(const std::string & group);
+    /**
+     * If remove=false: keep only parameters of the specified group.
+     * If remove=true: remove parameters of the specified group.
+     */
+    static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group, bool remove = false);
+
+    static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false);
+    static void readINIStr(const std::string & configContent, ParametersMap & parameters, bool modifiedOnly = false);
+    static void writeINI(const std::string & configFile, const ParametersMap & parameters);
+
+    /**
+     * Get removed parameters (backward compatibility)
+     * <OldKeyName, <isEqual, NewKeyName> >, when isEqual=true, the old value can be safely copied to new parameter
+     */
+    static const std::map<std::string, std::pair<bool, std::string> > & getRemovedParameters();
+
+    /**
+     * <NewKeyName, OldKeyName>
+     */
+    static const ParametersMap & getBackwardCompatibilityMap();
+
+    static std::string createDefaultWorkingDirectory();
+
+private:
+    Parameters();
+
+private:
+    static ParametersMap parameters_;
+    static ParametersMap parametersType_;
+    static ParametersMap descriptions_;
+    static Parameters instance_;
+
+    static std::map<std::string, std::pair<bool, std::string> > removedParameters_;
+    static ParametersMap backwardCompatibilityMap_;
+};
+
+}
+
+#endif /* PARAMETERS_H_ */

rtabmap/corelib/include/rtabmap/core/ParticleFilter.h

@@ -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.
+*/
+
+#ifndef PARTICLEFILTER_H_
+#define PARTICLEFILTER_H_
+
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/ULogger.h>
+
+
+namespace rtabmap {
+
+// taken from http://www.developpez.net/forums/d544518/c-cpp/c/equivalent-randn-matlab-c/
+#define TWOPI (6.2831853071795864769252867665590057683943387987502) /* 2 * pi */
+
+/*
+   RAND is a macro which returns a pseudo-random numbers from a uniform
+   distribution on the interval [0 1]
+*/
+#define RAND (rand())/((double) RAND_MAX)
+
+/*
+   RANDN is a macro which returns a pseudo-random numbers from a normal
+   distribution with mean zero and standard deviation one. This macro uses Box
+   Muller's algorithm
+*/
+#define RANDN (sqrt(-2.0*log(RAND))*cos(TWOPI*RAND))
+
+std::vector<double> cumSum(const std::vector<double> & v)
+{
+	std::vector<double> cum(v.size());
+	double sum = 0;
+	for(unsigned int i=0; i<v.size(); ++i)
+	{
+		cum[i] = v[i] + sum;
+		sum += v[i];
+	}
+	return cum;
+}
+
+std::vector<double> resample(const std::vector<double> & p, // particles
+							 const std::vector<double> & w, // weights
+							 bool normalizeWeights = false)
+{
+	std::vector<double> np; //new particles
+	if(p.size() != w.size() || p.size() == 0)
+	{
+		UERROR("particles (%d) and weights (%d) are not the same size", p.size(), w.size());
+		return np;
+	}
+
+	std::vector<double> cs;
+	if(normalizeWeights)
+	{
+		double wSum = uSum(w);
+		std::vector<double> wNorm(w.size());
+		for(unsigned int i=0; i<w.size(); ++i)
+		{
+			wNorm[i] = w[i]/wSum;
+		}
+		cs = cumSum(wNorm); // cumulative sum
+	}
+	else
+	{
+		cs = cumSum(w); // cumulative sum
+	}
+	for(unsigned int j=0; j<cs.size(); ++j)
+	{
+		cs[j]/=cs.back();
+	}
+
+	np.resize(p.size());
+	for(unsigned int i=0; i<np.size(); ++i)
+	{
+		unsigned int index = 0;
+		double randnum = RAND;
+		for(unsigned int j=0; j<cs.size(); ++j)
+		{
+			if(randnum < cs[j])
+			{
+				index = j;
+				break;
+			}
+		}
+		np[i] = p[index];
+	}
+	return np;
+}
+
+
+class ParticleFilter
+{
+public:
+	ParticleFilter(unsigned int nParticles = 200,
+				   double noise = 0.1,
+				   double lambda = 10.0,
+				   double initValue = 0.0) :
+		noise_(noise),
+		lambda_(lambda)
+	{
+		particles_.resize(nParticles, initValue);
+	}
+
+	void init(double initValue = 0.0f)
+	{
+		particles_ = std::vector<double>(particles_.size(), initValue);
+	}
+
+	double filter(double val)
+	{
+		std::vector<double> weights(particles_.size(), 1);
+		double sumWeights = 0;
+		for(unsigned int i=0; i<particles_.size(); ++i)
+		{
+			// add noise to particle
+			particles_[i] += noise_ * RANDN;
+
+			// compute weight
+			double dist = fabs(particles_[i] - val);
+			//dist = sqrt(dist*dist);
+			double w = exp(-lambda_*dist);
+			if(uIsFinite(w) && w > 0)
+			{
+				weights[i] = w;
+			}
+			sumWeights += weights[i];
+		}
+
+
+		//normalize and compute estimated value
+		double value =0.0;
+		for(unsigned int i=0; i<weights.size(); ++i)
+		{
+			weights[i] /= sumWeights;
+			value += weights[i] * particles_[i];
+		}
+
+		//resample the particles
+		particles_ = resample(particles_, weights, false);
+
+		return value;
+	}
+
+private:
+	std::vector<double> particles_;
+	double noise_;
+	double lambda_;
+};
+
+}
+
+
+#endif /* PARTICLEFILTER_H_ */

rtabmap/corelib/include/rtabmap/core/ProgressState.h

@@ -0,0 +1,62 @@
+/*
+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.
+*/
+
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_
+
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+class ProgressState
+{
+public:
+	ProgressState():canceled_(false){}
+	virtual bool callback(const std::string & msg) const
+	{
+		if(!msg.empty())
+			UDEBUG("msg=%s", msg.c_str());
+		return true;
+	}
+	virtual ~ProgressState(){}
+
+	void setCanceled(bool canceled)
+	{
+		canceled_ = canceled;
+	}
+	bool isCanceled() const
+	{
+		return canceled_;
+	}
+
+private:
+	bool canceled_;
+};
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_PROGRESSSTATE_H_ */

rtabmap/corelib/include/rtabmap/core/PythonInterface.h

@@ -0,0 +1,41 @@
+/*
+ * PythonInterface.h
+ *
+ *  Created on: Jan. 14, 2021
+ *      Author: mathieu
+ */
+
+#ifndef CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_
+#define CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_
+
+
+#include <string>
+#include <rtabmap/utilite/UMutex.h>
+
+namespace pybind11 {
+class scoped_interpreter;
+class gil_scoped_release;
+}
+
+namespace rtabmap {
+
+/**
+ * Create a single PythonInterface on main thread at
+ * global scope before any Python classes.
+ */
+class PythonInterface
+{
+public:
+	PythonInterface();
+	virtual ~PythonInterface();
+
+private:
+	pybind11::scoped_interpreter* guard_;
+	pybind11::gil_scoped_release* release_;
+};
+
+std::string getPythonTraceback();
+
+}
+
+#endif /* CORELIB_SRC_PYTHON_PYTHONINTERFACE_H_ */

rtabmap/corelib/include/rtabmap/core/Recovery.h

@@ -0,0 +1,56 @@
+/*
+Copyright (c) 2010-2017, 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.
+*/
+
+#ifndef RECOVERY_H_
+#define RECOVERY_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <string>
+
+namespace rtabmap {
+
+class ProgressState;
+
+/**
+ * Return true on success. The database is
+ * renamed to "*.backup.db" before recovering.
+ * @param corruptedDatabase database to recover
+ * @param keepCorruptedDatabase if false and on recovery success, the backup database is removed
+ * @param errorMsg error message if the function returns false
+ * @param progressState A ProgressState object used to get status of the recovery process
+ */
+bool RTABMAP_CORE_EXPORT databaseRecovery(
+		const std::string & corruptedDatabase,
+		bool keepCorruptedDatabase = true,
+		std::string * errorMsg = 0,
+		ProgressState * progressState = 0);
+
+}
+
+
+#endif /* RECOVERY_H_ */

rtabmap/corelib/include/rtabmap/core/Registration.h

@@ -0,0 +1,119 @@
+/*
+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.
+*/
+
+#ifndef RTABMAP_REGISTRATION_H_
+#define RTABMAP_REGISTRATION_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Signature.h>
+#include <rtabmap/core/RegistrationInfo.h>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT Registration
+{
+public:
+	enum Type {
+		kTypeUndef = -1,
+		kTypeVis = 0,
+		kTypeIcp = 1,
+		kTypeVisIcp = 2
+	};
+	static double COVARIANCE_LINEAR_EPSILON;
+	static double COVARIANCE_ANGULAR_EPSILON;
+
+public:
+	static Registration * create(const ParametersMap & parameters);
+	static Registration * create(Type & type, const ParametersMap & parameters = ParametersMap());
+
+public:
+	virtual ~Registration();
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	bool isImageRequired() const;
+	bool isScanRequired() const;
+	bool isUserDataRequired() const;
+
+	bool canUseGuess() const;
+
+	int getMinVisualCorrespondences() const;
+	float getMinGeometryCorrespondencesRatio() const;
+
+	bool repeatOnce() const {return repeatOnce_;}
+	bool force3DoF() const {return force3DoF_;}
+
+	// take ownership!
+	void setChildRegistration(Registration * child);
+
+	Transform computeTransformation(
+			const Signature & from,
+			const Signature & to,
+			Transform guess = Transform::getIdentity(),
+			RegistrationInfo * info = 0) const;
+	Transform computeTransformation(
+			const SensorData & from,
+			const SensorData & to,
+			Transform guess = Transform::getIdentity(),
+			RegistrationInfo * info = 0) const;
+
+	Transform computeTransformationMod(
+			Signature & from,
+			Signature & to,
+			Transform guess = Transform::getIdentity(),
+			RegistrationInfo * info = 0) const;
+
+protected:
+	// take ownership of child
+	Registration(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
+
+	// It is safe to modify the signatures in the implementation, if so, the
+	// child registration will use these modifications.
+	virtual Transform computeTransformationImpl(
+			Signature & from,
+			Signature & to,
+			Transform guess,
+			RegistrationInfo & info) const = 0;
+
+	virtual bool isImageRequiredImpl() const {return false;}
+	virtual bool isScanRequiredImpl() const {return false;}
+	virtual bool isUserDataRequiredImpl() const {return false;}
+	virtual bool canUseGuessImpl() const {return false;}
+	virtual int getMinVisualCorrespondencesImpl() const {return 0;}
+	virtual float getMinGeometryCorrespondencesRatioImpl() const {return 0.0f;}
+
+private:
+	bool repeatOnce_;
+	bool force3DoF_;
+	Registration * child_;
+
+};
+
+}
+
+#endif /* RTABMAP_REGISTRATION_H_ */

rtabmap/corelib/include/rtabmap/core/RegistrationIcp.h

@@ -0,0 +1,96 @@
+/*
+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.
+*/
+
+#ifndef REGISTRATIONICP_H_
+#define REGISTRATIONICP_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Registration.h>
+#include <rtabmap/core/Signature.h>
+
+namespace rtabmap {
+
+// Geometrical registration
+class RTABMAP_CORE_EXPORT RegistrationIcp : public Registration
+{
+public:
+	// take ownership of child
+	RegistrationIcp(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
+	virtual ~RegistrationIcp();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+
+protected:
+	virtual Transform computeTransformationImpl(
+			Signature & from,
+			Signature & to,
+			Transform guess,
+			RegistrationInfo & info) const;
+	virtual bool isScanRequiredImpl() const {return true;}
+	virtual bool canUseGuessImpl() const {return true;}
+	virtual float getMinGeometryCorrespondencesRatioImpl() const {return _correspondenceRatio;}
+
+private:
+	int _strategy;
+	float _maxTranslation;
+	float _maxRotation;
+	float _voxelSize;
+	int _downsamplingStep;
+	float _rangeMin;
+	float _rangeMax;
+	float _maxCorrespondenceDistance;
+	bool _reciprocalCorrespondences;
+	int _maxIterations;
+	float _epsilon;
+	float _correspondenceRatio;
+	bool _force4DoF;
+	int _filtersEnabled;
+	bool _pointToPlane;
+	int _pointToPlaneK;
+	float _pointToPlaneRadius;
+	float _pointToPlaneGroundNormalsUp;
+	float _pointToPlaneMinComplexity;
+	int _pointToPlaneLowComplexityStrategy;
+	std::string _libpointmatcherConfig;
+	int _libpointmatcherKnn;
+	float _libpointmatcherEpsilon;
+	bool _libpointmatcherIntensity;
+	float _outlierRatio;
+	unsigned int _ccSamplingLimit;
+	bool _ccFilterOutFarthestPoints;
+	double _ccMaxFinalRMS;
+	std::string _debugExportFormat;
+	std::string _workingDir;
+
+	void * _libpointmatcherICP;
+	void * _libpointmatcherICPFilters;
+};
+
+}
+
+#endif /* REGISTRATIONICP_H_ */

rtabmap/corelib/include/rtabmap/core/RegistrationInfo.h

@@ -0,0 +1,105 @@
+/*
+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.
+*/
+
+#ifndef REGISTRATIONINFO_H_
+#define REGISTRATIONINFO_H_
+
+
+namespace rtabmap {
+
+class RegistrationInfo
+{
+public:
+	RegistrationInfo() :
+		totalTime(0.0),
+		inliers(0),
+		inliersRatio(0),
+		inliersMeanDistance(0.0f),
+		inliersDistribution(0.0f),
+		matches(0),
+		icpInliersRatio(0),
+		icpTranslation(0.0f),
+		icpRotation(0.0f),
+		icpStructuralComplexity(0.0f),
+		icpStructuralDistribution(0.0f),
+		icpCorrespondences(0),
+		icpRMS(0)
+
+	{
+	}
+
+	RegistrationInfo copyWithoutData() const
+	{
+		RegistrationInfo output;
+		output.totalTime = totalTime;
+		output.covariance = covariance.clone();
+		output.rejectedMsg = rejectedMsg;
+		output.inliers = inliers;
+		output.inliersPerCam = inliersPerCam;
+		output.inliersMeanDistance = inliersMeanDistance;
+		output.inliersDistribution = inliersDistribution;
+		output.matches = matches;
+		output.matchesPerCam = matchesPerCam;
+		output.icpInliersRatio = icpInliersRatio;
+		output.icpTranslation = icpTranslation;
+		output.icpRotation = icpRotation;
+		output.icpStructuralComplexity = icpStructuralComplexity;
+		output.icpStructuralDistribution = icpStructuralDistribution;
+		output.icpCorrespondences = icpCorrespondences;
+        output.icpRMS = icpRMS;
+		return output;
+	}
+
+	cv::Mat covariance;
+	std::string rejectedMsg;
+	double totalTime;
+
+	// RegistrationVis
+	int inliers;
+	float inliersRatio;
+	float inliersMeanDistance;
+	float inliersDistribution;
+	std::vector<int> inliersIDs;
+	int matches;
+	std::vector<int> matchesIDs;
+	std::vector<int> projectedIDs; // "From" IDs
+	std::vector<int> inliersPerCam;
+	std::vector<int> matchesPerCam;
+
+	// RegistrationIcp
+	float icpInliersRatio;
+	float icpTranslation;
+	float icpRotation;
+	float icpStructuralComplexity;
+	float icpStructuralDistribution;
+	int   icpCorrespondences;
+	float icpRMS;
+};
+
+}
+
+#endif /* REGISTRATIONINFO_H_ */

rtabmap/corelib/include/rtabmap/core/RegistrationVis.h

@@ -0,0 +1,120 @@
+/*
+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.
+*/
+
+#ifndef REGISTRATIONVIS_H_
+#define REGISTRATIONVIS_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Registration.h>
+#include <rtabmap/core/Signature.h>
+
+namespace rtabmap {
+
+class Feature2D;
+
+#ifdef RTABMAP_PYTHON
+class PyMatcher;
+#endif
+
+// Visual registration
+class RTABMAP_CORE_EXPORT RegistrationVis : public Registration
+{
+public:
+	// take ownership of child
+	RegistrationVis(const ParametersMap & parameters = ParametersMap(), Registration * child = 0);
+	virtual ~RegistrationVis();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	float getInlierDistance() const {return _inlierDistance;}
+	int getIterations() const {return _iterations;}
+	int getMinInliers() const {return _minInliers;}
+	int getNNType() const {return _nnType;}
+	float getNNDR() const {return _nndr;}
+	int getEstimationType() const {return _estimationType;}
+
+	const Feature2D * getDetector() const {return _detectorFrom;}
+
+protected:
+	virtual Transform computeTransformationImpl(
+			Signature & from,
+			Signature & to,
+			Transform guess,
+			RegistrationInfo & info) const;
+
+	virtual bool isImageRequiredImpl() const {return true;}
+	virtual bool canUseGuessImpl() const {return _correspondencesApproach != 0 || _guessWinSize>0;}
+	virtual int getMinVisualCorrespondencesImpl() const {return _minInliers;}
+
+private:
+	int _minInliers;
+	float _inlierDistance;
+	int _iterations;
+	int _refineIterations;
+	float _epipolarGeometryVar;
+	int _estimationType;
+	float _PnPReprojError;
+	int _PnPFlags;
+	int _PnPRefineIterations;
+	int  _PnPVarMedianRatio;
+	float _PnPMaxVar;
+	bool _PnPSplitLinearCovarianceComponents;
+	unsigned int _multiSamplingPolicy;
+	int _correspondencesApproach;
+	int _flowWinSize;
+	int _flowIterations;
+	float _flowEps;
+	int _flowMaxLevel;
+	bool _flowGpu;
+	float _nndr;
+	int _nnType;
+	bool _gmsWithRotation;
+	bool _gmsWithScale;
+	double _gmsThresholdFactor;
+	int _guessWinSize;
+	bool _guessMatchToProjection;
+	int _bundleAdjustment;
+	bool _depthAsMask;
+	float _maskFloorThreshold;
+	float _minInliersDistributionThr;
+	float _maxInliersMeanDistance;
+
+	ParametersMap _featureParameters;
+	ParametersMap _bundleParameters;
+
+	Feature2D * _detectorFrom;
+	Feature2D * _detectorTo;
+
+#ifdef RTABMAP_PYTHON
+	PyMatcher * _pyMatcher;
+#endif
+};
+
+}
+
+#endif /* REGISTRATION_H_ */

rtabmap/corelib/include/rtabmap/core/Rtabmap.h

@@ -0,0 +1,401 @@
+/*
+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.
+*/
+
+#ifndef RTABMAP_H_
+#define RTABMAP_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/SensorData.h"
+#include "rtabmap/core/Statistics.h"
+#include "rtabmap/core/Link.h"
+#include "rtabmap/core/ProgressState.h"
+
+#include <opencv2/core/core.hpp>
+#include <list>
+#include <stack>
+#include <set>
+
+namespace rtabmap
+{
+
+class EpipolarGeometry;
+class Memory;
+class BayesFilter;
+class Signature;
+class Optimizer;
+class PythonInterface;
+
+class RTABMAP_CORE_EXPORT Rtabmap
+{
+public:
+	enum VhStrategy {kVhNone, kVhEpipolar, kVhUndef};
+
+public:
+	Rtabmap();
+	virtual ~Rtabmap();
+
+	/**
+	 * @brief Main loop of rtabmap.
+	 * @param data Sensor data to process.
+	 * @param odomPose Odometry pose, should be non-null for RGB-D SLAM mode.
+	 * @param covariance Odometry covariance.
+	 * @param externalStats External statistics to be saved in the database for convenience
+	 * @return true if data has been added to map.
+	 */
+	bool process(
+			const SensorData & data,
+			Transform odomPose,
+			const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
+			const std::vector<float> & odomVelocity = std::vector<float>(),
+			const std::map<std::string, float> & externalStats = std::map<std::string, float>());
+	// for convenience
+	bool process(
+			const SensorData & data,
+			Transform odomPose,
+			float odomLinearVariance,
+			float odomAngularVariance,
+			const std::vector<float> & odomVelocity = std::vector<float>(),
+			const std::map<std::string, float> & externalStats = std::map<std::string, float>());
+	// for convenience, loop closure detection only
+	bool process(
+			const cv::Mat & image,
+			int id=0, const std::map<std::string, float> & externalStats = std::map<std::string, float>());
+
+	/**
+	 * Initialize Rtabmap with parameters and a database
+	 * @param parameters Parameters overriding default parameters and database parameters
+	 *                   (@see loadDatabaseParameters)
+	 * @param databasePath The database input/output path. If not set, an
+	 *                     empty database is used in RAM. If set and the file doesn't exist,
+	 *                     it will be created empty. If the database exists, nodes and
+	 *                     vocabulary will be loaded in working memory.
+	 * @param loadDatabaseParameters If an existing database is used (@see databasePath),
+	 *                               the parameters inside are loaded and set to current
+	 *                               Rtabmap instance.
+	 */
+	void init(const ParametersMap & parameters, const std::string & databasePath = "", bool loadDatabaseParameters = false);
+	/**
+	 * Initialize Rtabmap with parameters from a configuration file and a database
+	 * @param configFile Configuration file (*.ini) overriding default parameters and database parameters
+	 *                   (@see loadDatabaseParameters)
+	 * @param databasePath The database input/output path. If not set, an
+	 *                     empty database is used in RAM. If set and the file doesn't exist,
+	 *                     it will be created empty. If the database exists, nodes and
+	 *                     vocabulary will be loaded in working memory.
+	 * @param loadDatabaseParameters If an existing database is used (@see databasePath),
+	 *                               the parameters inside are loaded and set to current
+	 *                               Rtabmap instance.
+	 */
+	void init(const std::string & configFile = "", const std::string & databasePath = "", bool loadDatabaseParameters = false);
+
+	/**
+	 * Close rtabmap. This will delete rtabmap object if set.
+	 * @param databaseSaved true=database saved, false=database discarded.
+	 * @param databasePath output database file name, ignored if
+	 *                     Db/Sqlite3InMemory=false (opened database is
+	 *                     then overwritten).
+	 */
+	void close(bool databaseSaved = true, const std::string & ouputDatabasePath = "");
+
+	const std::string & getWorkingDir() const {return _wDir;}
+	bool isRGBDMode() const { return _rgbdSlamMode; }
+	int getLoopClosureId() const {return _loopClosureHypothesis.first;}
+	float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
+	int getHighestHypothesisId() const {return _highestHypothesis.first;}
+	float getHighestHypothesisValue() const {return _highestHypothesis.second;}
+	int getLastLocationId() const;
+	std::list<int> getWM() const; // working memory
+	std::set<int> getSTM() const; // short-term memory
+	int getWMSize() const; // working memory size
+	int getSTMSize() const; // short-term memory size
+	std::map<int, int> getWeights() const;
+	int getTotalMemSize() const;
+	double getLastProcessTime() const {return _lastProcessTime;};
+	bool isInSTM(int locationId) const;
+	bool isIDsGenerated() const;
+	const Statistics & getStatistics() const;
+	const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
+	const std::multimap<int, Link> & getLocalConstraints() const {return _constraints;}
+	Transform getPose(int locationId) const;
+	Transform getMapCorrection() const {return _mapCorrection;}
+	const Memory * getMemory() const {return _memory;}
+	float getGoalReachedRadius() const {return _goalReachedRadius;}
+	float getLocalRadius() const {return _localRadius;}
+	const Transform & getLastLocalizationPose() const {return _lastLocalizationPose;}
+
+	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
+	void setTimeThreshold(float maxTimeAllowed); // in ms
+	int getMemoryThreshold() const {return _maxMemoryAllowed;} // in nodes
+	void setMemoryThreshold(int maxMemoryAllowed); // in nodes
+
+	void setInitialPose(const Transform & initialPose);
+	int triggerNewMap();
+	bool labelLocation(int id, const std::string & label);
+	/**
+	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
+	 * compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * If you have one dimension unsigned 8 bits raw data, make sure to transpose it
+	 * (to have multiple rows instead of multiple columns) in order to be detected as
+	 * not compressed.
+	 */
+	bool setUserData(int id, const cv::Mat & data);
+	void generateDOTGraph(const std::string & path, int id=0, int margin=5);
+	void exportPoses(
+			const std::string & path,
+			bool optimized,
+			bool global,
+			int format // 0=raw, 1=rgbd-slam format, 2=KITTI format, 3=TORO, 4=g2o
+	);
+	void resetMemory();
+	void dumpPrediction() const;
+	void dumpData() const;
+	void parseParameters(const ParametersMap & parameters);
+	const ParametersMap & getParameters() const {return _parameters;}
+	void setWorkingDirectory(std::string path);
+	void rejectLastLoopClosure();
+	void deleteLastLocation();
+	void setOptimizedPoses(const std::map<int, Transform> & poses, const std::multimap<int, Link> & constraints);
+	Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const;
+	// Use getGraph() instead with withImages=true, withScan=true, withUserData=true and withGrid=true.
+	RTABMAP_DEPRECATED
+		void get3DMap(std::map<int, Signature> & signatures,
+				std::map<int, Transform> & poses,
+				std::multimap<int, Link> & constraints,
+				bool optimized,
+				bool global) const;
+	void getGraph(std::map<int, Transform> & poses,
+			std::multimap<int, Link> & constraints,
+			bool optimized,
+			bool global,
+			std::map<int, Signature> * signatures = 0,
+			bool withImages = false,
+			bool withScan = false,
+			bool withUserData = false,
+			bool withGrid = false,
+			bool withWords = true,
+			bool withGlobalDescriptors = true) const;
+	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius, int k=0, std::map<int, float> * distsSqr=0); // If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks.
+	std::map<int, Transform> getNodesInRadius(int nodeId, float radius, int k=0, std::map<int, float> * distsSqr=0); // If nodeId==0, return poses around latest node. If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
+	int detectMoreLoopClosures(
+			float clusterRadiusMax = 0.5f,
+			float clusterAngle = M_PI/6.0f,
+			int iterations = 1,
+			bool intraSession = true,
+			bool interSession = true,
+			const ProgressState * state = 0,
+			float clusterRadiusMin = 0.0f);
+	bool globalBundleAdjustment(
+			int optimizerType = 1 /*g2o*/,
+			bool rematchFeatures = true,
+			int iterations = 0,
+			float pixelVariance = 0.0f);
+	int cleanupLocalGrids(
+			const std::map<int, Transform> & mapPoses,
+			const cv::Mat & map,
+			float xMin,
+			float yMin,
+			float cellSize,
+			int cropRadius = 1,
+			bool filterScans = false);
+	int refineLinks();
+	bool addLink(const Link & link);
+	cv::Mat getInformation(const cv::Mat & covariance) const;
+	void addNodesToRepublish(const std::vector<int> & ids);
+
+	int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
+	void clearPath(int status); // -1=failed 0=idle/executing 1=success
+	bool computePath(int targetNode, bool global);
+	bool computePath(const Transform & targetPose, float tolerance = -1.0f); // only in current optimized map, tolerance (m) < 0 means RGBD/LocalRadius, 0 means infinite
+	const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
+	std::vector<std::pair<int, Transform> > getPathNextPoses() const;
+	std::vector<int> getPathNextNodes() const;
+	int getPathCurrentGoalId() const;
+	unsigned int getPathCurrentIndex() const {return _pathCurrentIndex;}
+	unsigned int getPathCurrentGoalIndex() const {return _pathGoalIndex;}
+	const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
+
+	std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
+	std::map<int, std::map<int, Transform> > getPaths(const std::map<int, Transform> & poses, const Transform & target, int maxGraphDepth = 0) const;
+	void adjustLikelihood(std::map<int, float> & likelihood) const;
+	std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
+											const std::map<int, float> & likelihood) const;
+
+private:
+	void optimizeCurrentMap(int id,
+			bool lookInDatabase,
+			std::map<int, Transform> & optimizedPoses,
+			cv::Mat & covariance,
+			std::multimap<int, Link> * constraints = 0,
+			double * error = 0,
+			int * iterationsDone = 0) const;
+	std::map<int, Transform> optimizeGraph(
+			int fromId,
+			const std::set<int> & ids,
+			const std::map<int, Transform> & guessPoses,
+			bool lookInDatabase,
+			cv::Mat & covariance,
+			std::multimap<int, Link> * constraints = 0,
+			double * error = 0,
+			int * iterationsDone = 0) const;
+	void updateGoalIndex();
+	bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);
+
+	void createGlobalScanMap();
+
+	void setupLogFiles(bool overwrite = false);
+	void flushStatisticLogs();
+
+private:
+	// Modifiable parameters
+	bool _publishStats;
+	bool _publishLastSignatureData;
+	bool _publishPdf;
+	bool _publishLikelihood;
+	bool _publishRAMUsage;
+	bool _computeRMSE;
+	bool _saveWMState;
+	float _maxTimeAllowed; // in ms
+	unsigned int _maxMemoryAllowed; // signatures count in WM
+	float _loopThr;
+	float _loopRatio;
+	float _aggressiveLoopThr;
+	int _virtualPlaceLikelihoodRatio;
+	float _maxLoopClosureDistance;
+	bool _verifyLoopClosureHypothesis;
+	unsigned int _maxRetrieved;
+	unsigned int _maxLocalRetrieved;
+	unsigned int _maxRepublished;
+	bool _rawDataKept;
+	bool _statisticLogsBufferedInRAM;
+	bool _statisticLogged;
+	bool _statisticLoggedHeaders;
+	bool _rgbdSlamMode;
+	float _rgbdLinearUpdate;
+	float _rgbdAngularUpdate;
+	float _rgbdLinearSpeedUpdate;
+	float _rgbdAngularSpeedUpdate;
+	float _newMapOdomChangeDistance;
+	bool _neighborLinkRefining;
+	bool _proximityByTime;
+	bool _proximityBySpace;
+	bool _scanMatchingIdsSavedInLinks;
+	bool _loopClosureIdentityGuess;
+	float _localRadius;
+	float _localImmunizationRatio;
+	int _proximityMaxGraphDepth;
+	int _proximityMaxPaths;
+	int _proximityMaxNeighbors;
+	float _proximityFilteringRadius;
+	bool _proximityRawPosesUsed;
+	float _proximityAngle;
+	bool _proximityOdomGuess;
+	double _proximityMergedScanCovFactor;
+	std::string _databasePath;
+	bool _optimizeFromGraphEnd;
+	float _optimizationMaxError;
+	bool _startNewMapOnLoopClosure;
+	bool _startNewMapOnGoodSignature;
+	float _goalReachedRadius; // meters
+	bool _goalsSavedInUserData;
+	int _pathStuckIterations;
+	float _pathLinearVelocity;
+	float _pathAngularVelocity;
+	bool _forceOdom3doF;
+	bool _restartAtOrigin;
+	bool _loopCovLimited;
+	bool _loopGPS;
+	int _maxOdomCacheSize;
+	bool _localizationSmoothing;
+	double _localizationPriorInf;
+	bool _localizationSecondTryWithoutProximityLinks;
+	bool _createGlobalScanMap;
+	float _markerPriorsLinearVariance;
+	float _markerPriorsAngularVariance;
+
+	std::pair<int, float> _loopClosureHypothesis;
+	std::pair<int, float> _highestHypothesis;
+	double _lastProcessTime;
+	bool _someNodesHaveBeenTransferred;
+	float _distanceTravelled;
+	float _distanceTravelledSinceLastLocalization;
+	bool _optimizeFromGraphEndChanged;
+
+	// Abstract classes containing all loop closure
+	// strategies for a type of signature or configuration.
+	EpipolarGeometry * _epipolarGeometry;
+	BayesFilter * _bayesFilter;
+	Optimizer * _graphOptimizer;
+	ParametersMap _parameters;
+
+	Memory * _memory;
+
+	FILE* _foutFloat;
+	FILE* _foutInt;
+	std::list<std::string> _bufferedLogsF;
+	std::list<std::string> _bufferedLogsI;
+
+	Statistics statistics_;
+
+	std::string _wDir;
+
+	std::map<int, Transform> _optimizedPoses;
+	std::multimap<int, Link> _constraints;
+	Transform _mapCorrection;
+	Transform _mapCorrectionBackup; // used in localization mode when odom is lost
+	Transform _lastLocalizationPose; // Corrected odometry pose. In mapping mode, this corresponds to last pose return by getLocalOptimizedPoses().
+	int _lastLocalizationNodeId; // for localization mode
+	cv::Mat _localizationCovariance;
+	std::map<int, std::pair<cv::Point3d, Transform> > _gpsGeocentricCache;
+	bool _currentSessionHasGPS;
+	LaserScan _globalScanMap;
+	std::map<int, Transform> _globalScanMapPoses;
+	std::map<int, Transform> _odomCachePoses;       // used in localization mode to reject loop closures
+	std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
+	std::map<int, Transform> _markerPriors;
+
+	std::set<int> _nodesToRepublish;
+
+	// Planning stuff
+	int _pathStatus;
+	std::vector<std::pair<int,Transform> > _path;
+	std::set<unsigned int> _pathUnreachableNodes;
+	unsigned int _pathCurrentIndex;
+	unsigned int _pathGoalIndex;
+	Transform _pathTransformToGoal;
+	int _pathStuckCount;
+	float _pathStuckDistance;
+
+#ifdef RTABMAP_PYTHON
+	PythonInterface * _python;
+#endif
+
+};
+
+} // namespace rtabmap
+#endif /* RTABMAP_H_ */

rtabmap/corelib/include/rtabmap/core/RtabmapEvent.h

@@ -0,0 +1,265 @@
+/*
+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.
+*/
+
+#ifndef RTABMAPEVENT_H_
+#define RTABMAPEVENT_H_
+
+
+
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UVariant.h>
+#include "rtabmap/core/Statistics.h"
+#include "rtabmap/core/Parameters.h"
+
+namespace rtabmap
+{
+
+////////// The RtabmapEvent class //////////////
+class RtabmapEvent : public UEvent
+{
+public:
+	RtabmapEvent(const Statistics & stats) :
+		UEvent(0),
+		_stats(stats) {}
+
+	virtual ~RtabmapEvent() {}
+	const Statistics & getStats() const {return _stats;}
+	virtual std::string getClassName() const {return std::string("RtabmapEvent");}
+
+private:
+	Statistics _stats;
+};
+
+class RtabmapEventCmd : public UEvent
+{
+public:
+	enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Cmd enum ?!
+	enum Cmd {
+			kCmdUndef,
+			kCmdInit,             // params: [string] database path + ParametersMap
+			kCmdResetMemory,
+			kCmdClose,            // params: [bool] database saved (default true), [string] output database path (empty=use same database to save, only work when Db/Sqlite3InMemory=true)
+			kCmdUpdateParams,     // params: ParametersMap
+			kCmdDumpMemory,
+			kCmdDumpPrediction,
+			kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
+			kCmdExportPoses,      // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o)
+			kCmdCleanDataBuffer,
+			kCmdPublish3DMap,     // params: [bool] global, [bool] optimized, [bool] graphOnly
+			kCmdRepublishData,    // params: [vector<int>] ids
+			kCmdTriggerNewMap,
+			kCmdPause,
+			kCmdResume,
+			kCmdGoal,             // params: [string] label or [int] location ID
+			kCmdCancelGoal,
+			kCmdLabel,            // params: [string] label, [int] location ID
+			kCmdRemoveLabel       // params: [string] label
+	};
+public:
+	RtabmapEventCmd(Cmd cmd, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			value2_(value2),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const UVariant & value3, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			value2_(value2),
+			value3_(value3),
+			parameters_(parameters){}
+	RtabmapEventCmd(Cmd cmd, const UVariant & value1, const UVariant & value2, const UVariant & value3, const UVariant & value4, const ParametersMap & parameters = ParametersMap()) :
+			UEvent(0),
+			cmd_(cmd),
+			value1_(value1),
+			value2_(value2),
+			value3_(value3),
+			value4_(value4),
+			parameters_(parameters){}
+
+	virtual ~RtabmapEventCmd() {}
+	Cmd getCmd() const {return cmd_;}
+
+	const UVariant & value1() const {return value1_;}
+	const UVariant & value2() const {return value2_;}
+	const UVariant & value3() const {return value3_;}
+	const UVariant & value4() const {return value4_;}
+
+	const ParametersMap & getParameters() const {return parameters_;}
+
+	virtual std::string getClassName() const {return std::string("RtabmapEventCmd");}
+
+private:
+	Cmd cmd_;
+	UVariant value1_;
+	UVariant value2_;
+	UVariant value3_;
+	UVariant value4_;
+	ParametersMap parameters_;
+};
+
+class RtabmapEventInit : public UEvent
+{
+public:
+	enum dummy {d}; // Hack, to fix Eclipse complaining about not defined Status enum ?!
+	enum Status {
+		kInitializing,
+		kInitialized,
+		kClosing,
+		kClosed,
+		kInfo,
+		kError
+	};
+
+public:
+	RtabmapEventInit(Status status, const std::string & info = std::string()) :
+		UEvent(0),
+		_status(status),
+		_info(info)
+	{}
+
+	// for convenience
+	RtabmapEventInit(const std::string & info) :
+		UEvent(0),
+		_status(kInfo),
+		_info(info)
+	{}
+
+	Status getStatus() const {return _status;}
+	const std::string & getInfo() const {return _info;}
+
+	virtual ~RtabmapEventInit() {}
+	virtual std::string getClassName() const {return std::string("RtabmapEventInit");}
+private:
+	Status _status;
+	std::string _info; // "Loading signatures", "Loading words" ...
+};
+
+class RtabmapEvent3DMap : public UEvent
+{
+public:
+	RtabmapEvent3DMap(int codeError = 0):
+		UEvent(codeError){}
+	RtabmapEvent3DMap(
+			const std::map<int, Signature> & signatures,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & constraints) :
+		UEvent(0),
+		_signatures(signatures),
+		_poses(poses),
+		_constraints(constraints)
+	{}
+
+	virtual ~RtabmapEvent3DMap() {}
+
+	const std::map<int, Signature> & getSignatures() const {return _signatures;}
+	const std::map<int, Transform> & getPoses() const {return _poses;}
+	const std::multimap<int, Link> & getConstraints() const {return _constraints;}
+
+	virtual std::string getClassName() const {return std::string("RtabmapEvent3DMap");}
+
+private:
+	std::map<int, Signature> _signatures;
+	std::map<int, Transform> _poses;
+	std::multimap<int, Link> _constraints;
+};
+
+class RtabmapGlobalPathEvent : public UEvent
+{
+public:
+	RtabmapGlobalPathEvent():
+		UEvent(0),
+		_planningTime(0.0) {}
+	RtabmapGlobalPathEvent(
+			int goalId,
+			const std::vector<std::pair<int, Transform> > & poses,
+			double planningTime) :
+				UEvent(goalId),
+				_poses(poses),
+				_planningTime(planningTime) {}
+	RtabmapGlobalPathEvent(
+			int goalId,
+			const std::string & goalLabel,
+			const std::vector<std::pair<int, Transform> > & poses,
+			double planningTime) :
+				UEvent(goalId),
+				_goalLabel(goalLabel),
+				_poses(poses),
+				_planningTime(planningTime) {}
+
+	virtual ~RtabmapGlobalPathEvent() {}
+	int getGoal() const {return this->getCode();}
+	const std::string & getGoalLabel() const {return _goalLabel;}
+	double getPlanningTime() const {return _planningTime;}
+	const std::vector<std::pair<int, Transform> > & getPoses() const {return _poses;}
+	virtual std::string getClassName() const {return std::string("RtabmapGlobalPathEvent");}
+
+private:
+	std::string _goalLabel;
+	std::vector<std::pair<int, Transform> > _poses;
+	double _planningTime;
+};
+
+class RtabmapLabelErrorEvent : public UEvent
+{
+public:
+	RtabmapLabelErrorEvent(int id, const std::string & label):
+		UEvent(id),
+		_label(label){}
+
+	virtual ~RtabmapLabelErrorEvent() {}
+	int id() const {return this->getCode();}
+	const std::string & label() const {return _label;}
+	virtual std::string getClassName() const {return std::string("RtabmapLabelErrorEvent");}
+
+private:
+	std::string _label;
+};
+
+class RtabmapGoalStatusEvent : public UEvent
+{
+public:
+	RtabmapGoalStatusEvent(int status):
+		UEvent(status){}
+
+	virtual ~RtabmapGoalStatusEvent() {}
+	virtual std::string getClassName() const {return std::string("RtabmapGoalStatusEvent");}
+};
+
+} // namespace rtabmap
+
+#endif /* RTABMAPEVENT_H_ */

rtabmap/corelib/include/rtabmap/core/RtabmapThread.h

@@ -0,0 +1,122 @@
+/*
+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.
+*/
+
+#ifndef RTABMAPTHREAD_H_
+#define RTABMAPTHREAD_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/utilite/UThreadNode.h>
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UMutex.h>
+
+#include "rtabmap/core/RtabmapEvent.h"
+#include "rtabmap/core/SensorData.h"
+#include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/OdometryEvent.h"
+
+#include <queue>
+
+class UTimer;
+
+namespace rtabmap {
+
+class Rtabmap;
+
+class RTABMAP_CORE_EXPORT RtabmapThread :
+	public UThreadNode,
+	public UEventsHandler
+{
+public:
+	enum State {
+		kStateDetecting,
+		kStateProcessCommand
+	};
+
+public:
+	// take ownership
+	RtabmapThread(Rtabmap * rtabmap);
+	virtual ~RtabmapThread();
+
+	void clearBufferedData();
+	void setDetectorRate(float rate);
+	void setDataBufferSize(unsigned int bufferSize);
+	void createIntermediateNodes(bool enabled);
+
+	float getDetectorRate() const {return _rate;}
+	unsigned int getDataBufferSize() const {return _dataBufferMaxSize;}
+	bool getCreateIntermediateNodes() const {return _createIntermediateNodes;}
+
+	/**
+	 * Close rtabmap. This will delete rtabmap object if set.
+	 * @param databaseSaved true=database saved, false=database discarded.
+	 * @param databasePath output database file name, ignored if
+	 *                     Db/Sqlite3InMemory=false (opened database is
+	 *                     then overwritten).
+	 */
+	void close(bool databaseSaved, const std::string & databasePath = "");
+
+protected:
+	virtual bool handleEvent(UEvent * anEvent);
+
+private:
+	virtual void mainLoopBegin();
+	virtual void mainLoop();
+	virtual void mainLoopKill();
+	void process();
+	void addData(const OdometryEvent & odomEvent);
+	bool getData(OdometryEvent & data);
+	void pushNewState(State newState, const RtabmapEventCmd & cmdEvent = RtabmapEventCmd(RtabmapEventCmd::kCmdUndef));
+	void publishMap(bool optimized, bool full, bool graphOnly) const;
+
+private:
+	UMutex _stateMutex;
+	std::queue<State> _state;
+	std::queue<RtabmapEventCmd> _stateParam;
+
+	std::list<OdometryEvent> _dataBuffer;
+	std::list<double> _newMapEvents;
+	UMutex _dataMutex;
+	USemaphore _dataAdded;
+	unsigned int _dataBufferMaxSize;
+	float _rate;
+	bool _createIntermediateNodes;
+	UTimer * _frameRateTimer;
+	double _previousStamp;
+
+	Rtabmap * _rtabmap;
+	bool _paused;
+	Transform lastPose_;
+	cv::Mat covariance_;
+
+	cv::Mat _userData;
+	UMutex _userDataMutex;
+};
+
+} /* namespace rtabmap */
+#endif /* RTABMAPTHREAD_H_ */

rtabmap/corelib/include/rtabmap/core/SensorCapture.h

@@ -0,0 +1,93 @@
+/*
+Copyright (c) 2010-2022, Mathieu Labbe
+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 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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <opencv2/highgui/highgui.hpp>
+#include <rtabmap/core/SensorCaptureInfo.h>
+#include "rtabmap/core/SensorData.h"
+#include <set>
+#include <stack>
+#include <list>
+#include <vector>
+
+class UDirectory;
+class UTimer;
+
+namespace rtabmap
+{
+
+/**
+ * Class Camera
+ *
+ */
+class RTABMAP_CORE_EXPORT SensorCapture
+{
+public:
+	virtual ~SensorCapture();
+	SensorData takeData(SensorCaptureInfo * info = 0);
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
+	virtual std::string getSerial() const = 0;
+	virtual bool odomProvided() const { return false; }
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06) { return false; }
+
+	//getters
+	float getFrameRate() const {return _frameRate;}
+	const Transform & getLocalTransform() const {return _localTransform;}
+
+	//setters
+	void setFrameRate(float frameRate) {_frameRate = frameRate;}
+	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
+
+	void resetTimer();
+protected:
+	/**
+	 * Constructor
+	 *
+	 * @param frameRate the frame rate (Hz), 0 for fast as the sensor can
+	 * @param localTransform the transform from base frame to sensor frame
+	 */
+	SensorCapture(float frameRate = 0, const Transform & localTransform = Transform::getIdentity());
+
+	/**
+	 * returned rgb and depth images should be already rectified if calibration was loaded
+	 */
+	virtual SensorData captureData(SensorCaptureInfo * info = 0) = 0;
+
+	int getNextSeqID() {return ++_seq;}
+
+private:
+	float _frameRate;
+	Transform _localTransform;
+	UTimer * _frameRateTimer;
+	int _seq;
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/SensorCaptureInfo.h

@@ -0,0 +1,82 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/Transform.h"
+#include <string>
+
+namespace rtabmap
+{
+
+class SensorCaptureInfo
+{
+
+public:
+	SensorCaptureInfo() :
+		cameraName(""),
+		id(0),
+		stamp(0.0),
+		timeCapture(0.0f),
+		timeDeskewing(0.0f),
+		timeDisparity(0.0f),
+		timeMirroring(0.0f),
+		timeStereoExposureCompensation(0.0f),
+		timeImageDecimation(0.0f),
+		timeHistogramEqualization(0.0f),
+		timeScanFromDepth(0.0f),
+		timeUndistortDepth(0.0f),
+		timeBilateralFiltering(0.0f),
+		timeTotal(0.0f),
+		odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
+	{
+	}
+	virtual ~SensorCaptureInfo() {}
+
+	std::string cameraName;
+	int id;
+	double stamp;
+	float timeCapture;
+	float timeDeskewing;
+	float timeDisparity;
+	float timeMirroring;
+	float timeStereoExposureCompensation;
+	float timeImageDecimation;
+	float timeHistogramEqualization;
+	float timeScanFromDepth;
+	float timeUndistortDepth;
+	float timeBilateralFiltering;
+	float timeTotal;
+	Transform odomPose;
+	cv::Mat odomCovariance;
+	std::vector<float> odomVelocity;
+};
+
+//backward compatibility
+RTABMAP_DEPRECATED typedef SensorCaptureInfo CameraInfo;
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/SensorCaptureThread.h

@@ -0,0 +1,216 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsSender.h>
+
+namespace clams
+{
+class DiscreteDepthDistortionModel;
+}
+
+namespace rtabmap
+{
+
+class Camera;
+class Lidar;
+class SensorCapture;
+class SensorCaptureInfo;
+class SensorData;
+class StereoDense;
+class IMUFilter;
+class Feature2D;
+
+/**
+ * Class CameraThread
+ *
+ */
+class RTABMAP_CORE_EXPORT SensorCaptureThread :
+	public UThread,
+	public UEventsSender
+{
+public:
+	// ownership transferred
+	SensorCaptureThread(
+			Camera * camera,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param camera the camera to take images from
+	 * @param odomSensor an odometry sensor to get a pose (can be again the camera)
+	 * @param odomAsGt set odometry sensor pose as ground truth instead of odometry
+	 * @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame  (without optical rotation)
+	 */
+	SensorCaptureThread(
+			Camera * camera,
+			SensorCapture * odomSensor,
+			const Transform & extrinsics,
+			double poseTimeOffset = 0.0,
+			float poseScaleFactor = 1.0f,
+			double poseWaitTime = 0.1,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param lidar the lidar to take scans from
+	 */
+	SensorCaptureThread(
+			Lidar * lidar,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param lidar the lidar to take scans from
+	 * @param camera the camera to take images from. If the camera is providing a pose, it can be used for deskewing
+	 */
+	SensorCaptureThread(
+			Lidar * lidar,
+			Camera * camera,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param lidar the lidar to take scans from
+	 * @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the lidar)
+	 */
+	SensorCaptureThread(
+			Lidar * lidar,
+			SensorCapture * odomSensor,
+			double poseTimeOffset = 0.0,
+			float poseScaleFactor = 1.0f,
+			double poseWaitTime = 0.1,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param lidar the lidar to take scans from
+	 * @param camera the camera to take images from
+	 * @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the camera or lidar)
+	 * @param extrinsics the static transform between odometry frame to camera frame (without optical rotation)
+	 */
+	SensorCaptureThread(
+			Lidar * lidar,
+			Camera * camera,
+			SensorCapture * odomSensor,
+			const Transform & extrinsics,
+			double poseTimeOffset = 0.0,
+			float poseScaleFactor = 1.0f,
+			double poseWaitTime = 0.1,
+			const ParametersMap & parameters = ParametersMap());
+	virtual ~SensorCaptureThread();
+
+	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
+	void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
+	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
+	void setImageDecimation(int decimation) {_imageDecimation = decimation;}
+	void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;}
+	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
+	void setFrameRate(float frameRate);
+	RTABMAP_DEPRECATED void setImageRate(float frameRate) {setFrameRate(frameRate);}
+	void setDistortionModel(const std::string & path);
+	void setOdomAsGroundTruth(bool enabled) {_odomAsGt = enabled;}
+	void enableBilateralFiltering(float sigmaS, float sigmaR);
+	void disableBilateralFiltering() {_bilateralFiltering = false;}
+	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
+	void disableIMUFiltering();
+	void enableFeatureDetection(const ParametersMap & parameters = ParametersMap());
+	void disableFeatureDetection();
+
+	// Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.
+	RTABMAP_DEPRECATED void setScanParameters(
+			bool fromDepth,
+			int downsampleStep, // decimation of the depth image in case the scan is from depth image
+			float rangeMin,
+			float rangeMax,
+			float voxelSize,
+			int normalsK,
+			float normalsRadius,
+			bool forceGroundNormalsUp,
+			bool deskewing);
+	void setScanParameters(
+			bool fromDepth,
+			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
+			float rangeMin=0.0f,
+			float rangeMax=0.0f,
+			float voxelSize = 0.0f,
+			int normalsK = 0,
+			float normalsRadius = 0.0f,
+			float groundNormalsUp = 0.0f,
+			bool deskewing = false);
+
+	void postUpdate(SensorData * data, SensorCaptureInfo * info = 0) const;
+
+	//getters
+	bool isPaused() const {return !this->isRunning();}
+	bool isCapturing() const {return this->isRunning();}
+	bool odomProvided() const;
+
+	Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
+	SensorCapture * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted
+	Lidar * lidar() {return _lidar;} // return null if not set, valid until CameraThread is deleted
+
+private:
+	virtual void mainLoopBegin();
+	virtual void mainLoop();
+	virtual void mainLoopKill();
+
+private:
+	Camera * _camera;
+	SensorCapture * _odomSensor;
+	Lidar * _lidar;
+	Transform _extrinsicsOdomToCamera;
+	bool _odomAsGt;
+	double _poseTimeOffset;
+	float _poseScaleFactor;
+	double _poseWaitTime;
+	bool _mirroring;
+	bool _stereoExposureCompensation;
+	bool _colorOnly;
+	int _imageDecimation;
+	int _histogramMethod;
+	bool _stereoToDepth;
+	bool _scanDeskewing;
+	bool _scanFromDepth;
+	int _scanDownsampleStep;
+	float _scanRangeMin;
+	float _scanRangeMax;
+	float _scanVoxelSize;
+	int _scanNormalsK;
+	float _scanNormalsRadius;
+	float _scanForceGroundNormalsUp;
+	StereoDense * _stereoDense;
+	clams::DiscreteDepthDistortionModel * _distortionModel;
+	bool _bilateralFiltering;
+	float _bilateralSigmaS;
+	float _bilateralSigmaR;
+	IMUFilter * _imuFilter;
+	bool _imuBaseFrameConversion;
+	Feature2D * _featureDetector;
+	bool _depthAsMask;
+};
+
+//backward compatibility
+RTABMAP_DEPRECATED typedef SensorCaptureThread CameraThread;
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/SensorData.h

@@ -0,0 +1,440 @@
+/*
+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.
+*/
+
+#ifndef SENSORDATA_H_
+#define SENSORDATA_H_
+
+#include <rtabmap/core/rtabmap_core_export.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/StereoCameraModel.h>
+#include <rtabmap/core/Transform.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <rtabmap/core/LaserScan.h>
+#include <rtabmap/core/IMU.h>
+#include <rtabmap/core/GPS.h>
+#include <rtabmap/core/EnvSensor.h>
+#include <rtabmap/core/Landmark.h>
+#include <rtabmap/core/GlobalDescriptor.h>
+
+namespace rtabmap
+{
+
+/**
+ * An id is automatically generated if id=0.
+ */
+class RTABMAP_CORE_EXPORT SensorData
+{
+public:
+	// empty constructor
+	SensorData();
+
+	// Appearance-only constructor
+	SensorData(
+			const cv::Mat & image,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Mono constructor
+	SensorData(
+			const cv::Mat & image,
+			const CameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// RGB-D constructor
+	SensorData(
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const CameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// RGB-D constructor + depth confidence
+	SensorData(
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		const cv::Mat & depth_confidence,
+		const CameraModel & cameraModel,
+		int id = 0,
+		double stamp = 0.0,
+		const cv::Mat & userData = cv::Mat());
+
+	// RGB-D constructor + laser scan
+	SensorData(
+			const LaserScan & laserScan,
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const CameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+	
+	// RGB-D constructor + confidence + laser scan
+	SensorData(
+			const LaserScan & laserScan,
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const cv::Mat & depthConfidence,
+			const CameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Multi-cameras RGB-D constructor
+	SensorData(
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const std::vector<CameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+	
+	// Multi-cameras RGB-D constructor + depth confidence
+	SensorData(
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const cv::Mat & depthConfidence,
+			const std::vector<CameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Multi-cameras RGB-D constructor + laser scan
+	SensorData(
+			const LaserScan & laserScan,
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const std::vector<CameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Multi-cameras RGB-D constructor + depth confidence + laser scan
+	SensorData(
+			const LaserScan & laserScan,
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const cv::Mat & depthConfidence,
+			const std::vector<CameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Stereo constructor
+	SensorData(
+			const cv::Mat & left,
+			const cv::Mat & right,
+			const StereoCameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Stereo constructor + laser scan
+	SensorData(
+			const LaserScan & laserScan,
+			const cv::Mat & left,
+			const cv::Mat & right,
+			const StereoCameraModel & cameraModel,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Multi-cameras stereo constructor
+	SensorData(
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const std::vector<StereoCameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Multi-cameras stereo constructor + laser scan
+	SensorData(
+			const LaserScan & laserScan,
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const std::vector<StereoCameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// IMU constructor
+	SensorData(
+			const IMU & imu,
+			int id = 0,
+			double stamp = 0.0);
+
+	virtual ~SensorData();
+
+	bool isValid() const {
+		return !(_id == 0 &&
+			_stamp == 0.0 &&
+			_imageRaw.empty() &&
+			_imageCompressed.empty() &&
+			_depthOrRightRaw.empty() &&
+			_depthOrRightCompressed.empty() &&
+			_depthConfidenceRaw.empty() &&
+			_depthConfidenceCompressed.empty() &&
+			_laserScanRaw.isEmpty() &&
+			_laserScanCompressed.isEmpty() &&
+			_cameraModels.empty() &&
+			_stereoCameraModels.empty() &&
+			_userDataRaw.empty() &&
+			_userDataCompressed.empty() &&
+			_keypoints.size() == 0 &&
+			_descriptors.empty() &&
+			imu_.empty());
+	}
+
+	int id() const {return _id;}
+	void setId(int id) {_id = id;}
+	double stamp() const {return _stamp;}
+	void setStamp(double stamp) {_stamp = stamp;}
+
+	const cv::Mat & imageCompressed() const {return _imageCompressed;}
+	const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
+	const cv::Mat & depthConfidenceCompressed() const {return _depthConfidenceCompressed;}
+	const LaserScan & laserScanCompressed() const {return _laserScanCompressed;}
+
+	const cv::Mat & imageRaw() const {return _imageRaw;}
+	const cv::Mat & depthOrRightRaw() const {return _depthOrRightRaw;}
+	const cv::Mat & depthConfidenceRaw() const {return _depthConfidenceRaw;}
+	const LaserScan & laserScanRaw() const {return _laserScanRaw;}
+
+	/**
+	 * Set image data. Detect automatically if raw or compressed.
+	 * A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * @param clearPreviousData, clear previous raw and compressed images before setting the new ones.
+	 */
+	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const CameraModel & model, bool clearPreviousData = true);
+	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const cv::Mat & depth_confidence, const CameraModel & model, bool clearPreviousData = true);
+	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const std::vector<CameraModel> & models, bool clearPreviousData = true);
+	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const cv::Mat & depth_confidence, const std::vector<CameraModel> & models, bool clearPreviousData = true);
+	void setStereoImage(const cv::Mat & left, const cv::Mat & right, const StereoCameraModel & stereoCameraModel, bool clearPreviousData = true);
+	void setStereoImage(const cv::Mat & left, const cv::Mat & right, const std::vector<StereoCameraModel> & stereoCameraModels, bool clearPreviousData = true);
+
+	/**
+	 * Set laser scan data. Detect automatically if raw or compressed.
+	 * A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * @param clearPreviousData, clear previous raw and compressed scans before setting the new one.
+	 */
+	void setLaserScan(const LaserScan & laserScan, bool clearPreviousData = true);
+
+	void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
+	void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
+	void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModels.clear(); _stereoCameraModels.push_back(stereoCameraModel);}
+	void setStereoCameraModels(const std::vector<StereoCameraModel> & stereoCameraModels) {_stereoCameraModels = stereoCameraModels;}
+
+	//for convenience
+	cv::Mat depthRaw() const {return !(_depthOrRightRaw.type()==CV_8UC1 || _depthOrRightRaw.type()==CV_8UC3) ? _depthOrRightRaw : cv::Mat();}
+	cv::Mat rightRaw() const {return   _depthOrRightRaw.type()==CV_8UC1 || _depthOrRightRaw.type()==CV_8UC3  ? _depthOrRightRaw : cv::Mat();}
+
+	// Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.
+	RTABMAP_DEPRECATED void setImageRaw(const cv::Mat & image);
+	// Use setRGBDImage() or setStereoImage() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.
+	RTABMAP_DEPRECATED void setDepthOrRightRaw(const cv::Mat & image);
+	// Use setLaserScan() with clearNotUpdated=false or removeRawData() instead. To be backward compatible, this function doesn't clear compressed data.
+	RTABMAP_DEPRECATED void setLaserScanRaw(const LaserScan & scan);
+	// Use setUserData() or removeRawData() instead.
+	RTABMAP_DEPRECATED void setUserDataRaw(const cv::Mat & data);
+
+	void uncompressData();
+	void uncompressData(
+			cv::Mat * imageRaw,
+			cv::Mat * depthOrRightRaw,
+			LaserScan * laserScanRaw = 0,
+			cv::Mat * userDataRaw = 0,
+			cv::Mat * groundCellsRaw = 0,
+			cv::Mat * obstacleCellsRaw = 0,
+			cv::Mat * emptyCellsRaw = 0,
+			cv::Mat * depthConfidenceRaw = 0);
+	void uncompressDataConst(
+			cv::Mat * imageRaw,
+			cv::Mat * depthOrRightRaw,
+			LaserScan * laserScanRaw = 0,
+			cv::Mat * userDataRaw = 0,
+			cv::Mat * groundCellsRaw = 0,
+			cv::Mat * obstacleCellsRaw = 0,
+			cv::Mat * emptyCellsRaw = 0,
+			cv::Mat * depthConfidenceRaw = 0) const;
+
+	const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
+	const std::vector<StereoCameraModel> & stereoCameraModels() const {return _stereoCameraModels;}
+
+	/**
+	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
+	 * compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * If you have one dimension unsigned 8 bits raw data, make sure to transpose it
+	 * (to have multiple rows instead of multiple columns) in order to be detected as
+	 * not compressed.
+	 * @param clearPreviousData, clear previous raw and compressed user data before setting the new one.
+	 */
+	void setUserData(const cv::Mat & userData, bool clearPreviousData = true);
+	const cv::Mat & userDataRaw() const {return _userDataRaw;}
+	const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
+
+	// detect automatically if raw or compressed. If raw, the data will be compressed.
+	void setOccupancyGrid(
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Mat & empty,
+			float cellSize,
+			const cv::Point3f & viewPoint);
+	// remove raw occupancy grids
+	void clearOccupancyGridRaw() {_groundCellsRaw = cv::Mat(); _obstacleCellsRaw = cv::Mat();}
+	const cv::Mat & gridGroundCellsRaw() const {return _groundCellsRaw;}
+	const cv::Mat & gridGroundCellsCompressed() const {return _groundCellsCompressed;}
+	const cv::Mat & gridObstacleCellsRaw() const {return _obstacleCellsRaw;}
+	const cv::Mat & gridObstacleCellsCompressed() const {return _obstacleCellsCompressed;}
+	const cv::Mat & gridEmptyCellsRaw() const {return _emptyCellsRaw;}
+	const cv::Mat & gridEmptyCellsCompressed() const {return _emptyCellsCompressed;}
+	float gridCellSize() const {return _cellSize;}
+	const cv::Point3f & gridViewPoint() const {return _viewPoint;}
+
+	void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & keypoints3D, const cv::Mat & descriptors);
+	const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
+	const std::vector<cv::Point3f> & keypoints3D() const {return _keypoints3D;}
+	const cv::Mat & descriptors() const {return _descriptors;}
+
+	void addGlobalDescriptor(const GlobalDescriptor & descriptor) {_globalDescriptors.push_back(descriptor);}
+	void setGlobalDescriptors(const std::vector<GlobalDescriptor> & descriptors) {_globalDescriptors = descriptors;}
+	void clearGlobalDescriptors() {_globalDescriptors.clear();}
+	const std::vector<GlobalDescriptor> & globalDescriptors() const {return _globalDescriptors;}
+
+	void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
+	const Transform & groundTruth() const {return groundTruth_;}
+
+	void setGlobalPose(const Transform & pose, const cv::Mat & covariance) {globalPose_ = pose; globalPoseCovariance_ = covariance;}
+	const Transform & globalPose() const {return globalPose_;}
+	const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
+
+	void setGPS(const GPS & gps) {gps_ = gps;}
+	const GPS & gps() const {return gps_;}
+
+	void setIMU(const IMU & imu) {imu_ = imu; }
+	const IMU & imu() const {return imu_;}
+
+	void setEnvSensors(const EnvSensors & sensors) {_envSensors = sensors;}
+	void addEnvSensor(const EnvSensor & sensor) {_envSensors.insert(std::make_pair(sensor.type(), sensor));}
+	const EnvSensors & envSensors() const {return _envSensors;}
+
+	void setLandmarks(const Landmarks & landmarks) {_landmarks = landmarks;}
+	const Landmarks & landmarks() const {return _landmarks;}
+
+	unsigned long getMemoryUsed() const; // Return memory usage in Bytes
+	/**
+	 * Clear compressed rgb/depth (left/right) images, compressed laser scan and compressed user data.
+	 * Raw data are kept is set.
+	 */
+	void clearCompressedData(bool images = true, bool scan = true, bool userData = true);
+	/**
+	 * Clear raw rgb/depth (left/right) images, raw laser scan and raw user data.
+	 * Compressed data are kept is set.
+	 */
+	void clearRawData(bool images = true, bool scan = true, bool userData = true);
+
+	bool isPointVisibleFromCameras(const cv::Point3f & pt) const; // assuming point is in robot frame
+
+#ifdef HAVE_OPENCV_CUDEV
+	const cv::cuda::GpuMat & imageRawGpu() const {return _imageRawGpu;}
+	void setImageRawGpu(const cv::cuda::GpuMat & image) {_imageRawGpu = image;}
+	const cv::cuda::GpuMat & depthOrRightRawGpu() const {return _depthOrRightRawGpu;}
+	void setDepthOrRightRawGpu(const cv::cuda::GpuMat & image) {_depthOrRightRawGpu = image;}
+#endif
+
+private:
+	int _id;
+	double _stamp;
+
+	cv::Mat _imageCompressed;          // compressed image
+	cv::Mat _depthOrRightCompressed;   // compressed image
+	cv::Mat _depthConfidenceCompressed;   // compressed data
+	LaserScan _laserScanCompressed;      // compressed data
+
+	cv::Mat _imageRaw;          // CV_8UC1 or CV_8UC3
+	cv::Mat _depthOrRightRaw;   // depth CV_16UC1 or CV_32FC1, right image CV_8UC1 or CV_8UC3
+	cv::Mat _depthConfidenceRaw;   // CV_8UC1
+	LaserScan _laserScanRaw;
+
+	std::vector<CameraModel> _cameraModels;
+	std::vector<StereoCameraModel> _stereoCameraModels;
+
+	// user data
+	cv::Mat _userDataCompressed;      // compressed data
+	cv::Mat _userDataRaw;
+
+	// occupancy grid
+	cv::Mat _groundCellsCompressed;
+	cv::Mat _obstacleCellsCompressed;
+	cv::Mat _emptyCellsCompressed;
+	cv::Mat _groundCellsRaw;
+	cv::Mat _obstacleCellsRaw;
+	cv::Mat _emptyCellsRaw;
+	float _cellSize;
+	cv::Point3f _viewPoint;
+
+	// environmental sensors
+	EnvSensors _envSensors;
+
+	// landmarks
+	Landmarks _landmarks;
+
+	// features
+	std::vector<cv::KeyPoint> _keypoints;
+	std::vector<cv::Point3f> _keypoints3D;
+	cv::Mat _descriptors;
+
+	// global descriptors
+	std::vector<GlobalDescriptor> _globalDescriptors;
+
+	Transform groundTruth_;
+
+	Transform globalPose_;
+	cv::Mat globalPoseCovariance_; // 6x6 double
+
+	GPS gps_;
+
+	IMU imu_;
+
+#ifdef HAVE_OPENCV_CUDEV
+	// Temporary buffers used for some optimizations,
+	// particulary to avoid host<->device copies if same 
+	// data are re-used
+	cv::cuda::GpuMat _imageRawGpu;
+	cv::cuda::GpuMat _depthOrRightRawGpu;
+#endif
+};
+
+}
+
+
+#endif /* SENSORDATA_H_ */

rtabmap/corelib/include/rtabmap/core/SensorEvent.h

@@ -0,0 +1,94 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include <rtabmap/core/SensorCaptureInfo.h>
+#include <rtabmap/utilite/UEvent.h>
+#include "rtabmap/core/SensorData.h"
+
+namespace rtabmap
+{
+
+class SensorEvent :
+	public UEvent
+{
+public:
+	enum Code {
+		kCodeData,
+		kCodeNoMoreImages
+	};
+
+public:
+	SensorEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) :
+		UEvent(kCodeData),
+		data_(image, seq, stamp)
+	{
+		sensorCaptureInfo_.cameraName = cameraName;
+	}
+
+	SensorEvent() :
+		UEvent(kCodeNoMoreImages)
+	{
+	}
+
+	SensorEvent(const SensorData & data) :
+		UEvent(kCodeData),
+		data_(data)
+	{
+	}
+
+	SensorEvent(const SensorData & data, const std::string & cameraName) :
+		UEvent(kCodeData),
+		data_(data)
+	{
+		sensorCaptureInfo_.cameraName = cameraName;
+	}
+	SensorEvent(const SensorData & data, const SensorCaptureInfo & sensorCaptureInfo) :
+		UEvent(kCodeData),
+		data_(data),
+		sensorCaptureInfo_(sensorCaptureInfo)
+	{
+	}
+
+	// Image or descriptors
+	const SensorData & data() const {return data_;}
+	const std::string & cameraName() const {return sensorCaptureInfo_.cameraName;}
+	const SensorCaptureInfo & info() const {return sensorCaptureInfo_;}
+
+	virtual ~SensorEvent() {}
+	virtual std::string getClassName() const {return std::string("SensorEvent");}
+
+private:
+	SensorData data_;
+	SensorCaptureInfo sensorCaptureInfo_;
+};
+
+//backward compatibility
+RTABMAP_DEPRECATED typedef SensorEvent CameraEvent;
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/Signature.h

@@ -0,0 +1,173 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <pcl/point_types.h>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <map>
+#include <list>
+#include <vector>
+#include <set>
+
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Link.h>
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT Signature
+{
+
+public:
+	Signature();
+	Signature(int id,
+			int mapId = -1,
+			int weight = 0,
+			double stamp = 0.0,
+			const std::string & label = std::string(),
+			const Transform & pose = Transform(),
+			const Transform & groundTruthPose = Transform(),
+			const SensorData & sensorData = SensorData());
+	Signature(const SensorData & data);
+	virtual ~Signature();
+
+	/**
+	 * Must return a value between >=0 and <=1 (1 means 100% similarity).
+	 */
+	float compareTo(const Signature & signature) const;
+	bool isBadSignature() const;
+
+	int id() const {return _id;}
+	int mapId() const {return _mapId;}
+
+	void setWeight(int weight) {_modified=_weight!=weight;_weight = weight;}
+	int getWeight() const {return _weight;}
+
+	void setLabel(const std::string & label) {_modified=_label.compare(label)!=0;_label = label;}
+	const std::string & getLabel() const {return _label;}
+
+	double getStamp() const {return _stamp;}
+
+	void addLinks(const std::list<Link> & links);
+	void addLinks(const std::map<int, Link> & links);
+	void addLink(const Link & link);
+
+	bool hasLink(int idTo, Link::Type type = Link::kUndef) const;
+
+	void changeLinkIds(int idFrom, int idTo);
+
+	void removeLinks(bool keepSelfReferringLinks = false);
+	void removeLink(int idTo);
+	void removeVirtualLinks();
+
+	void addLandmark(const Link & landmark);
+	const std::map<int, Link> & getLandmarks() const {return _landmarks;}
+	void removeLandmarks();
+	void removeLandmark(int landmarkId);
+
+	void setSaved(bool saved) {_saved = saved;}
+	void setModified(bool modified) {_modified = modified; _linksModified = modified;}
+
+	const std::multimap<int, Link> & getLinks() const {return _links;}
+	bool isSaved() const {return _saved;}
+	bool isModified() const {return _modified || _linksModified;}
+	bool isLinksModified() const {return _linksModified;}
+
+	//visual words stuff
+	void removeAllWords();
+	void changeWordsRef(int oldWordId, int activeWordId);
+	void setWords(const std::multimap<int, int> & words, const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & words3, const cv::Mat & descriptors);
+	bool isEnabled() const {return _enabled;}
+	void setEnabled(bool enabled) {_enabled = enabled;}
+	const std::multimap<int, int> & getWords() const {return _words;}
+	const std::vector<cv::KeyPoint> & getWordsKpts() const {return _wordsKpts;}
+	int getInvalidWordsCount() const {return _invalidWordsCount;}
+	const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
+	const cv::Mat & getWordsDescriptors() const {return _wordsDescriptors;}
+	void setWordsDescriptors(const cv::Mat & descriptors);
+
+	//metric stuff
+	void setPose(const Transform & pose) {_pose = pose;}
+	void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
+	void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw) {
+		_velocity = std::vector<float>(6,0);
+		_velocity[0]=vx;
+		_velocity[1]=vy;
+		_velocity[2]=vz;
+		_velocity[3]=vroll;
+		_velocity[4]=vpitch;
+		_velocity[5]=vyaw;
+	}
+
+	const std::vector<cv::Point3f> & getWords3() const {return _words3;}
+	const Transform & getPose() const {return _pose;}
+	cv::Mat getPoseCovariance() const;
+	const Transform & getGroundTruthPose() const {return _groundTruthPose;}
+	const std::vector<float> & getVelocity() const {return _velocity;}
+
+	SensorData & sensorData() {return _sensorData;}
+	const SensorData & sensorData() const {return _sensorData;}
+
+	unsigned long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes
+
+private:
+	int _id;
+	int _mapId;
+	double _stamp;
+	std::multimap<int, Link> _links; // id, transform
+	std::map<int, Link> _landmarks;
+	int _weight;
+	std::string _label;
+	bool _saved; // If it's saved to bd
+	bool _modified;
+	bool _linksModified; // Optimization when updating signatures in database
+
+	// Contains all words (Some can be duplicates -> if a word appears 2
+	// times in the signature, it will be 2 times in this list)
+	// Words match with the CvSeq keypoints and descriptors
+	std::multimap<int, int> _words; // word <id, keypoint index>
+	std::vector<cv::KeyPoint> _wordsKpts;
+	std::vector<cv::Point3f> _words3; // in base_link frame (localTransform applied))
+	cv::Mat _wordsDescriptors;
+	std::map<int, int> _wordsChanged; // <oldId, newId>
+	bool _enabled;
+	int _invalidWordsCount;
+
+	Transform _pose;
+	Transform _groundTruthPose;
+	std::vector<float> _velocity;
+
+	SensorData _sensorData;
+};
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/Statistics.h

@@ -0,0 +1,342 @@
+/*
+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.
+*/
+
+#ifndef STATISTICS_H_
+#define STATISTICS_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <list>
+#include <vector>
+#include <rtabmap/core/Signature.h>
+#include <rtabmap/core/Link.h>
+
+namespace rtabmap {
+
+#define RTABMAP_STATS(PREFIX, NAME, UNIT) \
+	public: \
+		static std::string k##PREFIX##NAME() {return #PREFIX "/" #NAME "/" #UNIT;} \
+	private: \
+		class Dummy##PREFIX##NAME { \
+		public: \
+			Dummy##PREFIX##NAME() {if(!_defaultDataInitialized)_defaultData.insert(std::pair<std::string, float>(#PREFIX "/" #NAME "/" #UNIT, 0.0f));} \
+		}; \
+		Dummy##PREFIX##NAME dummy##PREFIX##NAME
+
+class RTABMAP_CORE_EXPORT Statistics
+{
+	RTABMAP_STATS(Loop, Id,); // Combined loop or proximity detection
+	RTABMAP_STATS(Loop, RejectedHypothesis,);
+	RTABMAP_STATS(Loop, Accepted_hypothesis_id,);
+	RTABMAP_STATS(Loop, Suppressed_hypothesis_id,);
+	RTABMAP_STATS(Loop, Highest_hypothesis_id,);
+	RTABMAP_STATS(Loop, Highest_hypothesis_value,);
+	RTABMAP_STATS(Loop, Vp_hypothesis,);
+	RTABMAP_STATS(Loop, Reactivate_id,);
+	RTABMAP_STATS(Loop, Hypothesis_ratio,);
+	RTABMAP_STATS(Loop, Hypothesis_reactivated,);
+	RTABMAP_STATS(Loop, Map_id,);
+	RTABMAP_STATS(Loop, Visual_words,);
+	RTABMAP_STATS(Loop, Visual_inliers,);
+	RTABMAP_STATS(Loop, Visual_inliers_ratio,);
+	RTABMAP_STATS(Loop, Visual_matches,);
+	RTABMAP_STATS(Loop, Distance_since_last_loc, m);
+	RTABMAP_STATS(Loop, Last_id,);
+	RTABMAP_STATS(Loop, Optimization_max_error, m);
+	RTABMAP_STATS(Loop, Optimization_max_error_ratio, );
+	RTABMAP_STATS(Loop, Optimization_max_ang_error, deg);
+	RTABMAP_STATS(Loop, Optimization_max_ang_error_ratio, );
+	RTABMAP_STATS(Loop, Optimization_error, );
+	RTABMAP_STATS(Loop, Optimization_iterations, );
+	RTABMAP_STATS(Loop, Linear_variance,);
+	RTABMAP_STATS(Loop, Angular_variance,);
+	RTABMAP_STATS(Loop, Landmark_detected,);
+	RTABMAP_STATS(Loop, Landmark_detected_node_ref,);
+	RTABMAP_STATS(Loop, Visual_inliers_mean_dist,m);
+	RTABMAP_STATS(Loop, Visual_inliers_distribution,);
+	RTABMAP_STATS(Loop, Proximity_links_cleared,);
+	//Odom correction
+	RTABMAP_STATS(Loop, Odom_correction_norm, m);
+	RTABMAP_STATS(Loop, Odom_correction_angle, deg);
+	RTABMAP_STATS(Loop, Odom_correction_x, m);
+	RTABMAP_STATS(Loop, Odom_correction_y, m);
+	RTABMAP_STATS(Loop, Odom_correction_z, m);
+	RTABMAP_STATS(Loop, Odom_correction_roll, deg);
+	RTABMAP_STATS(Loop, Odom_correction_pitch, deg);
+	RTABMAP_STATS(Loop, Odom_correction_yaw, deg);
+	// Map to Odom
+	RTABMAP_STATS(Loop, MapToOdom_norm, m);
+	RTABMAP_STATS(Loop, MapToOdom_angle, deg);
+	RTABMAP_STATS(Loop, MapToOdom_x, m);
+	RTABMAP_STATS(Loop, MapToOdom_y, m);
+	RTABMAP_STATS(Loop, MapToOdom_z, m);
+	RTABMAP_STATS(Loop, MapToOdom_roll, deg);
+	RTABMAP_STATS(Loop, MapToOdom_pitch, deg);
+	RTABMAP_STATS(Loop, MapToOdom_yaw, deg);
+	// Map to Base
+	RTABMAP_STATS(Loop, MapToBase_x, m);
+	RTABMAP_STATS(Loop, MapToBase_y, m);
+	RTABMAP_STATS(Loop, MapToBase_z, m);
+	RTABMAP_STATS(Loop, MapToBase_roll, deg);
+	RTABMAP_STATS(Loop, MapToBase_pitch, deg);
+	RTABMAP_STATS(Loop, MapToBase_yaw, deg);
+	RTABMAP_STATS(Loop, MapToBase_lin_std, m);
+	RTABMAP_STATS(Loop, MapToBase_lin_var, m2);
+
+	RTABMAP_STATS(Proximity, Time_detections,);
+	RTABMAP_STATS(Proximity, Space_last_detection_id,);
+	RTABMAP_STATS(Proximity, Space_paths,);
+	RTABMAP_STATS(Proximity, Space_visual_paths_checked,);
+	RTABMAP_STATS(Proximity, Space_scan_paths_checked,);
+	RTABMAP_STATS(Proximity, Space_detections_added_visually,);
+	RTABMAP_STATS(Proximity, Space_detections_added_icp_multi,);
+	RTABMAP_STATS(Proximity, Space_detections_added_icp_global,);
+
+	RTABMAP_STATS(NeighborLinkRefining, Accepted,);
+	RTABMAP_STATS(NeighborLinkRefining, Inliers,);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_inliers_ratio,);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_rotation, rad);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_translation, m);
+	RTABMAP_STATS(NeighborLinkRefining, ICP_complexity,);
+	RTABMAP_STATS(NeighborLinkRefining, Variance,);
+	RTABMAP_STATS(NeighborLinkRefining, Pts,);
+
+	RTABMAP_STATS(Memory, Working_memory_size,);
+	RTABMAP_STATS(Memory, Short_time_memory_size,);
+	RTABMAP_STATS(Memory, Database_memory_used, MB);
+	RTABMAP_STATS(Memory, Signatures_removed,);
+	RTABMAP_STATS(Memory, Immunized_globally,);
+	RTABMAP_STATS(Memory, Immunized_locally,);
+	RTABMAP_STATS(Memory, Immunized_locally_max,);
+	RTABMAP_STATS(Memory, Signatures_retrieved,);
+	RTABMAP_STATS(Memory, Images_buffered,);
+	RTABMAP_STATS(Memory, Rehearsal_sim,);
+	RTABMAP_STATS(Memory, Rehearsal_id,);
+	RTABMAP_STATS(Memory, Rehearsal_merged,);
+	RTABMAP_STATS(Memory, Local_graph_size,);
+	RTABMAP_STATS(Memory, Odom_cache_poses,);
+	RTABMAP_STATS(Memory, Odom_cache_links,);
+	RTABMAP_STATS(Memory, Small_movement,);
+	RTABMAP_STATS(Memory, Fast_movement,);
+	RTABMAP_STATS(Memory, New_landmark,);
+	RTABMAP_STATS(Memory, Odometry_variance_ang,);
+	RTABMAP_STATS(Memory, Odometry_variance_lin,);
+	RTABMAP_STATS(Memory, Distance_travelled, m);
+	RTABMAP_STATS(Memory, RAM_usage, MB);
+	RTABMAP_STATS(Memory, RAM_estimated, MB);
+	RTABMAP_STATS(Memory, Triangulated_points, );
+	RTABMAP_STATS(Memory, Closest_node_distance, m);
+	RTABMAP_STATS(Memory, Closest_node_angle, rad);
+
+	RTABMAP_STATS(Timing, Memory_update, ms);
+	RTABMAP_STATS(Timing, Neighbor_link_refining, ms);
+	RTABMAP_STATS(Timing, Proximity_by_time, ms);
+	RTABMAP_STATS(Timing, Proximity_by_space_search, ms);
+	RTABMAP_STATS(Timing, Proximity_by_space_visual, ms);
+	RTABMAP_STATS(Timing, Proximity_by_space, ms);
+	RTABMAP_STATS(Timing, Cleaning_neighbors, ms);
+	RTABMAP_STATS(Timing, Reactivation, ms);
+	RTABMAP_STATS(Timing, Add_loop_closure_link, ms);
+	RTABMAP_STATS(Timing, Map_optimization, ms);
+	RTABMAP_STATS(Timing, Likelihood_computation, ms);
+	RTABMAP_STATS(Timing, Posterior_computation, ms);
+	RTABMAP_STATS(Timing, Hypotheses_creation, ms);
+	RTABMAP_STATS(Timing, Hypotheses_validation, ms);
+	RTABMAP_STATS(Timing, Statistics_creation, ms);
+	RTABMAP_STATS(Timing, Memory_cleanup, ms);
+	RTABMAP_STATS(Timing, Total, ms);
+	RTABMAP_STATS(Timing, Forgetting, ms);
+	RTABMAP_STATS(Timing, Joining_trash, ms);
+	RTABMAP_STATS(Timing, Emptying_trash, ms);
+	RTABMAP_STATS(Timing, Finalizing_statistics, ms);
+	RTABMAP_STATS(Timing, RAM_estimation, ms);
+
+	RTABMAP_STATS(TimingMem, Pre_update, ms);
+	RTABMAP_STATS(TimingMem, Signature_creation, ms);
+	RTABMAP_STATS(TimingMem, Rehearsal, ms);
+	RTABMAP_STATS(TimingMem, Keypoints_detection, ms);
+	RTABMAP_STATS(TimingMem, Subpixel, ms);
+	RTABMAP_STATS(TimingMem, Stereo_correspondences, ms);
+	RTABMAP_STATS(TimingMem, Descriptors_extraction, ms);
+	RTABMAP_STATS(TimingMem, Rectification, ms);
+	RTABMAP_STATS(TimingMem, Keypoints_3D, ms);
+	RTABMAP_STATS(TimingMem, Keypoints_3D_motion, ms);
+	RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);
+	RTABMAP_STATS(TimingMem, Add_new_words, ms);
+	RTABMAP_STATS(TimingMem, Compressing_data, ms);
+	RTABMAP_STATS(TimingMem, Post_decimation, ms);
+	RTABMAP_STATS(TimingMem, Scan_filtering, ms);
+	RTABMAP_STATS(TimingMem, Occupancy_grid, ms);
+	RTABMAP_STATS(TimingMem, Markers_detection, ms);
+
+	RTABMAP_STATS(Keypoint, Dictionary_size, words);
+	RTABMAP_STATS(Keypoint, Current_frame, words);
+	RTABMAP_STATS(Keypoint, Indexed_words, words);
+	RTABMAP_STATS(Keypoint, Index_memory_usage, KB);
+
+	RTABMAP_STATS(Gt, Translational_rmse, m);
+	RTABMAP_STATS(Gt, Translational_mean, m);
+	RTABMAP_STATS(Gt, Translational_median, m);
+	RTABMAP_STATS(Gt, Translational_std, m);
+	RTABMAP_STATS(Gt, Translational_min, m);
+	RTABMAP_STATS(Gt, Translational_max, m);
+	RTABMAP_STATS(Gt, Rotational_rmse, deg);
+	RTABMAP_STATS(Gt, Rotational_mean, deg);
+	RTABMAP_STATS(Gt, Rotational_median, deg);
+	RTABMAP_STATS(Gt, Rotational_std, deg);
+	RTABMAP_STATS(Gt, Rotational_min, deg);
+	RTABMAP_STATS(Gt, Rotational_max, deg);
+	RTABMAP_STATS(Gt, Localization_linear_error, m);
+	RTABMAP_STATS(Gt, Localization_angular_error, deg);
+
+public:
+	static const std::map<std::string, float> & defaultData();
+	static std::string serializeData(const std::map<std::string, float> & data);
+	static std::map<std::string, float> deserializeData(const std::string & data);
+
+public:
+	Statistics();
+	virtual ~Statistics();
+
+	// name format = "Grp/Name/unit"
+	void addStatistic(const std::string & name, float value);
+
+	// setters
+	void setExtended(bool extended) {_extended = extended;}
+	void setRefImageId(int id) {_refImageId = id;}
+	void setRefImageMapId(int id) {_refImageMapId = id;}
+	void setLoopClosureId(int id) {_loopClosureId = id;}
+	void setLoopClosureMapId(int id) {_loopClosureMapId = id;}
+	void setProximityDetectionId(int id) {_proximiyDetectionId = id;}
+	void setProximityDetectionMapId(int id) {_proximiyDetectionMapId = id;}
+	void setStamp(double stamp) {_stamp = stamp;}
+
+	// Use addSignatureData() instead.
+	RTABMAP_DEPRECATED void setLastSignatureData(const Signature & data);
+	void addSignatureData(const Signature & data) {_signaturesData.insert(std::make_pair(data.id(), data));}
+	void setSignaturesData(const std::map<int, Signature> & data) {_signaturesData = data;}
+
+	void setPoses(const std::map<int, Transform> & poses) {_poses = poses;}
+	void setConstraints(const std::multimap<int, Link> & constraints) {_constraints = constraints;}
+	void setMapCorrection(const Transform & mapCorrection) {_mapCorrection = mapCorrection;}
+	void setLoopClosureTransform(const Transform & loopClosureTransform) {_loopClosureTransform = loopClosureTransform;}
+	void setLocalizationCovariance(const cv::Mat & covariance) {_localizationCovariance = covariance;}
+	void setLabels(const std::map<int, std::string> & labels) {_labels = labels;}
+	void setWeights(const std::map<int, int> & weights) {_weights = weights;}
+	void setPosterior(const std::map<int, float> & posterior) {_posterior = posterior;}
+	void setLikelihood(const std::map<int, float> & likelihood) {_likelihood = likelihood;}
+	void setRawLikelihood(const std::map<int, float> & rawLikelihood) {_rawLikelihood = rawLikelihood;}
+	void setLocalPath(const std::vector<int> & localPath) {_localPath=localPath;}
+	void setCurrentGoalId(int goal) {_currentGoalId=goal;}
+	void setReducedIds(const std::map<int, int> & reducedIds) {_reducedIds = reducedIds;}
+	void setWmState(const std::vector<int> & state) {_wmState = state;}
+	void setOdomCachePoses(const std::map<int, Transform> & poses) {_odomCachePoses = poses;}
+	void setOdomCacheConstraints(const std::multimap<int, Link> & constraints) {_odomCacheConstraints = constraints;}
+
+	// getters
+	bool extended() const {return _extended;}
+	int refImageId() const {return _refImageId;}
+	int refImageMapId() const {return _refImageMapId;}
+	int loopClosureId() const {return _loopClosureId;}
+	int loopClosureMapId() const {return _loopClosureMapId;}
+	int proximityDetectionId() const {return _proximiyDetectionId;}
+	int proximityDetectionMapId() const {return _proximiyDetectionMapId;}
+	double stamp() const {return _stamp;}
+
+	const Signature & getLastSignatureData() const {return _signaturesData.empty()?_dummyEmptyData:_signaturesData.rbegin()->second;}
+	const std::map<int, Signature> & getSignaturesData() const {return _signaturesData;}
+
+	const std::map<int, Transform> & poses() const {return _poses;}
+	const std::multimap<int, Link> & constraints() const {return _constraints;}
+	const Transform & mapCorrection() const {return _mapCorrection;}
+	const Transform & loopClosureTransform() const {return _loopClosureTransform;}
+	const cv::Mat & localizationCovariance() const {return _localizationCovariance;}
+	const std::map<int, std::string> & labels() const {return _labels;}
+	const std::map<int, int> & weights() const {return _weights;}
+	const std::map<int, float> & posterior() const {return _posterior;}
+	const std::map<int, float> & likelihood() const {return _likelihood;}
+	const std::map<int, float> & rawLikelihood() const {return _rawLikelihood;}
+	const std::vector<int> & localPath() const {return _localPath;}
+	int currentGoalId() const {return _currentGoalId;}
+	const std::map<int, int> & reducedIds() const {return _reducedIds;}
+	const std::vector<int> & wmState() const {return _wmState;}
+	const std::map<int, Transform> & odomCachePoses() const {return _odomCachePoses;}
+	const std::multimap<int, Link> & odomCacheConstraints() const {return _odomCacheConstraints;}
+
+	const std::map<std::string, float> & data() const {return _data;}
+
+private:
+	bool _extended; // 0 -> only loop closure and last signature ID fields are filled
+
+	int _refImageId;
+	int _refImageMapId;
+	int _loopClosureId;
+	int _loopClosureMapId;
+	int _proximiyDetectionId;
+	int _proximiyDetectionMapId;
+	double _stamp;
+
+	std::map<int, Signature> _signaturesData;
+	Signature _dummyEmptyData;
+
+	std::map<int, Transform> _poses;
+	std::multimap<int, Link> _constraints;
+	Transform _mapCorrection;
+	Transform _loopClosureTransform;
+	cv::Mat _localizationCovariance;
+
+	std::map<int, std::string> _labels;
+	std::map<int, int> _weights;
+	std::map<int, float> _posterior;
+	std::map<int, float> _likelihood;
+	std::map<int, float> _rawLikelihood;
+
+	std::vector<int> _localPath;
+	int _currentGoalId;
+
+	std::map<int, int> _reducedIds;
+
+	std::vector<int> _wmState;
+
+	std::map<int, Transform> _odomCachePoses;
+	std::multimap<int, Link> _odomCacheConstraints;
+
+	// Format for statistics (Plottable statistics must go in that map) :
+	// {"Group/Name/Unit", value}
+	// Example : {"Timing/Total time/ms", 500.0f}
+	std::map<std::string, float> _data;
+	static std::map<std::string, float> _defaultData;
+	static bool _defaultDataInitialized;
+	// end extended data
+};
+
+}// end namespace rtabmap
+
+#endif /* STATISTICS_H_ */

rtabmap/corelib/include/rtabmap/core/Stereo.h

@@ -0,0 +1,114 @@
+/*
+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.
+*/
+
+#ifndef STEREO_H_
+#define STEREO_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Parameters.h>
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT Stereo {
+public:
+	static Stereo * create(const ParametersMap & parameters = ParametersMap());
+
+public:
+	Stereo(const ParametersMap & parameters = ParametersMap());
+	virtual ~Stereo() {}
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual std::vector<cv::Point2f> computeCorrespondences(
+			const cv::Mat & leftImage,
+			const cv::Mat & rightImage,
+			const std::vector<cv::Point2f> & leftCorners,
+			std::vector<unsigned char> & status) const;
+#ifdef HAVE_OPENCV_CUDEV
+	virtual std::vector<cv::Point2f> computeCorrespondences(
+			const cv::cuda::GpuMat & leftImage,
+			const cv::cuda::GpuMat & rightImage,
+			const std::vector<cv::Point2f> & leftCorners,
+			std::vector<unsigned char> & status) const;
+#endif
+
+	cv::Size winSize() const {return cv::Size(winWidth_, winHeight_);}
+	int iterations() const   {return iterations_;}
+	int maxLevel() const     {return maxLevel_;}
+	float minDisparity() const {return minDisparity_;}
+	float maxDisparity() const {return maxDisparity_;}
+	bool winSSD() const      {return winSSD_;}
+	virtual bool isGpuEnabled() const {return false;}
+
+private:
+	int winWidth_;
+	int winHeight_;
+	int iterations_;
+	int maxLevel_;
+	float minDisparity_;
+	float maxDisparity_;
+	bool winSSD_;
+};
+
+class RTABMAP_CORE_EXPORT StereoOpticalFlow : public Stereo {
+public:
+	StereoOpticalFlow(const ParametersMap & parameters = ParametersMap());
+	virtual ~StereoOpticalFlow() {}
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual std::vector<cv::Point2f> computeCorrespondences(
+			const cv::Mat & leftImage,
+			const cv::Mat & rightImage,
+			const std::vector<cv::Point2f> & leftCorners,
+			std::vector<unsigned char> & status) const;
+	
+#ifdef HAVE_OPENCV_CUDEV
+	virtual std::vector<cv::Point2f> computeCorrespondences(
+			const cv::cuda::GpuMat & leftImage,
+			const cv::cuda::GpuMat & rightImage,
+			const std::vector<cv::Point2f> & leftCorners,
+			std::vector<unsigned char> & status) const;
+#endif
+
+	float epsilon() const {return epsilon_;}
+	virtual bool isGpuEnabled() const;
+
+private:
+	void updateStatus(
+		const std::vector<cv::Point2f> & leftCorners,
+		const std::vector<cv::Point2f> & rightCorners,
+		std::vector<unsigned char> & status) const;
+
+private:
+	float epsilon_;
+	bool gpu_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* STEREO_H_ */

rtabmap/corelib/include/rtabmap/core/StereoCameraModel.h

@@ -0,0 +1,147 @@
+/*
+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.
+*/
+
+#ifndef STEREOCAMERAMODEL_H_
+#define STEREOCAMERAMODEL_H_
+
+#include <rtabmap/core/CameraModel.h>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT StereoCameraModel
+{
+public:
+	StereoCameraModel() : leftSuffix_("left"), rightSuffix_("right") {}
+	StereoCameraModel(
+			const std::string & name,
+			const cv::Size & imageSize1,
+			const cv::Mat & K1, const cv::Mat & D1, const cv::Mat & R1, const cv::Mat & P1,
+			const cv::Size & imageSize2,
+			const cv::Mat & K2, const cv::Mat & D2, const cv::Mat & R2, const cv::Mat & P2,
+			const cv::Mat & R, const cv::Mat & T, const cv::Mat & E, const cv::Mat & F,
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
+
+	// if R and T are not null, left and right camera models should be valid to be rectified.
+	StereoCameraModel(
+			const std::string & name,
+			const CameraModel & leftCameraModel,
+			const CameraModel & rightCameraModel,
+			const cv::Mat & R = cv::Mat(),
+			const cv::Mat & T = cv::Mat(),
+			const cv::Mat & E = cv::Mat(),
+			const cv::Mat & F = cv::Mat());
+	// if extrinsics transform is not null, left and right camera models should be valid to be rectified.
+	StereoCameraModel(
+			const std::string & name,
+			const CameraModel & leftCameraModel,
+			const CameraModel & rightCameraModel,
+			const Transform & extrinsics);
+
+	//minimal
+	StereoCameraModel(
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			double baseline,
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+			const cv::Size & imageSize = cv::Size(0,0));
+	//minimal to be saved
+	StereoCameraModel(
+			const std::string & name,
+			double fx,
+			double fy,
+			double cx,
+			double cy,
+			double baseline,
+			const Transform & localTransform = Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0),
+			const cv::Size & imageSize = cv::Size(0,0));
+	virtual ~StereoCameraModel() {}
+
+	bool isValidForProjection() const {return left_.isValidForProjection() && right_.isValidForProjection() && baseline() > 0.0;}
+	bool isValidForRectification() const {return left_.isValidForRectification() && right_.isValidForRectification();}
+
+	void initRectificationMap() {left_.initRectificationMap(); right_.initRectificationMap();}
+	bool isRectificationMapInitialized() const {return left_.isRectificationMapInitialized() && right_.isRectificationMapInitialized();}
+
+	void setName(const std::string & name, const std::string & leftSuffix = "left", const std::string & rightSuffix = "right");
+	const std::string & name() const {return name_;}
+
+	// backward compatibility
+	void setImageSize(const cv::Size & size) {left_.setImageSize(size); right_.setImageSize(size);}
+
+	bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true);
+	bool save(const std::string & directory, bool ignoreStereoTransform = true) const;
+	bool saveStereoTransform(const std::string & directory) const;
+	std::vector<unsigned char> serialize() const;
+	unsigned int deserialize(const std::vector<unsigned char>& data);
+	unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
+
+	double baseline() const {return right_.fx()!=0.0 && left_.fx() != 0.0 ? left_.Tx() / left_.fx() - right_.Tx()/right_.fx():0.0;}
+
+	float computeDepth(float disparity) const;
+	float computeDisparity(float depth) const; // m
+	float computeDisparity(unsigned short depth) const; // mm
+
+	const cv::Mat & R() const {return R_;} //extrinsic rotation matrix
+	const cv::Mat & T() const {return T_;} //extrinsic translation matrix
+	const cv::Mat & E() const {return E_;} //extrinsic essential matrix
+	const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
+
+	void scale(double scale);
+	void roi(const cv::Rect & roi);
+
+	void setLocalTransform(const Transform & transform) {left_.setLocalTransform(transform);}
+	const Transform & localTransform() const {return left_.localTransform();}
+	Transform stereoTransform() const;
+
+	const CameraModel & left() const {return left_;}
+	const CameraModel & right() const {return right_;}
+
+	const std::string & getLeftSuffix() const {return leftSuffix_;}
+	const std::string & getRightSuffix() const {return rightSuffix_;}
+
+private:
+	void updateStereoRectification();
+
+private:
+	std::string leftSuffix_;
+	std::string rightSuffix_;
+	CameraModel left_;
+	CameraModel right_;
+	std::string name_;
+	cv::Mat R_;
+	cv::Mat T_;
+	cv::Mat E_;
+	cv::Mat F_;
+};
+
+RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const StereoCameraModel& model);
+
+} // rtabmap
+
+#endif /* STEREOCAMERAMODEL_H_ */

rtabmap/corelib/include/rtabmap/core/StereoDense.h

@@ -0,0 +1,61 @@
+/*
+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.
+*/
+
+#ifndef STEREODENSE_H_
+#define STEREODENSE_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Parameters.h>
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT StereoDense {
+public:
+	enum Type {
+		kTypeBM = 0,
+		kTypeSGBM = 1
+	};
+	static StereoDense * create(const ParametersMap & parameters);
+	static StereoDense * create(StereoDense::Type type, const ParametersMap & parameters = ParametersMap());
+
+public:
+	virtual ~StereoDense() {}
+
+	virtual void parseParameters(const ParametersMap & parameters) {}
+	virtual cv::Mat computeDisparity(
+			const cv::Mat & leftImage,
+			const cv::Mat & rightImage) const = 0;
+
+protected:
+	StereoDense(const ParametersMap & parameters = ParametersMap()) {}
+};
+
+} /* namespace rtabmap */
+
+#endif /* STEREODENSE_H_ */

rtabmap/corelib/include/rtabmap/core/Transform.h

@@ -0,0 +1,200 @@
+/*
+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.
+*/
+
+#ifndef TRANSFORM_H_
+#define TRANSFORM_H_
+
+#include <rtabmap/core/rtabmap_core_export.h>
+#include <vector>
+#include <string>
+#include <map>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT Transform
+{
+public:
+
+	// Zero by default
+	Transform();
+	// rotation matrix r## and origin o##
+	Transform(float r11, float r12, float r13, float o14,
+		      float r21, float r22, float r23, float o24,
+			  float r31, float r32, float r33, float o34);
+	// should have 3 rows, 4 cols and type CV_32FC1
+	Transform(const cv::Mat & transformationMatrix);
+	// x,y,z, roll,pitch,yaw
+	Transform(float x, float y, float z, float roll, float pitch, float yaw);
+	// x,y,z, qx,qy,qz,qw
+	Transform(float x, float y, float z, float qx, float qy, float qz, float qw);
+	// x,y, theta
+	Transform(float x, float y, float theta);
+
+	Transform clone() const;
+
+	float r11() const {return data()[0];}
+	float r12() const {return data()[1];}
+	float r13() const {return data()[2];}
+	float r21() const {return data()[4];}
+	float r22() const {return data()[5];}
+	float r23() const {return data()[6];}
+	float r31() const {return data()[8];}
+	float r32() const {return data()[9];}
+	float r33() const {return data()[10];}
+
+	float o14() const {return data()[3];}
+	float o24() const {return data()[7];}
+	float o34() const {return data()[11];}
+
+	float & operator[](int index) {return data()[index];}
+	const float & operator[](int index) const {return data()[index];}
+	float & operator()(int row, int col) {return data()[row*4 + col];}
+	const float & operator()(int row, int col) const {return data()[row*4 + col];}
+
+	bool isNull() const;
+	bool isIdentity() const;
+
+	void setNull();
+	void setIdentity();
+
+	const cv::Mat & dataMatrix() const {return data_;}
+	const float * data() const {return (const float *)data_.data;}
+	float * data() {return (float *)data_.data;}
+	int size() const {return 12;}
+
+	float & x() {return data()[3];}
+	float & y() {return data()[7];}
+	float & z() {return data()[11];}
+	const float & x() const {return data()[3];}
+	const float & y() const {return data()[7];}
+	const float & z() const {return data()[11];}
+
+	float theta() const;
+
+	bool isInvertible() const;
+	Transform inverse() const;
+	Transform rotation() const;
+	Transform translation() const;
+	Transform to3DoF() const;
+	Transform to4DoF() const;
+	bool is3DoF() const;
+	bool is4DoF() const;
+
+	cv::Mat rotationMatrix() const;
+	cv::Mat translationMatrix() const;
+
+	void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
+	void getEulerAngles(float & roll, float & pitch, float & yaw) const;
+	void getTranslation(float & x, float & y, float & z) const;
+	float getAngle(const Transform & t) const;
+	float getNorm() const;
+	float getNormSquared() const;
+	float getDistance(const Transform & t) const;
+	float getDistanceSquared(const Transform & t) const;
+	Transform interpolate(float t, const Transform & other) const;
+	void normalizeRotation();
+	std::string prettyPrint() const;
+
+	Transform operator*(const Transform & t) const;
+	Transform & operator*=(const Transform & t);
+	bool operator==(const Transform & t) const;
+	bool operator!=(const Transform & t) const;
+
+	Eigen::Matrix4f toEigen4f() const;
+	Eigen::Matrix4d toEigen4d() const;
+	Eigen::Affine3f toEigen3f() const;
+	Eigen::Affine3d toEigen3d() const;
+
+	Eigen::Quaternionf getQuaternionf() const;
+	Eigen::Quaterniond getQuaterniond() const;
+
+public:
+	static Transform getIdentity();
+	static Transform fromEigen4f(const Eigen::Matrix4f & matrix);
+	static Transform fromEigen4d(const Eigen::Matrix4d & matrix);
+	static Transform fromEigen3f(const Eigen::Affine3f & matrix);
+	static Transform fromEigen3d(const Eigen::Affine3d & matrix);
+	static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
+	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
+	static Transform fromEigen3f(const Eigen::Matrix<float, 3, 4> & matrix);
+	static Transform fromEigen3d(const Eigen::Matrix<double, 3, 4> & matrix);
+
+	static Transform opengl_T_rtabmap() {return Transform(
+			 0.0f, -1.0f, 0.0f, 0.0f,
+			 0.0f,  0.0f, 1.0f, 0.0f,
+			-1.0f,  0.0f, 0.0f, 0.0f);}
+	static Transform rtabmap_T_opengl() {return Transform(
+			 0.0f,  0.0f,-1.0f, 0.0f,
+			-1.0f,  0.0f, 0.0f, 0.0f,
+			 0.0f,  1.0f, 0.0f, 0.0f);}
+
+	/**
+	 * Format (3 values): x y z
+	 * Format (6 values): x y z roll pitch yaw
+	 * Format (7 values): x y z qx qy qz qw
+	 * Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33
+	 * Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
+	 */
+	static Transform fromString(const std::string & string);
+	static bool canParseString(const std::string & string);
+
+	static Transform getTransform(
+				const std::map<double, Transform> & tfBuffer,
+				const double & stamp);
+	// Use Transform::getTransform() instead to get always accurate transforms.
+	RTABMAP_DEPRECATED static Transform getClosestTransform(
+				const std::map<double, Transform> & tfBuffer,
+				const double & stamp,
+				double * stampDiff);
+
+private:
+	cv::Mat data_;
+};
+
+RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const Transform& s);
+
+class TransformStamped
+{
+public:
+	TransformStamped(const Transform & transform, const double & stamp) :
+		transform_(transform),
+		stamp_(stamp)
+	{}
+	const Transform & transform() const {return transform_;}
+	const double & stamp() const {return stamp_;}
+
+private:
+	Transform transform_;
+	double stamp_;
+};
+
+}
+
+#endif /* TRANSFORM_H_ */

rtabmap/corelib/include/rtabmap/core/UserDataEvent.h

@@ -0,0 +1,59 @@
+/*
+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.
+*/
+
+#ifndef USERDATAEVENT_H_
+#define USERDATAEVENT_H_
+
+#include <opencv2/opencv.hpp>
+#include <rtabmap/utilite/UEvent.h>
+
+namespace rtabmap
+{
+
+/**
+ * The user data event.
+ */
+class UserDataEvent : public UEvent
+{
+public:
+	UserDataEvent(const cv::Mat & data) :
+		UEvent(0),
+		data_(data)
+	{}
+	~UserDataEvent() {}
+	virtual std::string getClassName() const {return "UserDataEvent";}
+
+	const cv::Mat & data() const {return data_;}
+
+private:
+	cv::Mat data_;
+};
+
+}
+
+#endif /* USERDATAEVENT_H_ */
+

rtabmap/corelib/include/rtabmap/core/VWDictionary.h

@@ -0,0 +1,152 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <list>
+#include <set>
+#include "rtabmap/core/Parameters.h"
+
+namespace rtabmap
+{
+
+class DBDriver;
+class VisualWord;
+class FlannIndex;
+
+class RTABMAP_CORE_EXPORT VWDictionary
+{
+public:
+	enum NNStrategy{
+		kNNFlannNaive,
+		kNNFlannKdTree,
+		kNNFlannLSH,
+		kNNBruteForce,
+		kNNBruteForceGPU,
+		kNNUndef};
+	static const int ID_START;
+	static const int ID_INVALID;
+	static std::string nnStrategyName(NNStrategy strategy)
+	{
+		switch(strategy) {
+		case kNNFlannNaive:
+			return "FLANN NAIVE";
+		case kNNFlannKdTree:
+			return "FLANN KD-TREE";
+		case kNNFlannLSH:
+			return "FLANN LSH";
+		case kNNBruteForce:
+			return "BRUTE FORCE";
+		case kNNBruteForceGPU:
+			return "BRUTE FORCE GPU";
+		default:
+			return "Unknown";
+		}
+	}
+
+public:
+	VWDictionary(const ParametersMap & parameters = ParametersMap());
+	virtual ~VWDictionary();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+
+	virtual void update();
+
+	virtual std::list<int> addNewWords(
+			const cv::Mat & descriptors,
+			int signatureId);
+	virtual void addWord(VisualWord * vw);
+
+	std::vector<int> findNN(const std::list<VisualWord *> & vws) const;
+	std::vector<int> findNN(const cv::Mat & descriptors) const;
+
+	void addWordRef(int wordId, int signatureId);
+	void removeAllWordRef(int wordId, int signatureId);
+	const VisualWord * getWord(int id) const;
+	VisualWord * getUnusedWord(int id) const;
+	void setLastWordId(int id) {_lastWordId = id;}
+	const std::map<int, VisualWord *> & getVisualWords() const {return _visualWords;}
+	float getNndrRatio() const {return _nndrRatio;}
+	unsigned int getNotIndexedWordsCount() const {return (int)_notIndexedWords.size();}
+	int getLastIndexedWordId() const;
+	int getTotalActiveReferences() const {return _totalActiveReferences;}
+	unsigned int getIndexedWordsCount() const;
+	unsigned int getIndexMemoryUsed() const; // KB
+	unsigned long getMemoryUsed() const; //Bytes
+	bool setNNStrategy(NNStrategy strategy); // Return true if the search tree has been re-initialized
+	bool isIncremental() const {return _incrementalDictionary;}
+	bool isIncrementalFlann() const {return _incrementalFlann;}
+	void setIncrementalDictionary();
+	void setFixedDictionary(const std::string & dictionaryPath);
+
+	void exportDictionary(const char * fileNameReferences, const char * fileNameDescriptors) const;
+
+	void clear(bool printWarningsIfNotEmpty = true);
+	std::vector<VisualWord *> getUnusedWords() const;
+	std::vector<int> getUnusedWordIds() const;
+	unsigned int getUnusedWordsSize() const {return (int)_unusedWords.size();}
+	void removeWords(const std::vector<VisualWord*> & words); // caller must delete the words
+	void deleteUnusedWords();
+
+public:
+	static cv::Mat convertBinTo32F(const cv::Mat & descriptorsIn, bool byteToFloat = true);
+	static cv::Mat convert32FToBin(const cv::Mat & descriptorsIn, bool byteToFloat = true);
+
+protected:
+	int getNextId();
+
+protected:
+	std::map<int, VisualWord *> _visualWords; //<id,VisualWord*>
+	int _totalActiveReferences; // keep track of all references for updating the common signature
+
+private:
+	bool _incrementalDictionary;
+	bool _incrementalFlann;
+	float _rebalancingFactor;
+	bool _byteToFloat;
+	float _nndrRatio;
+	std::string _dictionaryPath; // a pre-computed dictionary (.txt or .db)
+	std::string _newDictionaryPath; // a pre-computed dictionary (.txt or .db)
+	bool _newWordsComparedTogether;
+	int _lastWordId;
+	bool useDistanceL1_;
+	FlannIndex * _flannIndex;
+	cv::Mat _dataTree;
+	NNStrategy _strategy;
+	std::map<int ,int> _mapIndexId;
+	std::map<int ,int> _mapIdIndex;
+	std::map<int, VisualWord*> _unusedWords; //<id,VisualWord*>, note that these words stay in _visualWords
+	std::set<int> _notIndexedWords; // Words that are not indexed in the dictionary
+	std::set<int> _removedIndexedWords; // Words not anymore in the dictionary but still indexed in the dictionary
+};
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/VisualWord.h

@@ -0,0 +1,66 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <opencv2/core/core.hpp>
+#include <map>
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT VisualWord
+{
+public:
+	VisualWord(int id, const cv::Mat & descriptor, int signatureId = 0);
+	~VisualWord();
+
+	void addRef(int signatureId);
+	int removeAllRef(int signatureId);
+	unsigned long getMemoryUsed() const;
+
+	int getTotalReferences() const {return _totalReferences;}
+	int id() const {return _id;}
+	const cv::Mat & getDescriptor() const {return _descriptor;}
+	const std::map<int, int> & getReferences() const {return _references;} // (signature id , occurrence in the signature)
+
+	bool isSaved() const {return _saved;}
+	void setSaved(bool saved) {_saved = saved;}
+
+private:
+	int _id;
+	cv::Mat _descriptor;
+	bool _saved; // If it's saved to db
+
+	int _totalReferences;
+	std::map<int, int> _references; // (signature id , occurrence in the signature)
+	std::map<int, int> _oldReferences; // (signature id , occurrence in the signature)
+};
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraDepthAI.h

@@ -0,0 +1,116 @@
+/*
+Copyright (c) 2010-2021, 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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+#ifdef RTABMAP_DEPTHAI
+#ifndef DEPTHAI_OPENCV_SUPPORT
+#define DEPTHAI_OPENCV_SUPPORT
+#endif
+#include <depthai/depthai.hpp>
+#endif
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraDepthAI :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraDepthAI(
+			const std::string & mxidOrName = "",
+			int imageWidth = 1280, // 640 or 1280
+			float imageRate = 0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraDepthAI();
+
+	void setOutputMode(int outputMode = 0);
+	void setDepthProfile(int confThreshold = 200, int lrcThreshold = 5);
+	void setExtendedDisparity(bool extendedDisparity = false, bool enableCompanding = false);
+	void setSubpixelMode(bool enabled = false, int fractionalBits = 3);
+	void setDisparityWidthAndFilter(int disparityWidth = 96, int medianFilter = 5);
+	void setRectification(bool useSpecTranslation = false, float alphaScaling = 0.0f, bool enabled = true);
+	void setIMU(bool imuPublished, bool publishInterIMU);
+	void setIrIntensity(float dotIntensity = 0.0f, float floodIntensity = 0.0f);
+	void setDetectFeatures(int detectFeatures = 0, const std::string & blobPath = "");
+	void setGFTTDetector(bool useHarrisDetector = false, float minDistance = 7.0f, int numTargetFeatures = 1000);
+	void setSuperPointDetector(float threshold = 0.01f, bool nms = true, int nmsRadius = 4);
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_DEPTHAI
+	StereoCameraModel stereoModel_;
+	cv::Size targetSize_;
+	Transform imuLocalTransform_;
+	std::string mxidOrName_;
+	int outputMode_;
+	int confThreshold_;
+	int lrcThreshold_;
+	int imageWidth_;
+	bool extendedDisparity_;
+	bool enableCompanding_;
+	int subpixelFractionalBits_;
+	int disparityWidth_;
+	int medianFilter_;
+	bool useSpecTranslation_;
+	float alphaScaling_;
+	bool imagesRectified_;
+	bool imuPublished_;
+	bool publishInterIMU_;
+	float dotIntensity_;
+	float floodIntensity_;
+	int detectFeatures_;
+	bool useHarrisDetector_;
+	float minDistance_;
+	int numTargetFeatures_;
+	float threshold_;
+	bool nms_;
+	int nmsRadius_;
+	std::string blobPath_;
+	std::unique_ptr<dai::Device> device_;
+	std::shared_ptr<dai::DataOutputQueue> cameraQueue_;
+	std::map<double, cv::Vec3f> accBuffer_;
+	std::map<double, cv::Vec3f> gyroBuffer_;
+	UMutex imuMutex_;
+#endif
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraFreenect.h

@@ -0,0 +1,77 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+typedef struct _freenect_context freenect_context;
+typedef struct _freenect_device freenect_device;
+
+namespace rtabmap
+{
+
+class FreenectDevice;
+
+class RTABMAP_CORE_EXPORT CameraFreenect :
+	public Camera
+{
+public:
+	static bool available();
+	enum Type {kTypeColorDepth, kTypeIRDepth};
+
+public:
+	// default local transform z in, x right, y down));
+	CameraFreenect(int deviceId= 0,
+					Type type = kTypeColorDepth,
+					float imageRate=0.0f,
+					const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraFreenect();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_FREENECT
+	int deviceId_;
+	Type type_;
+	freenect_context * ctx_;
+	FreenectDevice * freenectDevice_;
+	StereoCameraModel stereoModel_;
+#endif
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraFreenect2.h

@@ -0,0 +1,101 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+namespace libfreenect2
+{
+class Freenect2;
+class Freenect2Device;
+class SyncMultiFrameListener;
+class Registration;
+class PacketPipeline;
+}
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraFreenect2 :
+	public Camera
+{
+public:
+	static bool available();
+
+	enum Type{
+		kTypeColor2DepthSD,
+		kTypeDepth2ColorSD,
+		kTypeDepth2ColorHD,
+		kTypeDepth2ColorHD2,
+		kTypeIRDepth,
+		kTypeColorIR
+	};
+
+public:
+	// default local transform z in, x right, y down));
+	CameraFreenect2(int deviceId= 0,
+					Type type = kTypeDepth2ColorSD,
+					float imageRate=0.0f,
+					const Transform & localTransform = Transform::getIdentity(),
+					float minDepth = 0.3f,
+					float maxDepth = 12.0f,
+					bool bilateralFiltering = true,
+					bool edgeAwareFiltering = true,
+					bool noiseFiltering = true,
+					const std::string & pipelineName = "");
+	virtual ~CameraFreenect2();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_FREENECT2
+	int deviceId_;
+	Type type_;
+	StereoCameraModel stereoModel_;
+	libfreenect2::Freenect2 * freenect2_;
+	libfreenect2::Freenect2Device *dev_;
+	libfreenect2::SyncMultiFrameListener * listener_;
+	libfreenect2::Registration * reg_;
+	float minKinect2Depth_;
+	float maxKinect2Depth_;
+	bool bilateralFiltering_;
+	bool edgeAwareFiltering_;
+	bool noiseFiltering_;
+	std::string pipelineName_;
+#endif
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraImages.h

@@ -0,0 +1,181 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/utilite/UTimer.h"
+#include <list>
+
+class UDirectory;
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraImages :
+	public Camera
+{
+public:
+	CameraImages();
+	CameraImages(
+			const std::string & path,
+			float imageRate = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraImages();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	virtual bool odomProvided() const { return odometry_.size() > 0; }
+	std::string getPath() const {return _path;}
+	unsigned int imagesCount() const;
+	std::vector<std::string> filenames() const;
+	bool isImagesRectified() const {return _rectifyImages;}
+	int getBayerMode() const {return _bayerMode;}
+	const CameraModel & cameraModel() const {return _model;}
+
+	void setPath(const std::string & dir) {_path=dir;}
+	virtual void setStartIndex(int index) {_startAt = index;} // negative means last
+	virtual void setMaxFrames(int value) {_maxFrames = value;}
+	void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
+	void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
+	void setBayerMode(int mode) {_bayerMode = mode;} // -1=disabled (default) 0=BayerBG, 1=BayerGB, 2=BayerRG, 3=BayerGR
+
+	void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "", bool syncImageRateWithStamps=true)
+	{
+		_filenamesAreTimestamps = fileNamesAreStamps;
+		_timestampsPath=filePath;
+		_syncImageRateWithStamps = syncImageRateWithStamps;
+	}
+
+	void setConfigForEachFrame(bool value)
+	{
+		_hasConfigForEachFrame = value;
+	}
+
+	void setScanPath(
+			const std::string & dir,
+			int maxScanPts = 0,
+			const Transform & localTransform=Transform::getIdentity())
+	{
+		_scanPath = dir;
+		_scanLocalTransform = localTransform;
+		_scanMaxPts = maxScanPts;
+	}
+
+	void setDepthFromScan(bool enabled, int fillHoles = 1, bool fillHolesFromBorder = false)
+	{
+		_depthFromScan = enabled;
+		_depthFromScanFillHoles = fillHoles;
+		_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
+	}
+
+	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
+	void setOdometryPath(const std::string & filePath, int format = 0)
+	{
+		_odometryPath = filePath;
+		_odometryFormat = format;
+	}
+
+	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
+	void setGroundTruthPath(const std::string & filePath, int format = 0)
+	{
+		_groundTruthPath = filePath;
+		_groundTruthFormat = format;
+	}
+
+	void setMaxPoseTimeDiff(double diff) {_maxPoseTimeDiff = diff;}
+	double getMaxPoseTimeDiff() const {return _maxPoseTimeDiff;}
+
+	void setDepth(bool isDepth, float depthScaleFactor = 1.0f)
+	{
+		_isDepth = isDepth;
+		_depthScaleFactor=depthScaleFactor;
+	}
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+	bool readPoses(
+		std::list<Transform> & outputPoses,
+		std::list<double> & stamps,
+		const std::string & filePath,
+		int format,
+		double maxTimeDiff) const;
+
+private:
+	std::string _path;
+	int _startAt;
+	int _maxFrames;
+	// If the list of files in the directory is refreshed
+	// on each call of takeImage()
+	bool _refreshDir;
+	bool _rectifyImages;
+	int _bayerMode;
+	bool _isDepth;
+	float _depthScaleFactor;
+	int _count;
+	int _framesPublished;
+	UDirectory * _dir;
+	std::string _lastFileName;
+
+	int _countScan;
+	UDirectory * _scanDir;
+	std::string _lastScanFileName;
+	std::string _scanPath;
+	Transform _scanLocalTransform;
+	int _scanMaxPts;
+
+	bool _depthFromScan;
+	int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical
+	bool _depthFromScanFillHolesFromBorder;
+
+	bool _filenamesAreTimestamps;
+	bool _hasConfigForEachFrame;
+	std::string _timestampsPath;
+	bool _syncImageRateWithStamps;
+
+	std::string _odometryPath;
+	int _odometryFormat;
+	std::string _groundTruthPath;
+	int _groundTruthFormat;
+	double _maxPoseTimeDiff;
+
+	std::list<double> _stamps;
+	std::list<Transform> odometry_;
+	std::list<cv::Mat> covariances_;
+	std::list<Transform> groundTruth_;
+	CameraModel _model;
+	std::list<CameraModel> _models;
+
+	UTimer _captureTimer;
+	double _captureDelay;
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraK4A.h

@@ -0,0 +1,98 @@
+/*
+Copyright (c) 2010-2019, 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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+#include "rtabmap/utilite/UTimer.h"
+
+#ifdef RTABMAP_K4A
+#include <k4a/k4atypes.h>
+#include <k4arecord/playback.h>
+#endif
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraK4A :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraK4A(int deviceId = 0,
+		float imageRate = 0.0f,
+		const Transform & localTransform = Transform::getIdentity());
+	CameraK4A(const std::string & fileName,
+		float imageRate = 0.0f,
+		const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraK4A();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+	void setIRDepthFormat(bool enabled);
+	void setPreferences(int rgb_resolution, int framerate, int depth_resolution);
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+	void close();
+
+private:
+
+#ifdef RTABMAP_K4A
+	k4a_device_t deviceHandle_;
+	k4a_device_configuration_t config_;
+	k4a_calibration_t calibration_;
+	k4a_transformation_t transformationHandle_;
+	k4a_capture_t captureHandle_;
+	k4a_playback_t playbackHandle_;
+	std::string serial_number_;
+
+	CameraModel model_;
+	int deviceId_;
+	std::string fileName_;
+	int rgb_resolution_;
+	int framerate_;
+	int depth_resolution_;
+	bool ir_;
+	double previousStamp_;
+	double timestampOffset_;
+	UTimer timer_;
+	Transform imuLocalTransform_;
+#endif
+
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraK4W2.h

@@ -0,0 +1,95 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+typedef struct IKinectSensor IKinectSensor;
+typedef struct ICoordinateMapper ICoordinateMapper;
+typedef struct _DepthSpacePoint DepthSpacePoint;
+typedef struct _ColorSpacePoint ColorSpacePoint;
+typedef struct tagRGBQUAD RGBQUAD;
+typedef struct IMultiSourceFrameReader IMultiSourceFrameReader;
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraK4W2 :
+	public Camera
+{
+public:
+	static bool available();
+
+	enum Type {
+		kTypeColor2DepthSD,
+		kTypeDepth2ColorSD,
+		kTypeDepth2ColorHD
+	};
+
+public:
+	static const int        cDepthWidth = 512;
+	static const int        cDepthHeight = 424;
+	static const int        cColorWidth = 1920;
+	static const int        cColorHeight = 1080;
+
+public:
+	// default local transform z in, x right, y down));
+	CameraK4W2(int deviceId = 0, // not used
+		Type type = kTypeDepth2ColorSD,
+		float imageRate = 0.0f,
+		const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraK4W2();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+	void close();
+
+private:
+#ifdef RTABMAP_K4W2
+	Type type_;
+	IKinectSensor*          pKinectSensor_;
+	ICoordinateMapper*      pCoordinateMapper_;
+	DepthSpacePoint*        pDepthCoordinates_;
+	ColorSpacePoint*        pColorCoordinates_;
+	IMultiSourceFrameReader* pMultiSourceFrameReader_;
+	RGBQUAD * pColorRGBX_;
+	INT_PTR hMSEvent;
+	CameraModel colorCameraModel_;
+#endif
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraMyntEye.h

@@ -0,0 +1,110 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+#include "rtabmap/utilite/USemaphore.h"
+#include <memory>
+
+namespace mynteye {
+class Device;
+class API;
+}
+
+namespace rtabmap
+{
+
+/**
+ * Class CameraMyntEye
+ *
+ */
+class RTABMAP_CORE_EXPORT CameraMyntEye : public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraMyntEye(const std::string & device = "", bool apiRectification = false, bool apiDepth = false, float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraMyntEye();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	virtual bool odomProvided() const { return false; }
+
+	void publishInterIMU(bool enabled);
+	void setAutoExposure();
+	void setManualExposure(int gain=24, int brightness=120, int constrast=116);
+	void setIrControl(int value);
+
+protected:
+	/**
+	 * returned rgb and depth images should be already rectified if calibration was loaded
+	 */
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_MYNTEYE
+	double hardTimeToSoftTime(std::uint64_t hardTime);
+	void getPoseAndIMU(const double & stamp, IMU & imu, int maxWaitTimeMs = 35) const;
+	double checkUpTimeStamp(std::uint64_t _hard_time, std::uint8_t stream);
+
+	std::shared_ptr<mynteye::Device> device_;
+	std::shared_ptr<mynteye::API> api_;
+	StereoCameraModel stereoModel_;
+	std::string deviceName_;
+	bool apiRectification_;
+	bool apiDepth_;
+	bool autoExposure_;
+	int gain_;
+	int brightness_;
+	int contrast_;
+	int irControl_;
+	USemaphore dataReady_;
+	UMutex dataMutex_;
+	cv::Mat leftFrameBuffer_;
+	cv::Mat rightFrameBuffer_;
+	std::pair<cv::Mat, cv::Mat> lastFrames_;
+	double lastFramesStamp_;
+	std::uint64_t stamp_;
+	bool publishInterIMU_;
+	Transform imuLocalTransform_;
+	std::map<double, std::pair<cv::Vec3f, cv::Vec3f> > imuBuffer_;
+	UMutex imuMutex_;
+
+	double softTimeBegin_;
+	std::uint64_t hardTimeBegin_;
+	std::uint64_t unitHardTime_;
+	std::vector<std::uint64_t> lastHardTimes_;
+	std::vector<std::uint64_t> acc_;
+#endif
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraOpenNI2.h

@@ -0,0 +1,93 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+namespace openni
+{
+class Device;
+class VideoStream;
+}
+
+namespace rtabmap
+{
+class RTABMAP_CORE_EXPORT CameraOpenNI2 :
+	public Camera
+{
+
+public:
+	static bool available();
+	static bool exposureGainAvailable();
+	enum Type {kTypeColorDepth, kTypeIRDepth, kTypeIR};
+
+public:
+	CameraOpenNI2(const std::string & deviceId = "",
+					Type type = kTypeColorDepth,
+					float imageRate = 0,
+					const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraOpenNI2();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+	bool setAutoWhiteBalance(bool enabled);
+	bool setAutoExposure(bool enabled);
+	bool setExposure(int value);
+	bool setGain(int value);
+	bool setMirroring(bool enabled);
+	void setOpenNI2StampsAndIDsUsed(bool used);
+	void setIRDepthShift(int horizontal, int vertical);
+	void setDepthDecimation(int decimation);
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_OPENNI2
+	Type _type;
+	openni::Device * _device;
+	openni::VideoStream * _color;
+	openni::VideoStream * _depth;
+	float _depthFx;
+	float _depthFy;
+	std::string _deviceId;
+	bool _openNI2StampsAndIDsUsed;
+	StereoCameraModel _stereoModel;
+	int _depthHShift;
+	int _depthVShift;
+	int _depthDecimation;
+#endif
+};
+
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraOpenNICV.h

@@ -0,0 +1,62 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraOpenNICV :
+	public Camera
+{
+
+public:
+	static bool available();
+
+public:
+	CameraOpenNICV(bool asus = false,
+					float imageRate = 0,
+					const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraOpenNICV();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const {return "";} // unknown with OpenCV
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+	bool _asus;
+	cv::VideoCapture _capture;
+	float _depthFocal;
+};
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraOpenni.h

@@ -0,0 +1,99 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/utilite/UMutex.h"
+#include "rtabmap/utilite/USemaphore.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+#include <pcl/pcl_config.h>
+
+#ifdef RTABMAP_OPENNI
+#if __linux__ && __i386__ && __cplusplus >= 201103L
+#warning "Openni driver is not available on i386 when building with c++11 support"
+#endif
+#include <pcl/io/openni_camera/openni_depth_image.h>
+#include <pcl/io/openni_camera/openni_image.h>
+#endif
+
+#include <boost/signals2/connection.hpp>
+
+namespace pcl
+{
+class Grabber;
+}
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraOpenni :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	// default local transform z in, x right, y down));
+	CameraOpenni(const std::string & deviceId="",
+			float imageRate = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraOpenni();
+#ifdef RTABMAP_OPENNI
+#if PCL_VERSION_COMPARE(>=, 1, 10, 0)
+	void image_cb (
+			const std::shared_ptr<openni_wrapper::Image>& rgb,
+			const std::shared_ptr<openni_wrapper::DepthImage>& depth,
+			float constant);
+#else
+    void image_cb (
+    	const boost::shared_ptr<openni_wrapper::Image>& rgb,
+			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
+			float constant);
+#endif
+#endif
+
+    virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+    virtual bool isCalibrated() const;
+    virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+    pcl::Grabber* interface_;
+    std::string deviceId_;
+    boost::signals2::connection connection_;
+    cv::Mat depth_;
+    cv::Mat rgb_;
+    float depthConstant_;
+    UMutex dataMutex_;
+    USemaphore dataReady_;
+};
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraRGBDImages.h

@@ -0,0 +1,62 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include <rtabmap/core/camera/CameraImages.h>
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraRGBDImages :
+	public CameraImages
+{
+public:
+	static bool available();
+
+public:
+	CameraRGBDImages(
+			const std::string & pathRGBImages,
+			const std::string & pathDepthImages,
+			float depthScaleFactor = 1.0f,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraRGBDImages();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+
+	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);cameraDepth_.setStartIndex(index);} // negative means last
+	virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);cameraDepth_.setMaxFrames(value);}
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+	CameraImages cameraDepth_;
+};
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraRealSense.h

@@ -0,0 +1,101 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/utilite/UMutex.h"
+#include "rtabmap/utilite/USemaphore.h"
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+namespace rs
+{
+	class context;
+	class device;
+	namespace slam {
+	class slam;
+	}
+}
+
+namespace rtabmap
+{
+
+class slam_event_handler;
+class RTABMAP_CORE_EXPORT CameraRealSense :
+	public Camera
+{
+public:
+	static bool available();
+	enum RGBSource {kColor, kInfrared, kFishEye};
+
+public:
+	// default local transform z in, x right, y down));
+	CameraRealSense(
+		int deviceId = 0,
+		int presetRGB = 0, // 0=best quality, 1=largest image, 2=highest framerate
+		int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate
+		bool computeOdometry = false,
+		float imageRate = 0,
+		const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraRealSense();
+
+	void setDepthScaledToRGBSize(bool enabled);
+	void setRGBSource(RGBSource source);
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	virtual bool odomProvided() const;
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_REALSENSE
+	rs::context * ctx_;
+	rs::device * dev_;
+	int deviceId_;
+	int presetRGB_;
+	int presetDepth_;
+	bool computeOdometry_;
+	bool depthScaledToRGBSize_;
+	RGBSource rgbSource_;
+	CameraModel cameraModel_;
+	std::vector<int> rsRectificationTable_;
+
+	int motionSeq_[2];
+	rs::slam::slam * slam_;
+	UMutex slamLock_;
+
+	std::map<double, std::pair<cv::Mat, cv::Mat> > bufferedFrames_;
+	std::pair<cv::Mat, cv::Mat> lastSyncFrames_;
+	UMutex dataMutex_;
+	USemaphore dataReady_;
+#endif
+};
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraRealSense2.h

@@ -0,0 +1,156 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/CameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+#include <pcl/pcl_config.h>
+
+#ifdef RTABMAP_REALSENSE2
+#include <librealsense2/rs.hpp>
+#include <librealsense2/hpp/rs_frame.hpp>
+#endif
+
+
+namespace rs2
+{
+	class context;
+	class device;
+	class syncer;
+}
+struct rs2_intrinsics;
+struct rs2_extrinsics;
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraRealSense2 :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraRealSense2(
+		const std::string & deviceId = "",
+		float imageRate = 0,
+		const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraRealSense2();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	virtual bool odomProvided() const;
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06);
+
+	// parameters are set during initialization
+	// D400 series
+	void setEmitterEnabled(bool enabled);
+	void setIRFormat(bool enabled, bool useDepthInsteadOfRightImage);
+	void setResolution(int width, int height, int fps = 30);
+	void setDepthResolution(int width, int height, int fps = 30);
+	void setGlobalTimeSync(bool enabled);
+
+	/**
+	 * Dual mode (D400+T265 or L500+T265)
+	 * @param enabled enable dual mode
+	 * @param extrinsics the extrinsics between T265 pose frame (middle of the camera) to D400/L500 main camera (without optical rotation).
+	 */
+	void setDualMode(bool enabled, const Transform & extrinsics);
+	void setJsonConfig(const std::string & json);
+	// T265 related parameters
+	void setImagesRectified(bool enabled);
+	void setOdomProvided(bool enabled, bool imageStreamsDisabled=false, bool onlyLeftStream = false);
+
+#ifdef RTABMAP_REALSENSE2
+private:
+	void close();
+	void imu_callback(rs2::frame frame);
+	void pose_callback(rs2::frame frame);
+	void frame_callback(rs2::frame frame);
+	void multiple_message_callback(rs2::frame frame);
+	void getPoseAndIMU(
+			const double & stamp,
+			Transform & pose,
+			unsigned int & poseConfidence,
+			IMU & imu,
+			int maxWaitTimeMs = 35);
+#endif
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_REALSENSE2
+	rs2::context ctx_;
+	std::vector<rs2::device> dev_;
+	std::string deviceId_;
+	rs2::syncer syncer_;
+	float depth_scale_meters_;
+	cv::Mat depthBuffer_;
+	cv::Mat rgbBuffer_;
+	CameraModel model_;
+	StereoCameraModel stereoModel_;
+	Transform imuLocalTransform_;
+	std::map<double, cv::Vec3f> accBuffer_;
+	std::map<double, cv::Vec3f> gyroBuffer_;
+	std::map<double, std::pair<Transform, unsigned int> > poseBuffer_; // <stamp, <Pose, confidence: 1=lost, 2=low, 3=high> >
+	UMutex poseMutex_;
+	UMutex imuMutex_;
+	double lastImuStamp_;
+	bool clockSyncWarningShown_;
+	bool imuGlobalSyncWarningShown_;
+
+	bool emitterEnabled_;
+	bool ir_;
+	bool irDepth_;
+	bool rectifyImages_;
+	bool odometryProvided_;
+	bool odometryImagesDisabled_;
+	bool odometryOnlyLeftStream_;
+	int cameraWidth_;
+	int cameraHeight_;
+	int cameraFps_;
+	int cameraDepthWidth_;
+	int cameraDepthHeight_;
+	int cameraDepthFps_;
+	bool globalTimeSync_;
+	bool dualMode_;
+	Transform dualExtrinsics_;
+	std::string jsonConfig_;
+	bool closing_;
+
+	static Transform realsense2PoseRotation_;
+	static Transform realsense2PoseRotationInv_;
+#endif
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraSeerSense.h

@@ -0,0 +1,52 @@
+#pragma once
+
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+#include "rtabmap/utilite/USemaphore.h"
+
+#ifdef RTABMAP_XVSDK
+#include <xv-sdk.h>
+#endif
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraSeerSense :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraSeerSense(
+		bool computeOdometry = false,
+		float imageRate = 0.0f,
+		const Transform & localTransform = Transform::getIdentity()
+	);
+	virtual ~CameraSeerSense();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	virtual bool odomProvided() const;
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0);
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_XVSDK
+	CameraModel cameraModel_;
+	bool computeOdometry_;
+	int imuId_;
+	int tofId_;
+	std::shared_ptr<xv::Device> device_;
+	std::map<double, std::pair<cv::Vec3d, cv::Vec3d>> imuBuffer_;
+	std::pair<double, std::pair<cv::Mat, cv::Mat>> lastData_;
+	UMutex imuMutex_;
+	UMutex dataMutex_;
+	USemaphore dataReady_;
+#endif
+};
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraStereoDC1394.h

@@ -0,0 +1,64 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+namespace rtabmap
+{
+
+class DC1394Device;
+
+class RTABMAP_CORE_EXPORT CameraStereoDC1394 :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoDC1394();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_DC1394
+	DC1394Device *device_;
+	StereoCameraModel stereoModel_;
+#endif
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h

@@ -0,0 +1,66 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+namespace FlyCapture2
+{
+class Camera;
+}
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraStereoFlyCapture2 :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoFlyCapture2();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_FLYCAPTURE2
+	FlyCapture2::Camera * camera_;
+	void * triclopsCtx_; // TriclopsContext
+#endif
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraStereoImages.h

@@ -0,0 +1,78 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include <rtabmap/core/camera/CameraImages.h>
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/Version.h"
+
+namespace rtabmap
+{
+
+class CameraImages;
+class RTABMAP_CORE_EXPORT CameraStereoImages :
+	public CameraImages
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoImages(
+			const std::string & pathLeftImages,
+			const std::string & pathRightImages,
+			bool rectifyImages = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoImages(
+			const std::string & pathLeftRightImages,
+			bool rectifyImages = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoImages();
+
+	void setRightGrayScale(bool enabled = true) {rightGrayScale_ = enabled;}
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+	virtual void setStartIndex(int index) {CameraImages::setStartIndex(index);camera2_->setStartIndex(index);} // negative means last
+	virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);camera2_->setMaxFrames(value);}
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+	CameraImages * camera2_;
+	StereoCameraModel stereoModel_;
+	bool rightGrayScale_;
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraStereoTara.h

@@ -0,0 +1,73 @@
+/*
+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.
+*/
+
+/**
+ * Contributed by e-consystemgit
+ * https://www.e-consystems.com/opensource-linux-webcam-software-application.asp
+ */
+
+#pragma once
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/camera/CameraVideo.h"
+#include "rtabmap/core/Version.h"
+
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraStereoTara :
+public Camera
+{
+public:
+	static bool available();
+public:
+
+	CameraStereoTara(
+			int device,
+			bool rectifyImages = false,
+			float imageRate = 0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+
+	virtual ~CameraStereoTara();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+	cv::VideoCapture capture_;
+	bool rectifyImages_;
+	StereoCameraModel stereoModel_;
+	std::string cameraName_;
+	int usbDevice_;
+};
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraStereoVideo.h

@@ -0,0 +1,95 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/camera/CameraVideo.h"
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraStereoVideo :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoVideo(
+			const std::string & pathSideBySide,
+			bool rectifyImages = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoVideo(
+			const std::string & pathLeft,
+			const std::string & pathRight,
+			bool rectifyImages = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoVideo(
+			int device,
+			bool rectifyImages = false,
+			float imageRate = 0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	CameraStereoVideo(
+			int deviceLeft,
+			int deviceRight,
+			bool rectifyImages = false,
+			float imageRate = 0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoVideo();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+	void setResolution(int width, int height) {_width=width, _height=height;}
+	void setFOURCC(const std::string & fourcc) { _fourcc = fourcc; }
+	void setRightGrayScale(bool enabled = true) {rightGrayScale_ = enabled;}
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+	cv::VideoCapture capture_;
+	cv::VideoCapture capture2_;
+	std::string path_;
+	std::string path2_;
+	bool rectifyImages_;
+	StereoCameraModel stereoModel_;
+	std::string cameraName_;
+	CameraVideo::Source src_;
+	int usbDevice_;
+	int usbDevice2_;
+	int _width;
+	int _height;
+	std::string _fourcc;
+	bool rightGrayScale_;
+};
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraStereoZed.h

@@ -0,0 +1,110 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/camera/CameraVideo.h"
+#include "rtabmap/core/Version.h"
+
+namespace sl
+{
+class Camera;
+}
+
+namespace rtabmap
+{
+class ZedIMUThread;
+
+class RTABMAP_CORE_EXPORT CameraStereoZed :
+	public Camera
+{
+public:
+	static bool available();
+	static int sdkVersion();
+public:
+	CameraStereoZed(
+			int deviceId,
+			int resolution = -1, // -1 = AUTO, 0=HD4K 1=HD2K 2=HD1080 3=HD1200 4=HD720 5=SVGA 6=VGA
+			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
+			int sensingMode = 0,// 0=STANDARD, 1=FILL
+			int confidenceThr = 100,
+			bool computeOdometry = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity(),
+			bool selfCalibration = true,
+			bool odomForce3DoF = false,
+			int texturenessConfidenceThr = 90); // introduced with ZED SDK 3
+	CameraStereoZed(
+			const std::string & svoFilePath,
+			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY, 3=NEURAL
+			int sensingMode = 0,// 0=STANDARD, 1=FILL
+			int confidenceThr = 100,
+			bool computeOdometry = false,
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity(),
+			bool selfCalibration = true,
+			bool odomForce3DoF = false,
+			int texturenessConfidenceThr = 90); // introduced with ZED SDK 3
+	virtual ~CameraStereoZed();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	virtual bool odomProvided() const;
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0);
+
+	void postInterIMUPublic(const IMU & imu, double stamp);
+	void setRightGrayScale(bool enabled = true);
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_ZED
+	sl::Camera * zed_;
+	StereoCameraModel stereoModel_;
+	Transform imuLocalTransform_;
+	CameraVideo::Source src_;
+	int usbDevice_;
+    std::string svoFilePath_;
+	int resolution_;
+	int quality_;
+	bool selfCalibration_;
+	int sensingMode_;
+	int confidenceThr_;
+	int texturenessConfidenceThr_; // introduced with ZED SDK 3
+	bool computeOdometry_;
+	bool lost_;
+	bool force3DoF_;
+	bool rightGrayScale_;
+	ZedIMUThread * imuPublishingThread_;
+#endif
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraStereoZedOC.h

@@ -0,0 +1,84 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include "rtabmap/core/StereoCameraModel.h"
+#include "rtabmap/core/Camera.h"
+#include "rtabmap/core/Version.h"
+
+namespace sl_oc {
+namespace video {
+class VideoCapture;
+}
+namespace sensors {
+class SensorCapture;
+}
+}
+
+namespace rtabmap
+{
+class ZedOCThread;
+
+class RTABMAP_CORE_EXPORT CameraStereoZedOC :
+	public Camera
+{
+public:
+	static bool available();
+
+public:
+	CameraStereoZedOC(
+			int deviceId,
+			int resolution = 3, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
+			float imageRate=0.0f,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraStereoZedOC();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+
+	void setRightGrayScale(bool enabled = true);
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+#ifdef RTABMAP_ZEDOC
+	sl_oc::video::VideoCapture * zed_;
+	sl_oc::sensors::SensorCapture * sensors_;
+	ZedOCThread * imuThread_;
+	StereoCameraModel stereoModel_;
+	int usbDevice_;
+	int resolution_;
+	uint64_t lastStamp_;
+	bool rightGrayScale_;
+#endif
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/camera/CameraVideo.h

@@ -0,0 +1,89 @@
+/*
+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.
+*/
+
+#pragma once
+
+#include <opencv2/highgui/highgui.hpp>
+#include "rtabmap/core/Camera.h"
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT CameraVideo :
+	public Camera
+{
+public:
+	enum Source{kVideoFile, kUsbDevice};
+
+public:
+	CameraVideo(int usbDevice = 0,
+			bool rectifyImages = false,
+			float imageRate = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	CameraVideo(const std::string & filePath,
+			bool rectifyImages = false,
+			float imageRate = 0,
+			const Transform & localTransform = Transform::getIdentity());
+	virtual ~CameraVideo();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual bool isCalibrated() const;
+	virtual std::string getSerial() const;
+	int getUsbDevice() const {return _usbDevice;}
+	const std::string & getFilePath() const {return _filePath;}
+
+	/**
+	 * Set wanted usb resolution, should be set before initialization. 0 means
+	 * default resolution. It won't be applied if a valid camera calibration
+	 * has been loaded, thus resolution from calibration is used.
+	 * */
+	void setResolution(int width, int height) {_width=width, _height=height;}
+	void setFOURCC(const std::string & fourcc) { _fourcc = fourcc; }
+
+protected:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+
+private:
+	// File type
+	std::string _filePath;
+	bool _rectifyImages;
+
+	cv::VideoCapture _capture;
+	Source _src;
+
+	// Usb camera
+	int _usbDevice;
+	std::string _guid;
+	int _width;
+	int _height;
+	std::string _fourcc;
+
+	CameraModel _model;
+};
+
+
+} // namespace rtabmap

rtabmap/corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h

@@ -0,0 +1,141 @@
+/*
+Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
+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 <organization> 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 <COPYRIGHT HOLDER> 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.
+
+RTAB-Map integration: Mathieu Labbe
+*/
+
+#ifndef DISCRETE_DEPTH_DISTORTION_MODEL_H
+#define DISCRETE_DEPTH_DISTORTION_MODEL_H
+
+#include <assert.h>
+#include <vector>
+#include <set>
+#include <Eigen/Core>
+#include <opencv2/opencv.hpp>
+#include <rtabmap/utilite/UMutex.h>
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+namespace clams
+{
+  class RTABMAP_CORE_EXPORT DiscreteFrustum
+  {
+  public:
+    DiscreteFrustum(int smoothing = 1, double bin_depth = 1.0, double max_dist = 10.0);
+    //! z value, not distance to origin.
+    //! thread-safe.
+    void addExample(double ground_truth, double measurement);
+    int index(double z) const;
+    void undistort(double* z) const;
+    void interpolatedUndistort(double* z) const;
+    void serialize(std::ostream& out, bool ascii) const;
+    void deserialize(std::istream& in, bool ascii);
+  
+  protected:
+    double max_dist_;
+    int num_bins_;
+    double bin_depth_;
+    Eigen::VectorXf counts_;
+    Eigen::VectorXf total_numerators_;
+    Eigen::VectorXf total_denominators_;
+    Eigen::VectorXf multipliers_;
+
+    friend class DiscreteDepthDistortionModel;
+  };
+
+  class RTABMAP_CORE_EXPORT DiscreteDepthDistortionModel
+  {
+  public:
+    // returns all divisors of num
+    static std::set<size_t> getDivisors(const size_t &num);
+
+    // returns divisor from divisors closest to ref
+    static size_t getClosestToRef(const std::set<size_t> &divisors, const double &ref);
+
+    // sets bin_width and bin_height to appropriate values
+    static void getBinSize(const size_t &width, const size_t &height, size_t &bin_width, size_t &bin_height);
+
+  public:
+    DiscreteDepthDistortionModel() :
+    	width_(0),
+		height_(0),
+		bin_width_(0),
+		bin_height_(0),
+		bin_depth_(0),
+		num_bins_x_(0),
+		num_bins_y_(0),
+		training_samples_(0)
+    {}
+    virtual ~DiscreteDepthDistortionModel();
+    DiscreteDepthDistortionModel(int width, int height, int bin_width = 8, int bin_height = 6, double bin_depth = 2.0, int smoothing = 1, double max_depth = 10.0);
+    DiscreteDepthDistortionModel(const DiscreteDepthDistortionModel& other);
+    DiscreteDepthDistortionModel& operator=(const DiscreteDepthDistortionModel& other);
+    void undistort(cv::Mat & depth) const;
+    //! Returns the number of training examples it used from this pair.
+    //! Thread-safe.
+    size_t accumulate(const cv::Mat& ground_truth, const cv::Mat& measurement);
+    void addExample(int v, int u, double ground_truth, double measurement);
+    void save(const std::string& path) const;
+	void load(const std::string& path);
+	void serialize(std::ostream& out, bool ascii) const;
+	void deserialize(std::istream& in, bool ascii);
+    cv::Mat visualize(const std::string& path = "") const;
+
+    int getWidth() const {return width_;}
+    int getHeight() const {return height_;}
+    size_t getTrainingSamples() const {return training_samples_;}
+    bool isValid() const
+    {
+    	return !frustums_.empty();
+    }
+
+  protected:
+    //! Image width.
+    int width_;
+    //! Image height.
+    int height_;
+    //! Width of each bin in pixels.
+    int bin_width_;
+    //! Height of each bin in pixels.
+    int bin_height_;
+    //! Depth of each bin in meters.
+    double bin_depth_;
+    int num_bins_x_;
+    int num_bins_y_;
+    //! frustums_[y][x]
+    std::vector< std::vector<DiscreteFrustum*> > frustums_;
+
+    size_t training_samples_;
+
+    void deleteFrustums();
+    DiscreteFrustum& frustum(int y, int x);
+    const DiscreteFrustum& frustum(int y, int x) const;
+
+    UMutex mutex_;
+  };
+
+}  // namespace clams
+
+#endif // DISCRETE_DEPTH_DISTORTION_MODEL_H

rtabmap/corelib/include/rtabmap/core/clams/frame_projector.h

@@ -0,0 +1,95 @@
+/*
+Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
+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 <organization> 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 <COPYRIGHT HOLDER> 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.
+
+RTAB-Map integration: Mathieu Labbe
+*/
+
+#ifndef FRAME_PROJECTOR_H
+#define FRAME_PROJECTOR_H
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <opencv2/core/core.hpp>
+#include <rtabmap/core/CameraModel.h>
+
+#define MAX_MULT 1.3
+#define MIN_MULT 0.7
+
+namespace clams
+{
+
+  //! "Projective" point comes from the OpenNI terminology, and refers to (u, v, z), i.e.
+  //! pixel id and depth value.  Here I've added color, too, so that this represents everything
+  //! that is known about a pixel in an RBGD camera.
+  class ProjectivePoint
+  {
+  public:
+	  ProjectivePoint() :
+		  u_(0),
+		  v_(0),
+		  z_(0.0f) {}
+
+    int u_;
+    int v_;
+    float z_; // in meters
+  };
+
+  //! This is essentially a pinhole camera model for an RGBD sensor, with
+  //! some extra functions added on for use during calibration.
+  class RTABMAP_CORE_EXPORT FrameProjector
+  {
+  public:
+    // For storing z values in meters.  This is not Euclidean distance.
+    typedef std::vector< std::vector< std::vector<double> > > RangeIndex;
+
+    FrameProjector(const rtabmap::CameraModel & model);
+
+    RangeIndex cloudToRangeIndex(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcd) const;
+    //! transform is applied to the map, then projected into a depth index.
+    //! The best depth estimate from the map corresponding to the measurement depth frame
+    //! will be returned.
+    cv::Mat estimateMapDepth(
+    		const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
+    		const rtabmap::Transform & transform,
+			const cv::Mat & measurement,
+			double coneRadius = 0.02,
+			double coneStdevThresh = 0.03) const;
+                          
+    pcl::PointXYZ project(const ProjectivePoint& ppt) const;
+    ProjectivePoint reproject(const pcl::PointXYZ& pt) const;
+
+  protected:
+    bool coneFit(const cv::Size& imageSize, const RangeIndex& rindex,
+                 int uc, int vc, double radius, double measurement_depth,
+                 double* mean, double* stdev) const;
+
+  private:
+    rtabmap::CameraModel model_;
+  };
+
+}  // namespace clams
+
+#endif // FRAME_PROJECTOR_H

rtabmap/corelib/include/rtabmap/core/clams/slam_calibrator.h

@@ -0,0 +1,50 @@
+/*
+Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
+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 <organization> 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 <COPYRIGHT HOLDER> 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.
+
+RTAB-Map integration: Mathieu Labbe
+*/
+
+#ifndef SLAM_CALIBRATOR_H
+#define SLAM_CALIBRATOR_H
+
+#include <rtabmap/core/clams/discrete_depth_distortion_model.h>
+#include <rtabmap/core/SensorData.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace clams
+{
+  
+DiscreteDepthDistortionModel RTABMAP_CORE_EXPORT calibrate(
+		const std::map<int, rtabmap::SensorData> & sequence,
+		const std::map<int, rtabmap::Transform> & trajectory,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
+		double coneRadius = 0.02,
+		double coneStdevThresh = 0.03);
+
+}
+
+#endif // SLAM_CALIBRATOR_H

rtabmap/corelib/include/rtabmap/core/global_map/CloudMap.h

@@ -0,0 +1,64 @@
+/*
+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.
+*/
+
+#ifndef CORELIB_SRC_CLOUDMAP_H_
+#define CORELIB_SRC_CLOUDMAP_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/GlobalMap.h>
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT CloudMap : public GlobalMap
+{
+public:
+	CloudMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap());
+
+	virtual void clear();
+
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}
+	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
+	const pcl::PointCloud<pcl::PointXYZ>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
+
+	unsigned long getMemoryUsed() const;
+
+protected:
+	virtual void assemble(const std::list<std::pair<int, Transform> > & newPoses);
+
+private:
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledGround_;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledObstacles_;
+	pcl::PointCloud<pcl::PointXYZ>::Ptr assembledEmptyCells_;
+};
+
+}
+
+#endif /* CORELIB_SRC_CLOUDMAP_H_ */

rtabmap/corelib/include/rtabmap/core/global_map/GridMap.h

@@ -0,0 +1,72 @@
+/*
+Copyright (c) 2010-2023, 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.
+*/
+
+#ifndef CORELIB_SRC_GRIDMAP_H_
+#define CORELIB_SRC_GRIDMAP_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/GlobalMap.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/PolygonMesh.h>
+
+namespace grid_map {
+	class GridMap;
+}
+
+namespace rtabmap {
+
+class RTABMAP_CORE_EXPORT GridMap : public GlobalMap
+{
+public:
+	GridMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap());
+
+	virtual ~GridMap();
+	virtual void clear();
+
+	const grid_map::GridMap * gridMap() const {return gridMap_;}
+
+	cv::Mat createHeightMap(float & xMin, float & yMin, float & cellSize) const;
+	cv::Mat createColorMap(float & xMin, float & yMin, float & cellSize) const;
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createTerrainCloud() const;
+	pcl::PolygonMesh::Ptr createTerrainMesh() const;
+
+protected:
+	virtual void assemble(const std::list<std::pair<int, Transform> > & newPoses);
+
+private:
+	cv::Mat toImage(const std::string & layer, float & xMin, float & yMin, float & cellSize) const;
+
+private:
+	grid_map::GridMap * gridMap_;
+	float minMapSize_;
+};
+
+}
+
+#endif /* CORELIB_SRC_OCCUPANCYGRID_H_ */