From f06b7c8b3e56b0cb6f8c44d7d60783342aad6413 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Jul 2025 18:49:48 -0700 Subject: [PATCH] ImageView: Depth colormap in base frame option. CloudViewer: added min/max range options for XYZ axes coloring. --- guilib/include/rtabmap/gui/CloudViewer.h | 12 + guilib/include/rtabmap/gui/ImageView.h | 3 + .../PointCloudColorHandlerIntensityField.h | 186 ++++++++++++++ ...PointCloudColorHandlerMinMaxGenericField.h | 187 ++++++++++++++ guilib/include/rtabmap/utilite/UCv2Qt.h | 24 +- guilib/src/CloudViewer.cpp | 236 +++++++----------- guilib/src/ImageView.cpp | 85 ++++++- 7 files changed, 563 insertions(+), 170 deletions(-) create mode 100644 guilib/include/rtabmap/gui/PointCloudColorHandlerIntensityField.h create mode 100644 guilib/include/rtabmap/gui/PointCloudColorHandlerMinMaxGenericField.h diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index 3f3e2795..594ff195 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -399,6 +399,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 > & getLocators() const {return _locators;} @@ -454,6 +460,10 @@ private: QAction * _aSetIntensityRedColormap; QAction * _aSetIntensityRainbowColormap; QAction * _aSetIntensityMaximum; + QAction * _aSetCloudColorRangeMin; + QAction * _aSetCloudColorRangeMax; + QAction * _aCloudColorRangeInverted; + QAction * _aClearCloudColorRanges; QAction * _aSetBackgroundColor; QAction * _aSetRenderingRate; QAction * _aSetEDLShading; @@ -494,6 +504,8 @@ private: double _renderingRate; vtkProp * _octomapActor; float _intensityAbsMax; + float _cloudColorRangeMin; + float _cloudColorRangeMax; double _coordinateFrameScale; }; diff --git a/guilib/include/rtabmap/gui/ImageView.h b/guilib/include/rtabmap/gui/ImageView.h index 6ff0c6da..8e8df852 100644 --- a/guilib/include/rtabmap/gui/ImageView.h +++ b/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 & refWords, const cv::Mat & depth = cv::Mat(), const QColor & color = Qt::yellow); void setFeatures(const std::vector & 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; diff --git a/guilib/include/rtabmap/gui/PointCloudColorHandlerIntensityField.h b/guilib/include/rtabmap/gui/PointCloudColorHandlerIntensityField.h new file mode 100644 index 00000000..88254b57 --- /dev/null +++ b/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 +#include +#include +#include + +namespace rtabmap +{ + +class PointCloudColorHandlerIntensityField : public pcl::visualization::PointCloudColorHandler +{ + typedef pcl::visualization::PointCloudColorHandler::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::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 getColor() const + { + vtkSmartPointer scalars; + if (!capable_ || !cloud_) + return scalars; +#else + virtual bool getColor(vtkSmartPointer &scalars) const + { + if (!capable_ || !cloud_) + return (false); +#endif + if (!scalars) + scalars = vtkSmartPointer::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(&(*scalars))->SetNumberOfTuples(j); + reinterpret_cast(&(*scalars))->SetArray(colors, j * 3, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE); + } + else + reinterpret_cast(&(*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_ */ \ No newline at end of file diff --git a/guilib/include/rtabmap/gui/PointCloudColorHandlerMinMaxGenericField.h b/guilib/include/rtabmap/gui/PointCloudColorHandlerMinMaxGenericField.h new file mode 100644 index 00000000..2feb11b7 --- /dev/null +++ b/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 +#include +#include + +namespace rtabmap +{ + + /// Same than pcl::visualization::PointCloudColorHandlerGenericField but with min and max parameters + class PointCloudColorHandlerMinMaxGenericField : public pcl::visualization::PointCloudColorHandler + { + using PointCloud = typename PointCloudColorHandler::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::lowest(), + float max = std::numeric_limits::max(), + bool inverted_color_scale = false) + : pcl::visualization::PointCloudColorHandler(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 getColor() const + { + vtkSmartPointer scalars; + if (!capable_ || !cloud_) + return scalars; +#else + virtual bool getColor(vtkSmartPointer &scalars) const + { + if (!capable_ || !cloud_) + return (false); +#endif + if (!scalars) + scalars = vtkSmartPointer::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(&(*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::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::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_ */ diff --git a/guilib/include/rtabmap/utilite/UCv2Qt.h b/guilib/include/rtabmap/utilite/UCv2Qt.h index da99bc00..cf2f66f1 100644 --- a/guilib/include/rtabmap/utilite/UCv2Qt.h +++ b/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 0) + if(uIsFinite(data[i]) && data[i] != 0) { - if(!uIsFinite(min) || (data[i] > 0 && 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 0) + if(uIsFinite(data[i]) && data[i] != 0) { - if(!uIsFinite(min) || (data[i] > 0 && 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; } diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 5c4476a9..7a8bdbed 100644 --- a/guilib/src/CloudViewer.cpp +++ b/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 #include @@ -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 -{ - typedef pcl::visualization::PointCloudColorHandler::PointCloud PointCloud; - typedef PointCloud::Ptr PointCloudPtr; - typedef PointCloud::ConstPtr PointCloudConstPtr; - -public: - typedef boost::shared_ptr Ptr; - typedef boost::shared_ptr ConstPtr; - - /** \brief Constructor. */ - PointCloudColorHandlerIntensityField (const PointCloudConstPtr &cloud, float maxAbsIntensity = 0.0f, int colorMap = 0) : - pcl::visualization::PointCloudColorHandler::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 getColor () const { - vtkSmartPointer scalars; - if (!capable_ || !cloud_) - return scalars; -#else - virtual bool getColor (vtkSmartPointer &scalars) const { - if (!capable_ || !cloud_) - return (false); -#endif - if (!scalars) - scalars = vtkSmartPointer::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; k0?(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(&(*scalars))->SetNumberOfTuples (j); - reinterpret_cast(&(*scalars))->SetArray (colors, j*3, 0, vtkUnsignedCharArray::VTK_DATA_ARRAY_DELETE); - } - else - reinterpret_cast(&(*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 (binaryCloud, "x")); + colorHandler.reset (new PointCloudColorHandlerMinMaxGenericField (binaryCloud, "x", + _cloudColorRangeMin==0.0f?std::numeric_limits::lowest():_cloudColorRangeMin, + _cloudColorRangeMax==0.0f?std::numeric_limits::max():_cloudColorRangeMax, + _aCloudColorRangeInverted->isChecked())); _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport); - colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField (binaryCloud, "y")); + colorHandler.reset (new PointCloudColorHandlerMinMaxGenericField (binaryCloud, "y", + _cloudColorRangeMin==0.0f?std::numeric_limits::lowest():_cloudColorRangeMin, + _cloudColorRangeMax==0.0f?std::numeric_limits::max():_cloudColorRangeMax, + _aCloudColorRangeInverted->isChecked())); _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport); - colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField (binaryCloud, "z")); + colorHandler.reset (new PointCloudColorHandlerMinMaxGenericField (binaryCloud, "z", + _cloudColorRangeMin==0.0f?std::numeric_limits::lowest():_cloudColorRangeMin, + _cloudColorRangeMax==0.0f?std::numeric_limits::max():_cloudColorRangeMax, + _aCloudColorRangeInverted->isChecked())); _visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, viewport); if(rgb) @@ -3611,9 +3498,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 +3895,29 @@ 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; + } else if(a == _aSetBackgroundColor) { QColor color = this->getDefaultBackgroundColor(); diff --git a/guilib/src/ImageView.cpp b/guilib/src/ImageView.cpp index 703a98ca..73bc9413 100644 --- a/guilib/src/ImageView.cpp +++ b/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(v); + for(int u=0; u 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(v); + for(int u=0; u 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())); }