ImageView: added option to set maximum range for depth colormap

guilib/include/rtabmap/gui/ImageView.h

@@ -71,6 +71,7 @@ public:
 	const QColor & getDefaultMatchingFeatureColor() const;
 	const QColor & getDefaultMatchingLineColor() const;
 	const QColor & getBackgroundColor() const;
+	float getDepthColorMapRange() const;
 	uCvQtDepthColorMap getDepthColorMap() const;
 
 	float viewScale() const;
@@ -87,6 +88,7 @@ public:
 	void setDefaultMatchingFeatureColor(const QColor & color);
 	void setDefaultMatchingLineColor(const QColor & color);
 	void setBackgroundColor(const QColor & color);
+	void setDepthColorMapRange(float value);
 
 	void setFeatures(const std::multimap<int, cv::KeyPoint> & refWords, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
 	void setFeatures(const std::vector<cv::KeyPoint> & features, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow);
@@ -133,6 +135,7 @@ private:
 	QColor _defaultFeatureColor;
 	QColor _defaultMatchingFeatureColor;
 	QColor _defaultMatchingLineColor;
+	float _depthColorMapRange;
 
 	QMenu * _menu;
 	QAction * _showImage;
@@ -153,6 +156,7 @@ private:
 	QAction * _colorMapBlackToWhite;
 	QAction * _colorMapRedToBlue;
 	QAction * _colorMapBlueToRed;
+	QAction * _colorMapRange;
 	QMenu * _featureMenu;
 	QMenu * _scaleMenu;
 

guilib/include/rtabmap/utilite/UCv2Qt.h

@@ -39,9 +39,17 @@ enum uCvQtDepthColorMap{
  * depth (float32, uint16) image and RGB/BGR 8bits images.
  * @param image the cv::Mat image (can be 1 channel [CV_8U, CV_16U or CV_32F] or 3 channels [CV_U8])
  * @param isBgr if 3 channels, it is BGR or RGB order.
+ * @param colorMap gradient of color to use to visualize depth
+ * @param depthMin fixed minimum range (m) of the depth gradient (if depthMax<=depthMin, max/min are computed based on data in depth image)
+ * @param depthMax fixed maximum range (m) of the depth gradient (if depthMax<=depthMin, max/min are computed based on data in depth image)
  * @return the QImage
  */
-inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true, uCvQtDepthColorMap colorMap = uCvQtDepthWhiteToBlack)
+inline QImage uCvMat2QImage(
+		const cv::Mat & image,
+		bool isBgr = true,
+		uCvQtDepthColorMap colorMap = uCvQtDepthWhiteToBlack,
+		float depthMin = 0,
+		float depthMax = 0)
 {
 	QImage qtemp;
 	if(!image.empty() && image.depth() == CV_8U)
@@ -87,18 +95,21 @@ inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true, uCvQtDepth
     {
 		// Assume depth image (float in meters)
 		const float * data = (const float *)image.data;
-		float min=data[0], max=data[0];
-		for(unsigned int i=1; i<image.total(); ++i)
+		float min=depthMax>depthMin?depthMin:data[0], max=depthMax>depthMin?depthMax:data[0];
+		if(depthMax <= depthMin)
 		{
-			if(uIsFinite(data[i]) && data[i] > 0)
+			for(unsigned int i=1; i<image.total(); ++i)
 			{
-				if(!uIsFinite(min) || (data[i] > 0 && data[i]<min))
+				if(uIsFinite(data[i]) && data[i] > 0)
 				{
-					min = data[i];
-				}
-				if(!uIsFinite(max) || (data[i] > 0 && data[i]>max))
-				{
-					max = data[i];
+					if(!uIsFinite(min) || (data[i] > 0 && data[i]<min))
+					{
+						min = data[i];
+					}
+					if(!uIsFinite(max) || (data[i] > 0 && data[i]>max))
+					{
+						max = data[i];
+					}
 				}
 			}
 		}
@@ -115,7 +126,7 @@ inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true, uCvQtDepth
 				}
 				else
 				{
-					*p = uchar(255.0f - ((data[x]-min)*255.0f)/(max-min));
+					*p = uchar(std::max(0.0f, std::min(255.0f, 255.0f - ((data[x]-min)*255.0f)/(max-min))));
 					if(*p == 255)
 					{
 						*p = 0;
@@ -147,18 +158,21 @@ inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true, uCvQtDepth
 	{
 		// Assume depth image (unsigned short in mm)
 		const unsigned short * data = (const unsigned short *)image.data;
-		unsigned short min=data[0], max=data[0];
-		for(unsigned int i=1; i<image.total(); ++i)
+		unsigned short min=depthMax>depthMin?(unsigned short)(depthMin*1000):data[0], max=depthMax>depthMin?(unsigned short)(depthMax*1000):data[0];
+		if(depthMax<=depthMin)
 		{
-			if(uIsFinite(data[i]) && data[i] > 0)
+			for(unsigned int i=1; i<image.total(); ++i)
 			{
-				if(!uIsFinite(min) || (data[i] > 0 && data[i]<min))
+				if(uIsFinite(data[i]) && data[i] > 0)
 				{
-					min = data[i];
-				}
-				if(!uIsFinite(max) || (data[i] > 0 && data[i]>max))
-				{
-					max = data[i];
+					if(!uIsFinite(min) || (data[i] > 0 && data[i]<min))
+					{
+						min = data[i];
+					}
+					if(!uIsFinite(max) || (data[i] > 0 && data[i]>max))
+					{
+						max = data[i];
+					}
 				}
 			}
 		}
@@ -175,7 +189,7 @@ inline QImage uCvMat2QImage(const cv::Mat & image, bool isBgr = true, uCvQtDepth
 				}
 				else
 				{
-					*p = uchar(255.0f - (float(data[x]-min)/float(max-min))*255.0f);
+					*p = uchar(std::max(0.0f, std::min(255.0f, 255.0f - (float(data[x]-min)/float(max-min))*255.0f)));
 					if(*p == 255)
 					{
 						*p = 0;

guilib/src/ImageView.cpp

@@ -170,6 +170,7 @@ ImageView::ImageView(QWidget * parent) :
 		_defaultFeatureColor(Qt::yellow),
 		_defaultMatchingFeatureColor(Qt::magenta),
 		_defaultMatchingLineColor(Qt::cyan),
+		_depthColorMapRange(0),
 		_imageItem(0),
 		_imageDepthItem(0)
 {
@@ -251,11 +252,13 @@ ImageView::ImageView(QWidget * parent) :
 	_colorMapBlueToRed = colorMap->addAction(tr("Blue to red"));
 	_colorMapBlueToRed->setCheckable(true);
 	_colorMapBlueToRed->setChecked(false);
+	_colorMapRange = colorMap->addAction(tr("Max Range..."));
 	group = new QActionGroup(this);
 	group->addAction(_colorMapWhiteToBlack);
 	group->addAction(_colorMapBlackToWhite);
 	group->addAction(_colorMapRedToBlue);
 	group->addAction(_colorMapBlueToRed);
+	group->addAction(_colorMapRange);
 	_saveImage = _menu->addAction(tr("Save picture..."));
 	_saveImage->setEnabled(false);
 
@@ -286,6 +289,7 @@ void ImageView::saveSettings(QSettings & settings, const QString & group) const
 	settings.setValue("graphics_view_scale", this->isGraphicsViewScaled());
 	settings.setValue("graphics_view_scale_to_height", this->isGraphicsViewScaledToHeight());
 	settings.setValue("colormap", _colorMapWhiteToBlack->isChecked()?0:_colorMapBlackToWhite->isChecked()?1:_colorMapRedToBlue->isChecked()?2:3);
+	settings.setValue("colormap_range", this->getDepthColorMapRange());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -316,6 +320,7 @@ void ImageView::loadSettings(QSettings & settings, const QString & group)
 	_colorMapBlackToWhite->setChecked(colorMap==1);
 	_colorMapRedToBlue->setChecked(colorMap==2);
 	_colorMapBlueToRed->setChecked(colorMap==3);
+	this->setDepthColorMapRange(settings.value("colormap_range", this->getDepthColorMapRange()).toFloat());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -379,6 +384,11 @@ const QColor & ImageView::getBackgroundColor() const
 	return _graphicsView->backgroundBrush().color();
 }
 
+float ImageView::getDepthColorMapRange() const
+{
+	return _depthColorMapRange;
+}
+
 uCvQtDepthColorMap ImageView::getDepthColorMap() const
 {
 	uCvQtDepthColorMap colorMap = uCvQtDepthWhiteToBlack;
@@ -676,6 +686,12 @@ void ImageView::setBackgroundColor(const QColor & color)
 	}
 }
 
+void ImageView::setDepthColorMapRange(float value)
+{
+	_depthColorMapRange = value;
+}
+
+
 void ImageView::computeScaleOffsets(const QRect & targetRect, float & scale, float & offsetX, float & offsetY) const
 {
 	scale = 1.0f;
@@ -950,6 +966,16 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 			this->setImageDepth(_imageDepthCv);
 		Q_EMIT configChanged();
 	}
+	else if(action == _colorMapRange)
+	{
+		bool ok = false;
+		double value = QInputDialog::getDouble(this, tr("Set depth colormap max range"), tr("Range (m), 0=no limit"), _depthColorMapRange, 0, 9999, 1, &ok);
+		if(ok)
+		{
+			this->setDepthColorMapRange(value);
+			Q_EMIT configChanged();
+		}
+	}
 	else if(action == _setAlpha)
 	{
 		bool ok = false;
@@ -1126,7 +1152,7 @@ void ImageView::setImage(const QImage & image)
 void ImageView::setImageDepth(const cv::Mat & imageDepth)
 {
 	_imageDepthCv = imageDepth;
-	setImageDepth(uCvMat2QImage(_imageDepthCv, true, getDepthColorMap()));
+	setImageDepth(uCvMat2QImage(_imageDepthCv, true, getDepthColorMap(), 0.0f, _depthColorMapRange));
 }
 
 void ImageView::setImageDepth(const QImage & imageDepth)