CameraThread: Added stereo to depth option. Added parameter "Mem/SaveDepth16Format".

corelib/include/rtabmap/core/CameraThread.h

@@ -52,6 +52,7 @@ public:
 
 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
 	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
+	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
 
 	//getters
 	bool isPaused() const {return !this->isRunning();}
@@ -68,6 +69,7 @@ private:
 	Camera * _camera;
 	bool _mirroring;
 	bool _colorOnly;
+	bool _stereoToDepth;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Memory.h

@@ -232,6 +232,7 @@ private:
 	float _similarityThreshold;
 	bool _rawDataKept;
 	bool _binDataKept;
+	bool _saveDepth16Format;
 	bool _notLinkedNodesKeptInDb;
 	bool _incrementalMemory;
 	int _maxStMemSize;

corelib/include/rtabmap/core/Parameters.h

@@ -186,6 +186,7 @@ class RTABMAP_EXP Parameters
 	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, SaveDepth16Format, 		bool, true, 	"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, STMSize, 		        unsigned int, 10, "Short-term memory size.");
 	RTABMAP_PARAM(Mem, IncrementalMemory, 	    bool, true, 	"SLAM mode, otherwise it is Localization mode.");

corelib/include/rtabmap/core/util2d.h

@@ -41,7 +41,8 @@ namespace util2d
 
 cv::Mat RTABMAP_EXP disparityFromStereoImages(
 		const cv::Mat & leftImage,
-		const cv::Mat & rightImage);
+		const cv::Mat & rightImage,
+		int type = CV_32FC1); // CV_32FC1 or CV_16SC1
 
 cv::Mat RTABMAP_EXP disparityFromStereoImages(
 		const cv::Mat & leftImage,
@@ -53,6 +54,10 @@ cv::Mat RTABMAP_EXP disparityFromStereoImages(
 		double flowEps = 0.02,
 		float maxCorrespondencesSlope = 0.1f);
 
+cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
+		float fx, float baseline,
+		int type = CV_32FC1); // CV_32FC1 or CV_16UC1
+
 cv::Mat RTABMAP_EXP depthFromStereoImages(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage,
@@ -78,6 +83,9 @@ cv::Mat RTABMAP_EXP depthFromStereoCorrespondences(
 		const std::vector<unsigned char> & mask,
 		float fx, float baseline);
 
+cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
+cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
+
 float RTABMAP_EXP getDepth(
 		const cv::Mat & depthImage,
 		float x, float y,

corelib/include/rtabmap/core/util3d.h

@@ -127,9 +127,6 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 					float maxDepth = 0,
 					const Transform & localTransform = Transform::getIdentity());
 
-cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
-cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
-
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan);
 
@@ -147,10 +144,6 @@ pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
 		const cv::Mat & disparity,
 		float cx, float cy, float fx, float baseline);
 
-cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
-		float fx, float baseline,
-		int type = CV_32FC1);
-
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
 		const std::list<pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP concatenateClouds(