ImageView: Depth color map in base frame (#1553)

* ImageView: Depth colormap in base frame option. CloudViewer: added min/max range options for XYZ axes coloring.

* CloudViewer: saving rendering options of the 3D view, re-apply current color index when moving through nodes.

* reset color inverted when reset

guilib/src/CloudViewer.cpp

@@ -27,6 +27,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/gui/CloudViewer.h"
 #include "rtabmap/gui/CloudViewerCellPicker.h"
+#include "rtabmap/gui/PointCloudColorHandlerIntensityField.h"
+#include "rtabmap/gui/PointCloudColorHandlerMinMaxGenericField.h"
 
 #include <rtabmap/core/Version.h>
 #include <rtabmap/core/util3d_transforms.h>
@@ -150,6 +152,8 @@ CloudViewer::CloudViewer(QWidget *parent, CloudViewerInteractorStyle * style) :
 		_renderingRate(5.0),
 		_octomapActor(0),
 		_intensityAbsMax(100.0f),
+		_cloudColorRangeMin(0.0f),
+		_cloudColorRangeMax(0.0f),
 		_coordinateFrameScale(1.0)
 {
 	this->setMinimumSize(200, 200);
@@ -337,6 +341,12 @@ void CloudViewer::createMenu()
 	_aSetIntensityRainbowColormap->setCheckable(true);
 	_aSetIntensityRainbowColormap->setChecked(false);
 	_aSetIntensityMaximum = new QAction("Set maximum absolute intensity...", this);
+	_aSetCloudColorRangeMin = new QAction("Set minimum color range...", this);
+	_aSetCloudColorRangeMax = new QAction("Set maximum color range...", this);
+	_aCloudColorRangeInverted = new QAction("Inverted color scale", this);
+	_aCloudColorRangeInverted->setCheckable(true);
+	_aCloudColorRangeInverted->setChecked(false);
+	_aClearCloudColorRanges = new QAction("Reset ranges", this);
 	_aSetBackgroundColor = new QAction("Set background color...", this);	
 	_aSetRenderingRate = new QAction("Set rendering rate...", this);
 	_aSetEDLShading = new QAction("Eye-Dome Lighting Shading", this);
@@ -402,6 +412,12 @@ void CloudViewer::createMenu()
 	scanMenu->addAction(_aSetIntensityRainbowColormap);
 	scanMenu->addAction(_aSetIntensityMaximum);
 
+	QMenu * cloudMenu = new QMenu("XYZ color", this);
+	cloudMenu->addAction(_aSetCloudColorRangeMin);
+	cloudMenu->addAction(_aSetCloudColorRangeMax);
+	cloudMenu->addAction(_aCloudColorRangeInverted);
+	cloudMenu->addAction(_aClearCloudColorRanges);
+
 	//menus
 	_menu = new QMenu(this);
 	_menu->addMenu(cameraMenu);
@@ -412,6 +428,7 @@ void CloudViewer::createMenu()
 	_menu->addMenu(gridMenu);
 	_menu->addMenu(normalsMenu);
 	_menu->addMenu(scanMenu);
+	_menu->addMenu(cloudMenu);
 	_menu->addAction(_aSetBackgroundColor);
 	_menu->addAction(_aSetRenderingRate);
 	_menu->addAction(_aSetEDLShading);
@@ -465,6 +482,10 @@ void CloudViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("intensity_rainbow_colormap", this->isIntensityRainbowColormap());
 	settings.setValue("intensity_max", (double)this->getIntensityMax());
 
+	settings.setValue("color_range_min", (double)this->getCloudColorRangeMin());
+	settings.setValue("color_range_max", (double)this->getCloudColorRangeMax());
+	settings.setValue("color_range_inverted", (double)this->isCloudColorRangeInverted());
+
 	settings.setValue("trajectory_shown", this->isTrajectoryShown());
 	settings.setValue("trajectory_size", this->getTrajectorySize());
 
@@ -516,6 +537,10 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setIntensityRainbowColormap(settings.value("intensity_rainbow_colormap", this->isIntensityRainbowColormap()).toBool());
 	this->setIntensityMax(settings.value("intensity_max", this->getIntensityMax()).toFloat());
 
+	this->setCloudColorRangeMin(settings.value("color_range_min", this->getCloudColorRangeMin()).toFloat());
+	this->setCloudColorRangeMax(settings.value("color_range_max", this->getCloudColorRangeMax()).toFloat());
+	this->setCloudColorRangeInverted(settings.value("color_range_inverted", this->isCloudColorRangeInverted()).toBool());
+
 	this->setTrajectoryShown(settings.value("trajectory_shown", this->isTrajectoryShown()).toBool());
 	this->setTrajectorySize(settings.value("trajectory_size", this->getTrajectorySize()).toUInt());
 
@@ -606,153 +631,6 @@ bool CloudViewer::updateCloudPose(
 	return false;
 }
 
-class PointCloudColorHandlerIntensityField : public pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>
-{
-	typedef pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloud PointCloud;
-	typedef PointCloud::Ptr PointCloudPtr;
-	typedef PointCloud::ConstPtr PointCloudConstPtr;
-
-public:
-	typedef boost::shared_ptr<PointCloudColorHandlerIntensityField > Ptr;
-	typedef boost::shared_ptr<const PointCloudColorHandlerIntensityField > ConstPtr;
-
-	/** \brief Constructor. */
-	PointCloudColorHandlerIntensityField (const PointCloudConstPtr &cloud, float maxAbsIntensity = 0.0f, int colorMap = 0) :
-		pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloudColorHandler (cloud),
-		maxAbsIntensity_(maxAbsIntensity),
-		colormap_(colorMap)
-		{
-		field_idx_  = pcl::getFieldIndex (*cloud, "intensity");
-		if (field_idx_ != -1)
-			capable_ = true;
-		else
-			capable_ = false;
-		}
-
-	/** \brief Empty destructor */
-	virtual ~PointCloudColorHandlerIntensityField () {}
-
-	/** \brief Obtain the actual color for the input dataset as vtk scalars.
-	 * \param[out] scalars the output scalars containing the color for the dataset
-	 * \return true if the operation was successful (the handler is capable and
-	 * the input cloud was given as a valid pointer), false otherwise
-	 */
-#if PCL_VERSION_COMPARE(>, 1, 11, 1)
-	virtual vtkSmartPointer<vtkDataArray> getColor () const {
-		vtkSmartPointer<vtkDataArray> scalars;
-		if (!capable_ || !cloud_)
-			return scalars;
-#else
-	virtual bool getColor (vtkSmartPointer<vtkDataArray> &scalars) const {
-		if (!capable_ || !cloud_)
-			return (false);
-#endif
-		if (!scalars)
-			scalars = vtkSmartPointer<vtkUnsignedCharArray>::New ();
-		scalars->SetNumberOfComponents (3);
-
-		vtkIdType nr_points = cloud_->width * cloud_->height;
-		// Allocate enough memory to hold all colors
-		float * intensities = new float[nr_points];
-		float intensity;
-		size_t point_offset = cloud_->fields[field_idx_].offset;
-		size_t j = 0;
-
-		// If XYZ present, check if the points are invalid
-		int x_idx = pcl::getFieldIndex (*cloud_, "x");
-		if (x_idx != -1)
-		{
-			float x_data, y_data, z_data;
-			size_t x_point_offset = cloud_->fields[x_idx].offset;
-
-			// Color every point
-			for (vtkIdType cp = 0; cp < nr_points; ++cp,
-			point_offset += cloud_->point_step,
-			x_point_offset += cloud_->point_step)
-			{
-				// Copy the value at the specified field
-				memcpy (&intensity, &cloud_->data[point_offset], sizeof (float));
-
-				memcpy (&x_data, &cloud_->data[x_point_offset], sizeof (float));
-				memcpy (&y_data, &cloud_->data[x_point_offset + sizeof (float)], sizeof (float));
-				memcpy (&z_data, &cloud_->data[x_point_offset + 2 * sizeof (float)], sizeof (float));
-
-				if (!std::isfinite (x_data) || !std::isfinite (y_data) || !std::isfinite (z_data))
-					continue;
-
-				intensities[j++] = intensity;
-			}
-		}
-		// No XYZ data checks
-		else
-		{
-			// Color every point
-			for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step)
-			{
-				// Copy the value at the specified field
-				memcpy (&intensity, &cloud_->data[point_offset], sizeof (float));
-
-				intensities[j++] = intensity;
-			}
-		}
-		if (j != 0)
-		{
-			// Allocate enough memory to hold all colors
-			unsigned char* colors = new unsigned char[j * 3];
-			float min, max;
-			if(maxAbsIntensity_>0.0f)
-			{
-				max = maxAbsIntensity_;
-			}
-			else
-			{
-				uMinMax(intensities, j, min, max);
-			}
-			for(size_t k=0; k<j; ++k)
-			{
-				colors[k*3+0] = colors[k*3+1] = colors[k*3+2] = max>0?(unsigned char)(std::min(intensities[k]/max*255.0f, 255.0f)):255;
-				if(colormap_ == 1)
-				{
-					colors[k*3+0] = 255;
-					colors[k*3+2] = 0;
-				}
-				else if(colormap_ == 2)
-				{
-					float r,g,b;
-					util2d::HSVtoRGB(&r, &g, &b, colors[k*3+0]*299.0f/255.0f, 1.0f, 1.0f);
-					colors[k*3+0] = r*255.0f;
-					colors[k*3+1] = g*255.0f;
-					colors[k*3+2] = b*255.0f;
-				}
-			}
-			reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (j);
-			reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetArray (colors, j*3, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE);
-		}
-		else
-			reinterpret_cast<vtkUnsignedCharArray*>(&(*scalars))->SetNumberOfTuples (0);
-		//delete [] colors;
-		delete [] intensities;
-#if PCL_VERSION_COMPARE(>, 1, 11, 1)
-		return scalars;
-#else
-		return (true);
-#endif
-	}
-
-protected:
-	/** \brief Get the name of the class. */
-	virtual std::string
-	getName () const { return ("PointCloudColorHandlerIntensityField"); }
-
-	/** \brief Get the name of the field used. */
-	virtual std::string
-	getFieldName () const { return ("intensity"); }
-
-private:
-	float maxAbsIntensity_;
-	int colormap_; // 0=grayscale, 1=redYellow, 2=RainbowHSV
-};
-
 bool CloudViewer::addCloud(
 		const std::string & id,
 		const pcl::PCLPointCloud2Ptr & binaryCloud,
@@ -799,11 +677,20 @@ bool CloudViewer::addCloud(
 		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 
 		// x,y,z
-		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "x"));
+		colorHandler.reset (new PointCloudColorHandlerMinMaxGenericField (binaryCloud, "x",
+			_cloudColorRangeMin==0.0f?std::numeric_limits<float>::lowest():_cloudColorRangeMin,
+			_cloudColorRangeMax==0.0f?std::numeric_limits<float>::max():_cloudColorRangeMax,
+			_aCloudColorRangeInverted->isChecked()));
 		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
-		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "y"));
+		colorHandler.reset (new PointCloudColorHandlerMinMaxGenericField (binaryCloud, "y",
+			_cloudColorRangeMin==0.0f?std::numeric_limits<float>::lowest():_cloudColorRangeMin,
+			_cloudColorRangeMax==0.0f?std::numeric_limits<float>::max():_cloudColorRangeMax,
+			_aCloudColorRangeInverted->isChecked()));
 		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
-		colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "z"));
+		colorHandler.reset (new PointCloudColorHandlerMinMaxGenericField (binaryCloud, "z",
+			_cloudColorRangeMin==0.0f?std::numeric_limits<float>::lowest():_cloudColorRangeMin,
+			_cloudColorRangeMax==0.0f?std::numeric_limits<float>::max():_cloudColorRangeMax,
+			_aCloudColorRangeInverted->isChecked()));
 		_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport);
 
 		if(rgb)
@@ -3285,6 +3172,12 @@ bool CloudViewer::getCloudVisibility(const std::string & id)
 	return false;
 }
 
+
+int CloudViewer::getCloudColorIndex(const std::string & id) const
+{
+	return _visualizer->getColorHandlerIndex(id);
+}
+	
 void CloudViewer::setCloudColorIndex(const std::string & id, int index)
 {
 	if(index>0)
@@ -3293,6 +3186,26 @@ void CloudViewer::setCloudColorIndex(const std::string & id, int index)
 	}
 }
 
+double CloudViewer::getCloudOpacity(const std::string & id) const
+{
+	double opacity = 1.0;
+	if(!_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id))
+	{
+#if VTK_MAJOR_VERSION >= 7
+		pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
+		if (am_it != _visualizer->getShapeActorMap()->end ())
+		{
+			vtkActor* actor = vtkActor::SafeDownCast (am_it->second);
+			if(actor)
+			{
+				opacity = actor->GetProperty ()->GetOpacity ();
+			}
+		}
+#endif
+	}
+	return opacity;
+}
+	
 void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
 {
 	double lastOpacity;
@@ -3320,6 +3233,12 @@ void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
 #endif
 }
 
+int CloudViewer::getCloudPointSize(const std::string & id) const
+{
+	double size = 1.0;
+	_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size, id);
+	return (int)size;
+}
 void CloudViewer::setCloudPointSize(const std::string & id, int size)
 {
 	double lastSize;
@@ -3611,9 +3530,33 @@ void CloudViewer::setIntensityMax(float value)
 	}
 	else
 	{
-		UERROR("Cannot set normals scale < 0, value=%f", value);
+		UERROR("Cannot set intensity < 0, value=%f", value);
 	}
 }
+float CloudViewer::getCloudColorRangeMin() const
+{
+	return _cloudColorRangeMin;
+}
+float CloudViewer::getCloudColorRangeMax() const
+{
+	return _cloudColorRangeMax;
+}
+bool CloudViewer::isCloudColorRangeInverted() const
+{
+	return _aCloudColorRangeInverted->isChecked();
+}
+void CloudViewer::setCloudColorRangeMin(float value)
+{
+	_cloudColorRangeMin = value;
+}
+void CloudViewer::setCloudColorRangeMax(float value)
+{
+	_cloudColorRangeMax = value;
+}
+void CloudViewer::setCloudColorRangeInverted(bool enabled)
+{
+	_aCloudColorRangeInverted->setChecked(enabled);
+}
 
 void CloudViewer::buildPickingLocator(bool enable)
 {
@@ -3984,6 +3927,30 @@ void CloudViewer::handleAction(QAction * a)
 	{
 		this->setIntensityRainbowColormap(_aSetIntensityRainbowColormap->isChecked());
 	}
+	else if(a == _aSetCloudColorRangeMin)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Set minimum axis color range"), tr("Range (0=auto)"), _cloudColorRangeMin, -99999, 99999, 2, &ok);
+		if(ok)
+		{
+			this->setCloudColorRangeMin(value);
+		}
+	}
+	else if(a == _aSetCloudColorRangeMax)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Set maximum axis color range"), tr("Range (0=auto)"), _cloudColorRangeMax, -99999, 99999, 2, &ok);
+		if(ok)
+		{
+			this->setCloudColorRangeMax(value);
+		}
+	}
+	else if(a == _aClearCloudColorRanges)
+	{
+		_cloudColorRangeMin = 0.0f;
+		_cloudColorRangeMax = 0.0f;
+		_aCloudColorRangeInverted->setChecked(false);
+	}
 	else if(a == _aSetBackgroundColor)
 	{
 		QColor color = this->getDefaultBackgroundColor();

guilib/src/DatabaseViewer.cpp

@@ -445,6 +445,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
 	connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
 	connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
+	connect(cloudViewer_, SIGNAL(configChanged()), this, SLOT(configModified()));
 	connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
 	connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
 	connect(ui_->actionConcise_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
@@ -653,6 +654,9 @@ void DatabaseViewer::readSettings()
 	ui_->graphicsView_A->loadSettings(settings, "ImageViewA");
 	ui_->graphicsView_B->loadSettings(settings, "ImageViewB");
 
+	// CloudViewer
+	cloudViewer_->loadSettings(settings, "CloudViewer");
+
 	// ICP parameters
 	settings.beginGroup("icp");
 	ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
@@ -747,6 +751,9 @@ void DatabaseViewer::writeSettings()
 	ui_->graphicsView_A->saveSettings(settings, "ImageViewA");
 	ui_->graphicsView_B->saveSettings(settings, "ImageViewB");
 
+	// CloudViewer
+	cloudViewer_->saveSettings(settings, "CloudViewer");
+
 	// save ICP parameters
 	settings.beginGroup("icp");
 	settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
@@ -5138,6 +5145,15 @@ void DatabaseViewer::update(int value,
 					cloudViewer_->removeAllLines();
 					cloudViewer_->removeAllFrustums();
 					cloudViewer_->removeOccupancyGridMap();
+					std::map<std::string, std::pair<int, int> > colorIndexAndPointSizeMap;
+					for(auto iter=cloudViewer_->getAddedClouds().constBegin(); iter!=cloudViewer_->getAddedClouds().constEnd(); ++iter) {
+						if(uStrContains(iter.key(), "cloud") || uStrContains(iter.key(), "scan")) {
+							colorIndexAndPointSizeMap.insert(std::make_pair(iter.key(), 
+								std::make_pair(
+									cloudViewer_->getCloudColorIndex(iter.key())+1,
+									cloudViewer_->getCloudPointSize(iter.key()))));
+						}
+					}
 					cloudViewer_->removeAllClouds();
 					cloudViewer_->removeOctomap();
 					cloudViewer_->removeElevationMap();
@@ -5195,6 +5211,10 @@ void DatabaseViewer::update(int value,
 							pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(laserScanRaw, laserScanRaw.localTransform());
 							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
 						}
+						if(colorIndexAndPointSizeMap.find("scan") != colorIndexAndPointSizeMap.end()) {
+							cloudViewer_->setCloudColorIndex("scan", colorIndexAndPointSizeMap.at("scan").first);
+							cloudViewer_->setCloudPointSize("scan", colorIndexAndPointSizeMap.at("scan").second);
+						}
 					}
 
 					// add RGB-D cloud
@@ -5281,6 +5301,10 @@ void DatabaseViewer::update(int value,
 								}
 
 								cloudViewer_->addCloud("cloud", cloudValidPoints, pose);
+								if(colorIndexAndPointSizeMap.find("cloud") != colorIndexAndPointSizeMap.end()) {
+									cloudViewer_->setCloudColorIndex("cloud", colorIndexAndPointSizeMap.at("cloud").first);
+									cloudViewer_->setCloudPointSize("cloud", colorIndexAndPointSizeMap.at("cloud").second);
+								}
 							}
 							else
 							{
@@ -5404,7 +5428,12 @@ void DatabaseViewer::update(int value,
 										}
 										if(ui_->checkBox_showCloud->isChecked())
 										{
-											cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
+											std::string cloudName = uFormat("cloud_%d", i);
+											cloudViewer_->addCloud(cloudName, cloud, pose);
+											if(colorIndexAndPointSizeMap.find(cloudName) != colorIndexAndPointSizeMap.end()) {
+												cloudViewer_->setCloudColorIndex(cloudName, colorIndexAndPointSizeMap.at(cloudName).first);
+												cloudViewer_->setCloudPointSize(cloudName, colorIndexAndPointSizeMap.at(cloudName).second);
+											}
 										}
 									}
 								}
@@ -5434,7 +5463,12 @@ void DatabaseViewer::update(int value,
 											cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
 										}
 
-										cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
+										std::string cloudName = uFormat("cloud_%d", i);
+										cloudViewer_->addCloud(cloudName, cloud, pose);
+										if(colorIndexAndPointSizeMap.find(cloudName) != colorIndexAndPointSizeMap.end()) {
+											cloudViewer_->setCloudColorIndex(cloudName, colorIndexAndPointSizeMap.at(cloudName).first);
+											cloudViewer_->setCloudPointSize(cloudName, colorIndexAndPointSizeMap.at(cloudName).second);
+										}
 									}
 								}
 							}

guilib/src/ImageView.cpp

@@ -266,14 +266,16 @@ ImageView::ImageView(QWidget * parent) :
 	_colorMapBlueToRed = colorMap->addAction(tr("Blue to red"));
 	_colorMapBlueToRed->setCheckable(true);
 	_colorMapBlueToRed->setChecked(false);
-	_colorMapMinRange = colorMap->addAction(tr("Min Range..."));
-	_colorMapMaxRange = colorMap->addAction(tr("Max Range..."));
+	_colorMapInCameraFrame = colorMap->addAction(tr("Camera Frame"));
+	_colorMapInCameraFrame->setCheckable(true);
+	_colorMapInCameraFrame->setChecked(true);
+	_colorMapMinRange = colorMap->addAction(tr("Min Z..."));
+	_colorMapMaxRange = colorMap->addAction(tr("Max Z..."));
 	group = new QActionGroup(this);
 	group->addAction(_colorMapWhiteToBlack);
 	group->addAction(_colorMapBlackToWhite);
 	group->addAction(_colorMapRedToBlue);
 	group->addAction(_colorMapBlueToRed);
-	group->addAction(_colorMapMaxRange);
 	_mouseTracking = _menu->addAction(tr("Show pixel depth"));
 	_mouseTracking->setCheckable(true);
 	_mouseTracking->setChecked(false);
@@ -311,6 +313,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_camera_frame", this->isDepthColorMapInCameraFrame());
 	settings.setValue("colormap_min_range", this->getDepthColorMapMinRange());
 	settings.setValue("colormap_max_range", this->getDepthColorMapMaxRange());
 	if(!group.isEmpty())
@@ -345,6 +348,7 @@ void ImageView::loadSettings(QSettings & settings, const QString & group)
 	_colorMapBlackToWhite->setChecked(colorMap==1);
 	_colorMapRedToBlue->setChecked(colorMap==2);
 	_colorMapBlueToRed->setChecked(colorMap==3);
+	this->setDepthColorMapInCameraFrame(settings.value("colormap_camera_frame", this->isDepthColorMapInCameraFrame()).toBool());
 	this->setDepthColorMapRange(
 			settings.value("colormap_min_range", this->getDepthColorMapMinRange()).toFloat(),
 			settings.value("colormap_max_range", settings.value("colormap_range" /*backward compatibility*/, this->getDepthColorMapMaxRange())).toFloat());
@@ -763,13 +767,20 @@ void ImageView::setBackgroundColor(const QColor & color)
 	}
 }
 
+void ImageView::setDepthColorMapInCameraFrame(bool enabled) {
+	_colorMapInCameraFrame->setChecked(enabled);
+}
+
+bool ImageView::isDepthColorMapInCameraFrame() const {
+	return _colorMapInCameraFrame->isChecked();
+}
+
 void ImageView::setDepthColorMapRange(float min, float max)
 {
 	_depthColorMapMinRange = min;
 	_depthColorMapMaxRange = max;
 }
 
-
 void ImageView::computeScaleOffsets(const QRect & targetRect, float & scale, float & offsetX, float & offsetY) const
 {
 	scale = 1.0f;
@@ -1054,10 +1065,17 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 		}
 		Q_EMIT configChanged();
 	}
+	else if(action == _colorMapInCameraFrame)
+	{
+		if(!_imageDepthCv.empty()) {
+			this->setImageDepth(_imageDepthCv, _imageDepthConfidenceCv);
+		}
+		Q_EMIT configChanged();
+	}
 	else if(action == _colorMapMinRange)
 	{
 		bool ok = false;
-		double value = QInputDialog::getDouble(this, tr("Set depth colormap min range"), tr("Range (m), 0=no limit"), _depthColorMapMinRange, 0, 9999, 1, &ok);
+		double value = QInputDialog::getDouble(this, tr("Set depth colormap min range"), tr("Range (m), 0=no limit"), _depthColorMapMinRange, -9999, 9999, 2, &ok);
 		if(ok)
 		{
 			this->setDepthColorMapRange(value, _depthColorMapMaxRange);
@@ -1070,7 +1088,7 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 	else if(action == _colorMapMaxRange)
 	{
 		bool ok = false;
-		double value = QInputDialog::getDouble(this, tr("Set depth colormap max range"), tr("Range (m), 0=no limit"), _depthColorMapMaxRange, 0, 9999, 1, &ok);
+		double value = QInputDialog::getDouble(this, tr("Set depth colormap max range"), tr("Range (m), 0=no limit"), _depthColorMapMaxRange, -9999, 9999, 2, &ok);
 		if(ok)
 		{
 			this->setDepthColorMapRange(_depthColorMapMinRange, value);
@@ -1350,8 +1368,61 @@ void ImageView::setImageDepth(const cv::Mat & imageDepth, const cv::Mat & imageD
 {
 	_imageDepthCv = imageDepth;
 	_imageDepthConfidenceCv = imageDepthConfidence;
+
+	QImage depth;
+	if(!_imageDepthCv.empty() && (_imageDepthCv.type() == CV_16UC1 || _imageDepthCv.type() == CV_32FC1)) {
+		if(_colorMapInCameraFrame->isChecked() || _models.empty() || !_models[0].isValidForProjection()) {
+			depth = uCvMat2QImage(_imageDepthCv, true, getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange);
+			if(!_colorMapInCameraFrame->isChecked()) {
+				UWARN("Trying to set depth color map in base frame but the the camera model "
+					  "is not valid for projection, showing depth in camera frame instead.");
+			}
+		}
+		else {
+			// convert the depth values in height values
+			cv::Mat depthInBaseFrame = _imageDepthCv.clone();
+			int subImageWidth = _imageDepthCv.cols / _models.size();
+			if(depthInBaseFrame.type() == CV_16UC1) {
+				for(int v=0; v<depthInBaseFrame.rows; ++v){
+					unsigned short * rowPtr = depthInBaseFrame.ptr<unsigned short>(v);
+					for(int u=0; u<depthInBaseFrame.cols; ++u){
+						unsigned short & val = rowPtr[u];
+						if(val > 0) {
+							cv::Point3f pt;
+							int cameraIndex = u/subImageWidth;
+							UASSERT(cameraIndex>=0 && cameraIndex < (int)_models.size() && subImageWidth == _models[cameraIndex].imageWidth());
+							_models[cameraIndex].project(u,v,float(val)/1000.0f, pt.x, pt.y, pt.z);
+							pt = util3d::transformPoint(pt, _models[cameraIndex].localTransform());
+							val = (unsigned short)(pt.z*1000.0f);
+						}
+					}
+				}
+			}
+			else { // CV_32FC1
+				for(int v=0; v<depthInBaseFrame.rows; ++v){
+					float * rowPtr = depthInBaseFrame.ptr<float>(v);
+					for(int u=0; u<depthInBaseFrame.cols; ++u){
+						float & val = rowPtr[u];
+						if(val > 0) {
+							cv::Point3f pt;
+							int cameraIndex = u/subImageWidth;
+							UASSERT(cameraIndex>=0 && cameraIndex < (int)_models.size() && subImageWidth == _models[cameraIndex].imageWidth());
+							_models[cameraIndex].project(u,v,val, pt.x, pt.y, pt.z);
+							pt = util3d::transformPoint(pt, _models[cameraIndex].localTransform());
+							val = pt.z;
+						}
+					}
+				}
+			}
+			depth = uCvMat2QImage(depthInBaseFrame, true, getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange);
+		}
+	}
+	else {
+		// right image grayscale or color
+		depth = uCvMat2QImage(_imageDepthCv, true, uCvQtDepthBlackToWhite);
+	}
 	setImageDepth(
-		uCvMat2QImage(_imageDepthCv, true, _imageDepthCv.type()==CV_8UC1?uCvQtDepthBlackToWhite:getDepthColorMap(), _depthColorMapMinRange, _depthColorMapMaxRange),
+		depth,
 		uCvMat2QImage(_imageDepthConfidenceCv, true, getDepthColorMap()));
 }