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/include/rtabmap/gui/CloudViewer.h

@@ -334,6 +334,9 @@ public:
 
 	bool getPose(const std::string & id, Transform & pose); //including meshes
 	bool getCloudVisibility(const std::string & id);
+	int getCloudColorIndex(const std::string & id) const;
+	double getCloudOpacity(const std::string & id) const;
+	int getCloudPointSize(const std::string & id) const;
 
 	const QMap<std::string, Transform> & getAddedClouds() const {return _addedClouds;} //including meshes
 	const QColor & getDefaultBackgroundColor() const;
@@ -399,6 +402,12 @@ public:
 	void setIntensityRedColormap(bool value);
 	void setIntensityRainbowColormap(bool value);
 	void setIntensityMax(float value);
+	float getCloudColorRangeMin() const;
+	float getCloudColorRangeMax() const;
+	bool isCloudColorRangeInverted() const;
+	void setCloudColorRangeMin(float value);
+	void setCloudColorRangeMax(float value);
+	void setCloudColorRangeInverted(bool enabled);
 	void buildPickingLocator(bool enable);
 	const std::map<std::string, vtkSmartPointer<vtkOBBTree> > & getLocators() const {return _locators;}
 
@@ -454,6 +463,10 @@ private:
 	QAction * _aSetIntensityRedColormap;
 	QAction * _aSetIntensityRainbowColormap;
 	QAction * _aSetIntensityMaximum;
+	QAction * _aSetCloudColorRangeMin;
+	QAction * _aSetCloudColorRangeMax;
+	QAction * _aCloudColorRangeInverted;
+	QAction * _aClearCloudColorRanges;
     QAction * _aSetBackgroundColor;
     QAction * _aSetRenderingRate;
     QAction * _aSetEDLShading;
@@ -494,6 +507,8 @@ private:
     double _renderingRate;
     vtkProp * _octomapActor;
     float _intensityAbsMax;
+	float _cloudColorRangeMin;
+	float _cloudColorRangeMax;
     double  _coordinateFrameScale;
 };
 

guilib/include/rtabmap/gui/ImageView.h

@@ -77,6 +77,7 @@ public:
 	float getDepthColorMapMinRange() const;
 	float getDepthColorMapMaxRange() const;
 	uCvQtDepthColorMap getDepthColorMap() const;
+	bool isDepthColorMapInCameraFrame() const;
 
 	float viewScale() const;
 
@@ -94,6 +95,7 @@ public:
 	void setDefaultMatchingLineColor(const QColor & color);
 	void setBackgroundColor(const QColor & color);
 	void setDepthColorMapRange(float min, float max);
+	void setDepthColorMapInCameraFrame(bool enabled);
 
 	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);
@@ -167,6 +169,7 @@ private:
 	QAction * _colorMapBlackToWhite;
 	QAction * _colorMapRedToBlue;
 	QAction * _colorMapBlueToRed;
+	QAction * _colorMapInCameraFrame;
 	QAction * _colorMapMinRange;
 	QAction * _colorMapMaxRange;
 	QAction * _mouseTracking;

guilib/include/rtabmap/gui/PointCloudColorHandlerIntensityField.h

@@ -0,0 +1,186 @@
+/*
+Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+	* Redistributions of source code must retain the above copyright
+	  notice, this list of conditions and the following disclaimer.
+	* Redistributions in binary form must reproduce the above copyright
+	  notice, this list of conditions and the following disclaimer in the
+	  documentation and/or other materials provided with the distribution.
+	* Neither the name of the Universite de Sherbrooke nor the
+	  names of its contributors may be used to endorse or promote products
+	  derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef GUILIB_SRC_POINTCLOUDCOLORHANDLEINTENSITYFIELD_H_
+#define GUILIB_SRC_POINTCLOUDCOLORHANDLEINTENSITYFIELD_H_
+
+#include <pcl/visualization/point_cloud_color_handlers.h>
+#include <pcl/pcl_config.h>
+#include <rtabmap/core/util2d.h>
+#include <rtabmap/utilite/UMath.h>
+
+namespace rtabmap
+{
+
+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:
+	/** \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
+};
+
+} /* namespace rtabmap */
+
+#endif /* GUILIB_SRC_POINTCLOUDCOLORHANDLEINTENSITYFIELD_H_ */

guilib/include/rtabmap/gui/PointCloudColorHandlerMinMaxGenericField.h

@@ -0,0 +1,187 @@
+/*
+Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+	* Redistributions of source code must retain the above copyright
+	  notice, this list of conditions and the following disclaimer.
+	* Redistributions in binary form must reproduce the above copyright
+	  notice, this list of conditions and the following disclaimer in the
+	  documentation and/or other materials provided with the distribution.
+	* Neither the name of the Universite de Sherbrooke nor the
+	  names of its contributors may be used to endorse or promote products
+	  derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef GUILIB_SRC_POINTCLOUDCOLORHANDLEMINMAXGENERICFIELD_H_
+#define GUILIB_SRC_POINTCLOUDCOLORHANDLEMINMAXGENERICFIELD_H_
+
+#include <limits>
+#include <pcl/visualization/point_cloud_color_handlers.h>
+#include <pcl/pcl_config.h>
+
+namespace rtabmap
+{
+
+	/// Same than pcl::visualization::PointCloudColorHandlerGenericField but with min and max parameters
+	class PointCloudColorHandlerMinMaxGenericField : public pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>
+	{
+		using PointCloud = typename PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloud;
+		using PointCloudPtr = typename PointCloud::Ptr;
+		using PointCloudConstPtr = typename PointCloud::ConstPtr;
+
+	public:
+		/** \brief Constructor. */
+		PointCloudColorHandlerMinMaxGenericField(const PointCloudConstPtr &cloud,
+												 const std::string &field_name,
+												 float min = std::numeric_limits<float>::lowest(),
+												 float max = std::numeric_limits<float>::max(),
+												 bool inverted_color_scale = false)
+			: pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>(cloud),
+			  field_name_(field_name),
+			  min_(min),
+			  max_(max),
+			  inverted_color_scale_(inverted_color_scale)
+		{
+			setInputCloud(cloud);
+		}
+
+		/** \brief Destructor. */
+		virtual ~PointCloudColorHandlerMinMaxGenericField() {}
+
+		/** \brief Get the name of the field used. */
+		virtual std::string getFieldName() const { return (field_name_); }
+
+#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<vtkFloatArray>::New ();
+			scalars->SetNumberOfComponents(1);
+
+			vtkIdType nr_points = cloud_->width * cloud_->height;
+			scalars->SetNumberOfTuples(nr_points);
+
+			float *colors = new float[nr_points];
+			float field_data;
+			int j = 0;
+			int point_offset = cloud_->fields[field_idx_].offset;
+
+			// 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;
+				int 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)
+				{
+					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;
+
+					// Copy the value at the specified field
+					memcpy(&field_data, &cloud_->data[point_offset], pcl::getFieldSize(cloud_->fields[field_idx_].datatype));
+					if(field_data < min_) {
+						field_data = min_;
+					}
+					if(field_data > max_) {
+						field_data = max_;
+					}
+					colors[j] = field_data * (inverted_color_scale_?-1.0f:1.0f);
+					j++;
+				}
+			}
+			// 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 (&field_data, &cloud_->data[point_offset], sizeof (float));
+					memcpy(&field_data, &cloud_->data[point_offset], pcl::getFieldSize(cloud_->fields[field_idx_].datatype));
+
+					if (!std::isfinite(field_data))
+						continue;
+
+					if(field_data < min_) {
+						field_data = min_;
+					}
+					if(field_data > max_) {
+						field_data = max_;
+					}
+					
+					colors[j] = field_data * (inverted_color_scale_?-1.0f:1.0f);
+					j++;
+				}
+			}
+			reinterpret_cast<vtkFloatArray *>(&(*scalars))->SetArray(colors, j, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
+
+#if PCL_VERSION_COMPARE(>, 1, 11, 1)
+			return scalars;
+#else
+			return (true);
+#endif
+		}
+
+		using PointCloudColorHandler<pcl::PCLPointCloud2>::getColor;
+
+		/** \brief Set the input cloud to be used.
+		 * \param[in] cloud the input cloud to be used by the handler
+		 */
+		virtual void
+		setInputCloud(const PointCloudConstPtr &cloud)
+		{
+			PointCloudColorHandler<pcl::PCLPointCloud2>::setInputCloud(cloud);
+			field_idx_ = pcl::getFieldIndex(*cloud, field_name_);
+			capable_ = field_idx_ != -1;
+			if (field_idx_ != -1 && cloud_->fields[field_idx_].datatype != pcl::PCLPointField::PointFieldTypes::FLOAT32)
+			{
+				capable_ = false;
+				PCL_ERROR("[pcl::PointCloudColorHandlerGenericField] This currently only works with float32 fields, but field %s has a different type.\n", field_name_.c_str());
+			}
+		}
+
+	protected:
+		/** \brief Class getName method. */
+		virtual std::string
+		getName() const { return ("PointCloudColorHandlerMinMaxGenericField"); }
+
+	private:
+		/** \brief Name of the field used to create the color handler. */
+		std::string field_name_;
+		float min_;
+		float max_;
+		bool inverted_color_scale_;
+	};
+
+} /* namespace rtabmap */
+
+#endif /* GUILIB_SRC_POINTCLOUDCOLORHANDLEMINMAXGENERICFIELD_H_ */

guilib/include/rtabmap/utilite/UCv2Qt.h

@@ -117,7 +117,7 @@ inline QImage uCvMat2QImage(
 		// Assume depth image (float in meters)
 		const float * data = (const float *)image.data;
 		float min,max;
-		if(depthMax>depthMin)
+		if(depthMin != 0 && depthMax != 0 && depthMax > depthMin)
 		{
 			min = depthMin;
 			max = depthMax;
@@ -127,23 +127,23 @@ inline QImage uCvMat2QImage(
 			min = max = data[0];
 			for(unsigned int i=1; i<image.total(); ++i)
 			{
-				if(uIsFinite(data[i]) && data[i] > 0)
+				if(uIsFinite(data[i]) && data[i] != 0)
 				{
-					if(!uIsFinite(min) || (data[i] > 0 && data[i]<min))
+					if(!uIsFinite(min) || (data[i] != 0 && data[i]<min))
 					{
 						min = data[i];
 					}
-					if(!uIsFinite(max) || (data[i] > 0 && data[i]>max))
+					if(!uIsFinite(max) || (data[i] != 0 && data[i]>max))
 					{
 						max = data[i];
 					}
 				}
 			}
-			if(depthMax > 0 && depthMax > depthMin)
+			if(depthMax != 0 && depthMax > depthMin)
 			{
 				max = depthMax;
 			}
-			if(depthMin>0 && (depthMin < depthMax || depthMin < max))
+			if(depthMin != 0 && (depthMin < depthMax || depthMin < max))
 			{
 				min = depthMin;
 			}
@@ -198,7 +198,7 @@ inline QImage uCvMat2QImage(
 		// Assume depth image (unsigned short in mm)
 		const unsigned short * data = (const unsigned short *)image.data;
 		unsigned short min,max;
-		if(depthMax>depthMin)
+		if(depthMin != 0 && depthMax != 0 && depthMax > depthMin)
 		{
 			min = depthMin*1000;
 			max = depthMax*1000;
@@ -208,23 +208,23 @@ inline QImage uCvMat2QImage(
 			min = max = data[0];
 			for(unsigned int i=1; i<image.total(); ++i)
 			{
-				if(uIsFinite(data[i]) && data[i] > 0)
+				if(uIsFinite(data[i]) && data[i] != 0)
 				{
-					if(!uIsFinite(min) || (data[i] > 0 && data[i]<min))
+					if(!uIsFinite(min) || (data[i] != 0 && data[i]<min))
 					{
 						min = data[i];
 					}
-					if(!uIsFinite(max) || (data[i] > 0 && data[i]>max))
+					if(!uIsFinite(max) || (data[i] != 0 && data[i]>max))
 					{
 						max = data[i];
 					}
 				}
 			}
-			if(depthMax > 0 && depthMax > depthMin)
+			if(depthMax != 0 && depthMax > depthMin)
 			{
 				max = depthMax*1000;
 			}
-			if(depthMin>0 && (depthMin < depthMax || depthMin*1000 < max))
+			if(depthMin != 0 && (depthMin < depthMax || depthMin*1000 < max))
 			{
 				min = depthMin*1000;
 			}

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..."));
+	_colorMapInCameraFrame = colorMap->addAction(tr("Camera Frame"));
-	_colorMapMaxRange = colorMap->addAction(tr("Max Range..."));
+	_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()));
 }