Support Inverted Depth compression (with png or rvl) (#1785)

* Support Inverted Depth compression (with png or rvl)

* Updated parameter's ui description

* ios: fixing minor version bump

* Improved test coverage for this branch

* fixing opencv debug assert

* SensorData: ignore invalid CameraModel

* added lazy decoding check

corelib/src/Compression.cpp

@@ -28,9 +28,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Compression.h"
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
 #include <opencv2/opencv.hpp>
 
 #include <zlib.h>
+#include <cmath>
+#include <cstring>
 
 namespace rtabmap {
 
@@ -67,16 +70,117 @@ int deserializeMatType(int serializedType)
 			((serializedType >> kSerializedCnShift) & 511) + 1);
 }
 
+// Default quantization when only the maximum depth is set in the format.
+const float kDefaultDepthQuantization = 100.0f;
+
+bool hasSignature(const unsigned char * bytes, size_t size, const void * signature)
+{
+	return bytes && size >= 8 && memcmp(bytes, signature, 8) == 0;
+}
+
+// Values over maxDepth, NaN, inf, 0 and negative values are set to 0 (invalid),
+// as well as values too close to be represented on 16 bits (under
+// depthQuantA / (65535 - depthQuantB) meters).
+cv::Mat depthToInvDepth(const cv::Mat & depth, float maxDepth, float quantization, float & depthQuantA, float & depthQuantB)
+{
+	UASSERT(depth.type() == CV_32FC1);
+	depthQuantA = quantization * (quantization + 1.0f);
+	depthQuantB = 1.0f - depthQuantA / maxDepth;
+	cv::Mat invDepth(depth.size(), CV_16UC1);
+	for(int i=0; i<depth.rows; ++i)
+	{
+		const float * in = depth.ptr<float>(i);
+		uint16_t * out = invDepth.ptr<uint16_t>(i);
+		for(int j=0; j<depth.cols; ++j)
+		{
+			const float d = in[j];
+			if(d > 0.0f && d < maxDepth) // false for NaN
+			{
+				// Rounded (ROS truncates), the decoding is the same.
+				const float v = depthQuantA / d + depthQuantB + 0.5f;
+				out[j] = v < 65536.0f ? (uint16_t)v : 0;
+			}
+			else
+			{
+				out[j] = 0;
+			}
+		}
+	}
+	return invDepth;
+}
+
+cv::Mat invDepthToDepth(const cv::Mat & invDepth, float depthQuantA, float depthQuantB)
+{
+	UASSERT(invDepth.type() == CV_16UC1);
+	cv::Mat depth(invDepth.size(), CV_32FC1);
+	for(int i=0; i<invDepth.rows; ++i)
+	{
+		const uint16_t * in = invDepth.ptr<uint16_t>(i);
+		float * out = depth.ptr<float>(i);
+		for(int j=0; j<invDepth.cols; ++j)
+		{
+			out[j] = in[j] ? depthQuantA / (float(in[j]) - depthQuantB) : 0.0f;
+		}
+	}
+	return depth;
+}
+
+void invDepthParameters(float depthQuantA, float depthQuantB, float & maxDepth, float & quantization)
+{
+	// inverse of depthQuantA = q*(q+1) and depthQuantB = 1 - depthQuantA/maxDepth
+	quantization = (std::sqrt(1.0f + 4.0f*depthQuantA) - 1.0f) / 2.0f;
+	maxDepth = depthQuantA / (1.0f - depthQuantB);
+}
+
 }  // namespace
 
-// format : ".jpg" ".png" ".rvl" "" (empty is general)
+bool parseImageCompressionFormat(const std::string & format, std::string & codec, float & maxDepth, float & quantization)
+{
+	codec.clear();
+	maxDepth = 0.0f;
+	quantization = 0.0f;
+	std::vector<std::string> fields = uListToVector(uSplit(format, ':'));
+	if(fields.empty())
+	{
+		return format.empty(); // empty is general (zlib)
+	}
+	if(fields[0].size() < 2 || fields[0][0] != '.')
+	{
+		return false;
+	}
+	if(fields.size() > 1)
+	{
+		// Inverse depth parameters only for formats supporting 16UC1
+		if((fields[0] != ".png" && fields[0] != ".rvl") || fields.size() > 3)
+		{
+			return false;
+		}
+		for(size_t i=1; i<fields.size(); ++i)
+		{
+			if(!uIsNumber(fields[i]) || uStr2Float(fields[i]) <= 0.0f)
+			{
+				return false;
+			}
+		}
+		maxDepth = uStr2Float(fields[1]);
+		quantization = fields.size() == 3 ? uStr2Float(fields[2]) : kDefaultDepthQuantization;
+	}
+	codec = fields[0];
+	return true;
+}
+
+// format : ".jpg" ".png" ".rvl" "" (empty is general), see parseImageCompressionFormat()
 CompressionThread::CompressionThread(const cv::Mat & mat, const std::string & format) :
 	uncompressedData_(mat),
 	format_(format),
 	image_(!format.empty()),
 	compressMode_(true)
 {
-	UASSERT(format.empty() || format.compare(".jpg") == 0 || format.compare(".png") == 0 || format.compare(".rvl") == 0);
+	std::string codec;
+	float maxDepth, quantization;
+	UASSERT_MSG(parseImageCompressionFormat(format, codec, maxDepth, quantization) &&
+			(codec.empty() || codec == ".jpg" || codec == ".png" || codec == ".rvl"),
+			uFormat("Invalid compression format \"%s\"", format.c_str()).c_str());
 }
 // assume image
 CompressionThread::CompressionThread(const cv::Mat & bytes, bool isImage) :
@@ -131,21 +235,43 @@ void CompressionThread::mainLoop()
 	this->kill();
 }
 
-// ".jpg" or ".png" or ".rvl"
+// ".jpg" or ".png" or ".rvl", with optional ":maxDepth[:quantization]" for 32FC1 depth images
 std::vector<unsigned char> compressImage(const cv::Mat & image, const std::string & format)
 {
 	std::vector<unsigned char> bytes;
 	if(!image.empty())
 	{
-		if(image.type() == CV_32FC1)
+		std::string codec;
+		float maxDepth, quantization;
+		if(!parseImageCompressionFormat(format, codec, maxDepth, quantization) || codec.empty())
+		{
+			UERROR("Invalid image compression format \"%s\"", format.c_str());
+			return bytes;
+		}
+
+		if(image.type() == CV_32FC1 && maxDepth > 0.0f)
+		{
+			float depthQuantA, depthQuantB;
+			cv::Mat invDepth = depthToInvDepth(image, maxDepth, quantization, depthQuantA, depthQuantB);
+			std::vector<unsigned char> invDepthBytes = compressImage(invDepth, codec);
+			if(!invDepthBytes.empty())
+			{
+				bytes.resize(kCompressedDepthInvHeaderSize + invDepthBytes.size());
+				memcpy(&bytes[0], kCompressedDepthInvSignature, 8);
+				memcpy(&bytes[8], &depthQuantA, 4);
+				memcpy(&bytes[12], &depthQuantB, 4);
+				memcpy(&bytes[kCompressedDepthInvHeaderSize], invDepthBytes.data(), invDepthBytes.size());
+			}
+		}
+		else if(image.type() == CV_32FC1)
 		{
 			//save in 8bits-4channel
 			cv::Mat bgra(image.size(), CV_8UC4, image.data);
 			cv::imencode(".png", bgra, bytes);
 		}
-		else if(format == ".rvl")
+		else if(codec == ".rvl")
 		{
-			bytes = {'D', 'E', 'P', 'T', 'H', 'R', 'V', 'L'};
+			bytes.assign(kCompressedDepthRvlSignature, kCompressedDepthRvlSignature+8);
 			int numPixels = image.rows * image.cols;
         	// In the worst case, RVL compression results in ~1.5x larger data.
         	bytes.resize(3 * numPixels + 20);
@@ -154,18 +280,18 @@ std::vector<unsigned char> compressImage(const cv::Mat & image, const std::strin
         	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);
+        	int compressedSize = rvl.CompressRVL(image.ptr<uint16_t>(), &bytes[kCompressedDepthRvlHeaderSize], numPixels);
+        	bytes.resize(kCompressedDepthRvlHeaderSize + compressedSize);
 		}
 		else
 		{
-			cv::imencode(format, image, bytes);
+			cv::imencode(codec, image, bytes);
 		}
 	}
 	return bytes;
 }
 
-// ".jpg" or ".png" or ".rvl"
+// ".jpg" or ".png" or ".rvl", with optional ":maxDepth[:quantization]" for 32FC1 depth images
 cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
 {
 	std::vector<unsigned char> bytes = compressImage(image, format);
@@ -178,24 +304,64 @@ cv::Mat compressImage2(const cv::Mat & image, const std::string & format)
 
 cv::Mat uncompressImage(const cv::Mat & bytes)
 {
-	cv::Mat image;
-	if(!bytes.empty())
+	if(bytes.empty())
 	{
-		if (compressedDepthFormat(bytes) == ".rvl")
+		return cv::Mat();
+	}
+	return uncompressImage(bytes.data, bytes.total()*bytes.elemSize());
+}
+
+cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
+{
+	return uncompressImage(bytes.data(), bytes.size());
+}
+
+cv::Mat uncompressImage(const unsigned char * bytes, size_t size)
+{
+	cv::Mat image;
+	if(bytes && size)
+	{
+		if(hasSignature(bytes, size, kCompressedDepthInvSignature))
 		{
+			if(size <= kCompressedDepthInvHeaderSize)
+			{
+				UERROR("Inverse depth image is truncated (%d bytes).", (int)size);
+				return image;
+			}
+			float depthQuantA, depthQuantB;
+			memcpy(&depthQuantA, &bytes[8], 4);
+			memcpy(&depthQuantB, &bytes[12], 4);
+			cv::Mat invDepth = uncompressImage(&bytes[kCompressedDepthInvHeaderSize], size - kCompressedDepthInvHeaderSize);
+			if(invDepth.type() == CV_16UC1)
+			{
+				image = invDepthToDepth(invDepth, depthQuantA, depthQuantB);
+			}
+			else if(!invDepth.empty())
+			{
+				UERROR("Inverse depth image should be 16UC1 (type=%d).", invDepth.type());
+			}
+		}
+		else if(hasSignature(bytes, size, kCompressedDepthRvlSignature))
+		{
+			if(size < kCompressedDepthRvlHeaderSize)
+			{
+				UERROR("RVL depth image is truncated (%d bytes).", (int)size);
+				return image;
+			}
 			uint32_t cols, rows;
-        	memcpy(&cols, &bytes.data[8], 4);
-        	memcpy(&rows, &bytes.data[12], 4);
+        	memcpy(&cols, &bytes[8], 4);
+        	memcpy(&rows, &bytes[12], 4);
 			image = cv::Mat(rows, cols, CV_16UC1);
 			RvlCodec rvl;
-        	rvl.DecompressRVL(&bytes.data[16], image.ptr<uint16_t>(), cols * rows);
+        	rvl.DecompressRVL(&bytes[kCompressedDepthRvlHeaderSize], image.ptr<uint16_t>(), cols * rows);
 		}
 		else
 		{
+			const cv::Mat buf(1, (int)size, CV_8UC1, (void *)bytes);
 #if CV_MAJOR_VERSION>2 || (CV_MAJOR_VERSION >=2 && CV_MINOR_VERSION >=4)
-			image = cv::imdecode(bytes, cv::IMREAD_UNCHANGED);
+			image = cv::imdecode(buf, cv::IMREAD_UNCHANGED);
 #else
-			image = cv::imdecode(bytes, -1);
+			image = cv::imdecode(buf, -1);
 #endif
 			if(image.type() == CV_8UC4)
 			{
@@ -210,36 +376,6 @@ cv::Mat uncompressImage(const cv::Mat & bytes)
 	return image;
 }
 
-cv::Mat uncompressImage(const std::vector<unsigned char> & bytes)
-{
-	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);
-#else
-			image = cv::imdecode(bytes, -1);
-#endif
-			if(image.type() == CV_8UC4)
-			{
-				image = cv::Mat(image.size(), CV_32FC1, image.data).clone();
-			}
-		}
-	}
-	return image;
-}
-
 std::vector<unsigned char> compressData(const cv::Mat & data)
 {
 	std::vector<unsigned char> bytes;
@@ -381,10 +517,17 @@ 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")
+		if(hasSignature(bytes, size, kCompressedDepthInvSignature) && size > kCompressedDepthInvHeaderSize)
+		{
+			float depthQuantA, depthQuantB, maxDepth, quantization;
+			memcpy(&depthQuantA, &bytes[8], 4);
+			memcpy(&depthQuantB, &bytes[12], 4);
+			invDepthParameters(depthQuantA, depthQuantB, maxDepth, quantization);
+			format = uFormat("%s:%g:%g",
+					compressedDepthFormat(&bytes[kCompressedDepthInvHeaderSize], size - kCompressedDepthInvHeaderSize).c_str(),
+					maxDepth, quantization);
+		}
+		else if(hasSignature(bytes, size, kCompressedDepthRvlSignature))
 		{
 			format = ".rvl";
 		}

corelib/src/Memory.cpp

@@ -102,6 +102,7 @@ Memory::Memory(const ParametersMap & parameters) :
     _imagePreDecimation(Parameters::defaultMemImagePreDecimation()),
 	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
 	_legacyDecimatedOctave(false),
+	_inverseDepthCompressionAllowed(true),
 	_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
 	_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
 	_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
@@ -228,6 +229,11 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 			// filling it that way; a new one gets the corrected scaling.
 			_legacyDecimatedOctave =
 					uStrNumCmp(_dbDriver->getDatabaseVersion(), "0.23.12") < 0;
+			// Depth images compressed as inverse depth cannot be read before 0.24, which
+			// would still open databases created with Db/TargetVersion < 0.24 or by an
+			// older version.
+			_inverseDepthCompressionAllowed =
+					uStrNumCmp(_dbDriver->getDatabaseVersion(), "0.24.0") >= 0;
 			// Only where the descriptors stored in the map end up different: keypoints
 			// from odometry, scaled into the pre-decimated image before being described.
 			if(_legacyDecimatedOctave && _useOdometryFeatures && _imagePreDecimation > 1)
@@ -826,6 +832,19 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemIntermediateNodeDataKept(), _saveIntermediateNodeData);
 	Parameters::parse(params, Parameters::kMemImageCompressionFormat(), _rgbCompressionFormat);
 	Parameters::parse(params, Parameters::kMemDepthCompressionFormat(), _depthCompressionFormat);
+	{
+		std::string codec;
+		float maxDepth, quantization;
+		if(!parseImageCompressionFormat(_depthCompressionFormat, codec, maxDepth, quantization) ||
+		   (codec != ".png" && codec != ".rvl"))
+		{
+			UWARN("Invalid %s=\"%s\", using default \"%s\".",
+					Parameters::kMemDepthCompressionFormat().c_str(),
+					_depthCompressionFormat.c_str(),
+					Parameters::defaultMemDepthCompressionFormat().c_str());
+			_depthCompressionFormat = Parameters::defaultMemDepthCompressionFormat();
+		}
+	}
 	Parameters::parse(params, Parameters::kMemRehearsalIdUpdatedToNewOne(), _idUpdatedToNewOneRehearsal);
 	Parameters::parse(params, Parameters::kMemGenerateIds(), _generateIds);
 	Parameters::parse(params, Parameters::kMemBadSignaturesIgnored(), _badSignaturesIgnored);
@@ -5227,7 +5246,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	if(!isIntermediateNode)
 	{
 		// We need raw images if we need to extract features and/or do tag detection
-		bool needRawImages = _feature2D->getMaxFeatures() >= 0 && 
+		bool needRawImages = (_feature2D->getMaxFeatures() >= 0 &&
 			(!_useOdometryFeatures ||
 			 data.keypoints().empty() ||
 			 (int)data.keypoints().size() != data.descriptors().rows ||
@@ -5235,7 +5254,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			 _detectMarkers ||
 			 _rotateImagesUpsideUp ||
 			 _imagePostDecimation > 1 ||
-			 (_createOccupancyGrid && _localMapMaker->isGridFromDepth()));
+			 (_createOccupancyGrid && _localMapMaker->isGridFromDepth()))) ||
+			// Images rectified below: stereo always, RGB-D unless only its features are
+			(!_imagesAlreadyRectified && !(_rectifyOnlyFeatures && data.stereoCameraModels().empty()));
 
 		// Note: we could avoid uncompressing scan if we don't do any filtering
 		// and if we don't use it for local occupancy grid
@@ -6625,6 +6646,44 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		std::vector<unsigned char> imageBytes;
 		std::vector<unsigned char> depthBytes;
 
+		std::string depthCompressionFormat = _depthCompressionFormat;
+		bool reuseCompressedDepth =
+				depthOrRightImage.data == data.depthOrRightRaw().data &&
+				!data.depthOrRightCompressed().empty();
+		if(!_inverseDepthCompressionAllowed)
+		{
+			std::string codec;
+			float maxDepth, quantization;
+			if(parseImageCompressionFormat(depthCompressionFormat, codec, maxDepth, quantization) && maxDepth > 0.0f)
+			{
+				static bool warned = false;
+				if(!warned)
+				{
+					UWARN("%s=\"%s\": inverse depth compression format is not compatible with database "
+						  "version %s (requires >= 0.24, see %s), \"%s\" format is used instead. This "
+						  "warning is only printed once.",
+						Parameters::kMemDepthCompressionFormat().c_str(),
+						depthCompressionFormat.c_str(),
+						_dbDriver?_dbDriver->getDatabaseVersion().c_str():"",
+						Parameters::kDbTargetVersion().c_str(),
+						codec.c_str());
+					warned = true;
+				}
+				depthCompressionFormat = codec;
+			}
+			if(reuseCompressedDepth &&
+			   compressedDepthFormat(data.depthOrRightCompressed()).find(':') != std::string::npos)
+			{
+				// Already compressed as inverse depth (e.g., received from ROS's
+				// compressed_depth_image_transport), re-compress it.
+				reuseCompressedDepth = false;
+				if(depthOrRightImage.empty())
+				{
+					depthOrRightImage = uncompressImage(data.depthOrRightCompressed());
+				}
+			}
+		}
+
 		if(!depthOrRightImage.empty() && depthOrRightImage.type() == CV_32FC1)
 		{
 			if(_saveDepth16Format)
@@ -6640,7 +6699,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				}
 				depthOrRightImage = util2d::cvtDepthFromFloat(depthOrRightImage);
 			}
-			else if(_depthCompressionFormat == ".rvl")
+			else if(depthCompressionFormat == ".rvl")
 			{
 				static bool warned = false;
 				if(!warned)
@@ -6650,13 +6709,16 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						  "images will be compressed in \".png\" format instead. Explicitly "
 						  "set %s to true to keep using \"%s\" format and images will be "
 						  "converted to 16bits for convenience (warning: that would "
-						  "remove all depth values over 65 meters). Explicitly set %s=\".png\" "
+						  "remove all depth values over 65 meters). Set %s=\".rvl:<maxDepth>:<quantization>\" "
+						  "(e.g., \".rvl:10:100\") to compress them in RVL as 16 bits inverse depth "
+						  "(lossy, see parameter's description). Explicitly set %s=\".png\" "
 						  "to suppress this warning. This warning is only printed once.",
 						Parameters::kMemSaveDepth16Format().c_str(),
 						Parameters::kMemDepthCompressionFormat().c_str(),
-						_depthCompressionFormat.c_str(),
+						depthCompressionFormat.c_str(),
 						Parameters::kMemSaveDepth16Format().c_str(),
-						_depthCompressionFormat.c_str(),
+						depthCompressionFormat.c_str(),
+						Parameters::kMemDepthCompressionFormat().c_str(),
 						Parameters::kMemDepthCompressionFormat().c_str());
 					warned = true;
 				}
@@ -6666,9 +6728,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		bool reuseCompressedImage =
 				image.data == data.imageRaw().data &&
 				!data.imageCompressed().empty();
-		bool reuseCompressedDepth =
-				depthOrRightImage.data == data.depthOrRightRaw().data &&
-				!data.depthOrRightCompressed().empty();
+		reuseCompressedDepth = reuseCompressedDepth &&
+				depthOrRightImage.data == data.depthOrRightRaw().data;
 		bool reuseCompressedDepthConfidence =
 				depthConfidence.data == data.depthConfidenceRaw().data &&
 				!data.depthConfidenceCompressed().empty();
@@ -6685,7 +6746,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		if(_compressionParallelized)
 		{
 			rtabmap::CompressionThread ctImage(image, _rgbCompressionFormat);
-			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
+			rtabmap::CompressionThread ctDepth(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?depthCompressionFormat:_rgbCompressionFormat);
 			rtabmap::CompressionThread ctDepthConfidence(depthConfidence);
 			rtabmap::CompressionThread ctLaserScan(laserScan.data());
 			rtabmap::CompressionThread ctUserData(data.userDataRaw());
@@ -6724,7 +6785,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		else
 		{
 			compressedImage = reuseCompressedImage?cv::Mat():compressImage2(image, _rgbCompressionFormat);
-			compressedDepth = reuseCompressedDepth?cv::Mat():compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
+			compressedDepth = reuseCompressedDepth?cv::Mat():compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?depthCompressionFormat:_rgbCompressionFormat);
 			compressedDepthConfidence = reuseCompressedDepthConfidence?cv::Mat():compressData2(depthConfidence);
 			compressedScan = reuseCompressedScan?data.laserScanCompressed().data():compressData2(laserScan.data());
 			compressedUserData = reuseCompressedUserData?data.userDataCompressed():compressData2(data.userDataRaw());

corelib/src/SensorData.cpp

@@ -313,6 +313,24 @@ SensorData::~SensorData()
 {
 }
 
+bool SensorData::keepCameraModel(
+		const CameraModel & model,
+		const cv::Mat & rgb,
+		const cv::Mat & depth,
+		bool clearPreviousData) const
+{
+	// An invalid model without any image is only a placeholder (e.g., scan-only data
+	// created with CameraModel()): it is not kept, so that cameraModels() is empty when
+	// there is no camera. An invalid model with an image is kept: images can be used
+	// without calibration, and they are split per camera model.
+	return model.isValidForProjection() ||
+			!rgb.empty() ||
+			!depth.empty() ||
+			(!clearPreviousData && (
+					!_imageRaw.empty() || !_imageCompressed.empty() ||
+					!_depthOrRightRaw.empty() || !_depthOrRightCompressed.empty()));
+}
+
 void SensorData::setRGBDImage(
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
@@ -320,7 +338,10 @@ void SensorData::setRGBDImage(
 		bool clearPreviousData)
 {
 	std::vector<CameraModel> models;
-	models.push_back(model);
+	if(keepCameraModel(model, rgb, depth, clearPreviousData))
+	{
+		models.push_back(model);
+	}
 	setRGBDImage(rgb, depth, models, clearPreviousData);
 }
 void SensorData::setRGBDImage(
@@ -331,7 +352,10 @@ void SensorData::setRGBDImage(
 	bool clearPreviousData)
 {
 	std::vector<CameraModel> models;
-	models.push_back(model);
+	if(keepCameraModel(model, rgb, depth, clearPreviousData))
+	{
+		models.push_back(model);
+	}
 	setRGBDImage(rgb, depth, depthConfidence, models, clearPreviousData);
 }
 void SensorData::setRGBDImage(

corelib/src/VisualWord.cpp

@@ -73,7 +73,7 @@ unsigned long VisualWord::getMemoryUsed() const
 {
 	unsigned long memoryUsage = sizeof(VisualWord);
 	memoryUsage += _references.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
-	memoryUsage += _descriptor.total() * _descriptor.elemSize();
+	memoryUsage += _descriptor.empty()?0:_descriptor.total() * _descriptor.elemSize();
 	return memoryUsage;
 }