add support for RVL depth image compression

corelib/include/rtabmap/core/Compression.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #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>
 
@@ -86,5 +87,9 @@ cv::Mat RTABMAP_CORE_EXPORT uncompressData(const unsigned char * bytes, unsigned
 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_ */

corelib/include/rtabmap/core/DBDriver.h

@@ -100,7 +100,7 @@ class RTABMAP_CORE_EXPORT DBDriver : public UThreadNode
 		int nodeId,
 		const std::vector<CameraModel> & models,
 		const std::vector<StereoCameraModel> & stereoModels);
-	void updateDepthImage(int nodeId, const cv::Mat & image);
+	void updateDepthImage(int nodeId, const cv::Mat & image, const std::string & format);
 	void updateLaserScan(int nodeId, const LaserScan & scan);
 
 public:
@@ -243,7 +243,8 @@ class RTABMAP_CORE_EXPORT DBDriver : public UThreadNode
 
 	virtual void updateDepthImageQuery(
 			int nodeId,
-			const cv::Mat & image) const = 0;
+			const cv::Mat & image,
+			const std::string & format) const = 0;
 
 	virtual void updateLaserScanQuery(
 			int nodeId,

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -103,7 +103,8 @@ class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver {
 
 	virtual void updateDepthImageQuery(
 			int nodeId,
-			const cv::Mat & image) const;
+			const cv::Mat & image,
+			const std::string & format) const;
 
 	void updateLaserScanQuery(
 			int nodeId,
@@ -173,7 +174,7 @@ class RTABMAP_CORE_EXPORT DBDriverSqlite3: public DBDriver {
 	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 & imageCompressed) 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;

corelib/include/rtabmap/core/Memory.h

@@ -303,6 +303,7 @@ class RTABMAP_CORE_EXPORT Memory
 	bool _notLinkedNodesKeptInDb;
 	bool _saveIntermediateNodeData;
 	std::string _rgbCompressionFormat;
+	std::string _depthCompressionFormat;
 	bool _incrementalMemory;
 	bool _localizationDataSaved;
 	bool _reduceGraph;

corelib/include/rtabmap/core/Parameters.h

@@ -209,6 +209,7 @@ class RTABMAP_CORE_EXPORT Parameters
     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());

corelib/include/rtabmap/core/rvl_codec.h

@@ -0,0 +1,37 @@
+// The following code is a C++ wrapper of the code presented by
+// Andrew D. Wilson in "Fast Lossless Depth Image Compression" at SIGCHI'17.
+// The original code is licensed under the MIT License.
+
+#ifndef RVL_CODEC_H_
+#define RVL_CODEC_H_
+
+#include <cstdint>
+#include "rtabmap/core/rtabmap_core_export.h"
+
+namespace rtabmap
+{
+
+class RTABMAP_CORE_EXPORT RvlCodec {
+public:
+    RvlCodec();
+    // Compress input data into output. The size of output can be equal to (1.5 * numPixels + 4) in the worst case.
+    int CompressRVL(const uint16_t * input, unsigned char * output, int numPixels);
+    // Decompress input data into output. The size of output must be equal to numPixels.
+    void DecompressRVL(const unsigned char * input, uint16_t * output, int numPixels);
+
+private:
+    RvlCodec(const RvlCodec &);
+    RvlCodec & operator=(const RvlCodec &);
+
+    void EncodeVLE(int value);
+    int DecodeVLE();
+
+    int *buffer_;
+    int *pBuffer_;
+    int word_;
+    int nibblesWritten_;
+};
+
+}  // namespace rtabmap
+
+#endif  // RVL_CODEC_H_

corelib/src/CMakeLists.txt

@@ -64,6 +64,8 @@ SET(SRC_FILES
     util3d_correspondences.cpp
     util3d_motion_estimation.cpp
 
+	rvl_codec.cpp
+
     SensorData.cpp
     Graph.cpp
     Compression.cpp

corelib/src/Compression.cpp

@@ -34,14 +34,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-// format : ".png" ".jpg" "" (empty is general)
+// format : ".jpg" ".png" ".rvl" "" (empty is general)
 CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
 	uncompressedData_(mat),
 	format_(format),
 	image_(!format.empty()),
 	compressMode_(true)
 {
-	UASSERT(format.empty() || format.compare(".png") == 0 || format.compare(".jpg") == 0);
+	UASSERT(format.empty() || format.compare(".jpg") == 0 || format.compare(".png") == 0 || format.compare(".rvl") == 0);
 }
 // assume image
 CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
@@ -96,7 +96,7 @@ void CompressionThread::mainLoop()
 	this->kill();
 }
 
-// ".png" or ".jpg"
+// ".jpg" or ".png" or ".rvl"
 std::vector<unsigned char> compressImage(const cv::Mat & image, const std::string & format)
 {
 	std::vector<unsigned char> bytes;
@@ -106,7 +106,21 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
 		{
 			//save in 8bits-4channel
 			cv::Mat bgra(image.size(), CV_8UC4, image.data);
-			cv::imencode(format, bgra, bytes);
+			cv::imencode(".png", bgra, bytes);
+		}
+		else if(format == ".rvl")
+		{
+			bytes = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L'};
+			int numPixels = image.rows * image.cols;
+        	// In the worst case, RVL compression results in ~1.5x larger data.
+        	bytes.resize(3 * numPixels + 20);
+        	uint32_t cols = image.cols;
+        	uint32_t rows = image.rows;
+        	memcpy(&bytes[8], &cols, 4);
+        	memcpy(&bytes[12], &rows, 4);
+        	RvlCodec rvl;
+        	int compressedSize = rvl.CompressRVL(image.ptr<uint16_t>(), &bytes[16], numPixels);
+        	bytes.resize(16 + compressedSize);
 		}
 		else
 		{
@@ -116,7 +130,7 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
 	return bytes;
 }
 
-// ".png" or ".jpg"
+// ".jpg" or ".png" or ".rvl"
 cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
 {
 	std::vector<unsigned char> bytes = compressImage(image, format);
@@ -129,39 +143,63 @@ cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
 
 cv::Mat uncompressImage(const cv::Mat & bytes)
 {
-	 cv::Mat image;
+	cv::Mat image;
 	if(!bytes.empty())
 	{
+		if (compressedDepthFormat(bytes) == ".rvl")
+		{
+			uint32_t cols, rows;
+        	memcpy(&cols, &bytes.data[8], 4);
+        	memcpy(&rows, &bytes.data[12], 4);
+			image = cv::Mat(rows, cols, CV_16UC1);
+			RvlCodec rvl;
+        	rvl.DecompressRVL(&bytes.data[16], image.ptr<uint16_t>(), cols * rows);
+		}
+		else
+		{
 #if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
+			image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
 #else
-		image = cv::imdecode(bytes, -1);
+			image = cv::imdecode(bytes, -1);
 #endif
-		if(image.type() == CV_8UC4)
-		{
-			// Using clone() or copyTo() caused a memory leak !?!?
-			// image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
-			cv::Mat depth(image.size(), CV_32FC1);
-			memcpy(depth.data, image.data, image.total()*image.elemSize());
-			image = depth;
+			if(image.type() == CV_8UC4)
+			{
+				// Using clone() or copyTo() caused a memory leak !?!?
+				// image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+				cv::Mat depth(image.size(), CV_32FC1);
+				memcpy(depth.data, image.data, image.total()*image.elemSize());
+				image = depth;
+			}
 		}
 	}
 	return image;
 }
 
 cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
 {
-	 cv::Mat image;
+	cv::Mat image;
 	if(bytes.size())
 	{
+		if (compressedDepthFormat(bytes) == ".rvl")
+		{
+			uint32_t cols, rows;
+        	memcpy(&cols, &bytes[8], 4);
+        	memcpy(&rows, &bytes[12], 4);
+			image = cv::Mat(rows, cols, CV_16UC1);
+			RvlCodec rvl;
+        	rvl.DecompressRVL(&bytes[16], image.ptr<uint16_t>(), cols * rows);
+		}
+		else
+		{
 #if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-		image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
+			image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
 #else
-		image = cv::imdecode(bytes, -1);
+			image = cv::imdecode(bytes, -1);
 #endif
-		if(image.type() == CV_8UC4)
-		{
-			image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+			if(image.type() == CV_8UC4)
+			{
+				image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
+			}
 		}
 	}
 	return image;
@@ -291,4 +329,33 @@ std::string uncompressString(const cv::Mat & bytes)
 	return "";
 }
 
+std::string compressedDepthFormat(const cv::Mat & bytes)
+{
+	return compressedDepthFormat(bytes.data, bytes.rows * bytes.cols * bytes.elemSize());
+}
+std::string compressedDepthFormat(const std::vector<unsigned char> & bytes)
+{
+	return compressedDepthFormat(bytes.data(), bytes.size());
+}
+std::string compressedDepthFormat(const unsigned char * bytes, size_t size)
+{
+	std::string format;
+	if(bytes && size)
+	{
+		size_t maxlen = std::min(size, size_t(8));
+		std::vector<unsigned char> signature(maxlen);
+		memcpy(&signature[0], bytes, maxlen);
+		if (std::string(signature.begin(), signature.end()) == "DEPTHRVL")
+		{
+			format = ".rvl";
+		}
+		else
+		{
+			// Assuming png by default
+			format = ".png";
+		}
+	}
+	return format;
+}
+
 } /* namespace rtabmap */

corelib/src/DBDriver.cpp

@@ -512,12 +512,13 @@ void DBDriver::updateCalibration(int nodeId, const std::vector<CameraModel> & mo
 	_dbSafeAccessMutex.unlock();
 }
 
-void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image)
+void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image, const std::string & format)
 {
 	_dbSafeAccessMutex.lock();
 	this->updateDepthImageQuery(
 			nodeId,
-			image);
+			image,
+			format);
 	_dbSafeAccessMutex.unlock();
 }
 

corelib/src/DBDriverSqlite3.cpp

@@ -4719,7 +4719,8 @@ void DBDriverSqlite3::updateCalibrationQuery(
 
 void DBDriverSqlite3::updateDepthImageQuery(
 		int nodeId,
-		const cv::Mat & image) const
+		const cv::Mat & image,
+		const std::string & format) const
 {
 	UDEBUG("");
 	if(_ppDb)
@@ -4738,7 +4739,8 @@ void DBDriverSqlite3::updateDepthImageQuery(
 		// Save depth
 		stepDepthUpdate(ppStmt,
 				nodeId,
-				image);
+				image,
+				format);
 
 		// Finalize (delete) the statement
 		rc = sqlite3_finalize(ppStmt);
@@ -6009,7 +6011,7 @@ std::string DBDriverSqlite3::queryStepDepthUpdate() const
 		return "UPDATE Data SET depth=? WHERE id=?;";
 	}
 }
-void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image) const
+void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & image, const std::string & format) const
 {
 	if(!ppStmt)
 	{
@@ -6023,7 +6025,7 @@ void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const c
 	if(!image.empty() && (image.type()!=CV_8UC1 || image.rows > 1))
 	{
 		// compress
-		imageCompressed = compressImage2(image, ".png");
+		imageCompressed = compressImage2(image, format);
 	}
 	else
 	{