CloudViewer: showing intensity as grayscale instead of generic color handler

guilib/src/CloudViewer.cpp

@@ -453,6 +453,119 @@ 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) :
+		pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::PointCloudColorHandler (cloud)
+		{
+		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
+	 */
+	virtual bool
+	getColor (vtkSmartPointer<vtkDataArray> &scalars) const
+	{
+		if (!capable_ || !cloud_)
+			return (false);
+
+		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;
+			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)(intensities[k]/max*255.0f):0;
+			}
+			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;
+		return (true);
+	}
+
+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"); }
+};
+
 bool CloudViewer::addCloud(
 		const std::string & id,
 		const pcl::PCLPointCloud2Ptr & binaryCloud,
@@ -513,8 +626,8 @@ bool CloudViewer::addCloud(
 		}
 		else if(hasIntensity)
 		{
-			//rgb
+			//intensity
-			colorHandler.reset(new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2>(binaryCloud, "intensity"));
+			colorHandler.reset(new PointCloudColorHandlerIntensityField(binaryCloud));
 			_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id, 1);
 		}
 		else if(previousColorIndex == 5)
@@ -1140,8 +1253,8 @@ bool CloudViewer::addTextureMesh (
 
   vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
   vtkTextureUnitManager* tex_manager = vtkOpenGLRenderWindow::SafeDownCast (_visualizer->getRenderWindow())->GetTextureUnitManager ();
-  if (!tex_manager)
+    if (!tex_manager)
-    return (false);
+      return (false);
  
     vtkSmartPointer<vtkTexture> texture = vtkSmartPointer<vtkTexture>::New ();
     // fill vtkTexture from pcl::TexMaterial structure