CloudViewer: fixed texture not shown with VTK>=7

guilib/src/CloudViewer.cpp

@@ -489,20 +489,43 @@ bool CloudViewer::updateCloudPose(
 		//UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
 		bool samePose = _addedClouds.find(id).value() == pose;
 		Eigen::Affine3f posef = pose.toEigen3f();
-		if(samePose ||
+		if(!samePose)
-		   _visualizer->updatePointCloudPose(id, posef))
 		{
-			_addedClouds.find(id).value() = pose;
+			// PointCloud / Mesh
-			if(!samePose)
+			bool updated = _visualizer->updatePointCloudPose(id, posef);
+
+#if VTK_MAJOR_VERSION >= 7
+			if(!updated)
 			{
+				// TextureMesh, cannot use updateShapePose because it searches for vtkLODActor, not a vtkActor
+				pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
+				vtkActor* actor;
+				if (am_it != _visualizer->getShapeActorMap()->end ())
+				{
+					actor = vtkActor::SafeDownCast (am_it->second);
+					if (actor)
+					{
+						vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
+						pcl::visualization::PCLVisualizer::convertToVtkMatrix (pose.toEigen3f().matrix (), matrix);
+						actor->SetUserMatrix (matrix);
+						actor->Modified ();
+						updated = true;
+					}
+				}
+			}
+#endif
+
+			if(updated)
+			{
+				_addedClouds.find(id).value() = pose;
 				std::string idNormals = id+"-normals";
 				if(_addedClouds.find(idNormals)!=_addedClouds.end())
 				{
 					_visualizer->updatePointCloudPose(idNormals, posef);
 					_addedClouds.find(idNormals).value() = pose;
 				}
+				return true;
 			}
-			return true;
 		}
 	}
 	return false;
@@ -831,7 +854,11 @@ bool CloudViewer::addCloudMesh(
 	UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
 	if(_visualizer->addPolygonMesh<pcl::PointXYZ>(cloud, polygons, id, 1))
 	{
+#if VTK_MAJOR_VERSION >= 7
+		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.1);
+#else
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
+#endif
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
@@ -865,7 +892,11 @@ bool CloudViewer::addCloudMesh(
 	UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
 	if(_visualizer->addPolygonMesh<pcl::PointXYZRGB>(cloud, polygons, id, 1))
 	{
+#if VTK_MAJOR_VERSION >= 7
+		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.1);
+#else
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
+#endif
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
@@ -899,7 +930,11 @@ bool CloudViewer::addCloudMesh(
 	UDEBUG("Adding %s with %d points and %d polygons", id.c_str(), (int)cloud->size(), (int)polygons.size());
 	if(_visualizer->addPolygonMesh<pcl::PointXYZRGBNormal>(cloud, polygons, id, 1))
 	{
+#if VTK_MAJOR_VERSION >= 7
+		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.1);
+#else
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
+#endif
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
@@ -932,7 +967,11 @@ bool CloudViewer::addCloudMesh(
 	UDEBUG("Adding %s with %d polygons", id.c_str(), (int)mesh->polygons.size());
 	if(_visualizer->addPolygonMesh(*mesh, id, 1))
 	{
+#if VTK_MAJOR_VERSION >= 7
+		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.1);
+#else
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
+#endif
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
 		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
@@ -966,25 +1005,26 @@ bool CloudViewer::addCloudTextureMesh(
 	UDEBUG("Adding %s", id.c_str());
 	if(this->addTextureMesh(*textureMesh, texture, id, 1))
 	{
-		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
+#if VTK_MAJOR_VERSION >= 7
-		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
+		vtkActor* actor = vtkActor::SafeDownCast (_visualizer->getShapeActorMap()->find(id)->second);
-		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
+#else
-		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
+		vtkActor* actor = vtkActor::SafeDownCast (_visualizer->getCloudActorMap()->find(id)->second.actor);
-		_visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
+#endif
+		UASSERT(actor);
 		if(!textureMesh->cloud.is_dense)
 		{
-			_visualizer->getCloudActorMap()->find(id)->second.actor->GetTexture()->SetInterpolate(1);
+			actor->GetTexture()->SetInterpolate(1);
-			_visualizer->getCloudActorMap()->find(id)->second.actor->GetTexture()->SetBlendingMode(vtkTexture::VTK_TEXTURE_BLENDING_MODE_REPLACE);
+			actor->GetTexture()->SetBlendingMode(vtkTexture::VTK_TEXTURE_BLENDING_MODE_REPLACE);
 		}
-		_visualizer->updatePointCloudPose(id, pose.toEigen3f());
 		if(_buildLocator)
 		{
 			vtkSmartPointer<vtkOBBTree> tree = vtkSmartPointer<vtkOBBTree>::New();
-			tree->SetDataSet(_visualizer->getCloudActorMap()->find(id)->second.actor->GetMapper()->GetInput());
+			tree->SetDataSet(actor->GetMapper()->GetInput());
 			tree->BuildLocator();
 			_locators.insert(std::make_pair(id, tree));
 		}
-		_addedClouds.insert(id, pose);
+		_addedClouds.insert(id, Transform::getIdentity());
+		this->updateCloudPose(id, pose);
 		return true;
 	}
 	return false;
@@ -1223,8 +1263,13 @@ bool CloudViewer::addTextureMesh (
 {
 	// Copied from PCL 1.8, modified to ignore vertex color and accept only one material (loaded from memory instead of file)
 
+#if VTK_MAJOR_VERSION >= 7
+  pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
+  if (am_it != _visualizer->getShapeActorMap()->end ())
+#else
   pcl::visualization::CloudActorMap::iterator am_it = _visualizer->getCloudActorMap()->find (id);
   if (am_it != _visualizer->getCloudActorMap()->end ())
+#endif
   {
     PCL_ERROR ("[PCLVisualizer::addTextureMesh] A shape with id <%s> already exists!"
                " Please choose a different id and retry.\n",
@@ -1325,7 +1370,11 @@ bool CloudViewer::addTextureMesh (
     mapper->SetInputData (polydata);
 #endif
 
+#if VTK_MAJOR_VERSION >= 7
+  vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New ();
+#else
   vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New ();
+#endif
   vtkTextureUnitManager* tex_manager = vtkOpenGLRenderWindow::SafeDownCast (_visualizer->getRenderWindow())->GetTextureUnitManager ();
     if (!tex_manager)
       return (false);
@@ -1374,17 +1423,20 @@ bool CloudViewer::addTextureMesh (
 	}
 
   // Save the pointer/ID pair to the global actor map
+#if VTK_MAJOR_VERSION >= 7
+  (*_visualizer->getShapeActorMap())[id] = actor;
+#else
   (*_visualizer->getCloudActorMap())[id].actor = actor;
-
+   // Save the viewpoint transformation matrix to the global actor map
-  // Save the viewpoint transformation matrix to the global actor map
   (*_visualizer->getCloudActorMap())[id].viewpoint_transformation_ = transformation;
+#endif
 
-  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetAmbient(0.5);
+  actor->GetProperty()->SetAmbient(0.5);
-  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
+  actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
-  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
+  actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG);
-  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
+  actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
-  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
+  actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
-  _visualizer->getCloudActorMap()->find(id)->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
+  actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
   return true;
 }
 
@@ -1401,10 +1453,17 @@ bool CloudViewer::addOccupancyGridMap(
 	float ySize = float(map8U.rows) * resolution;
 
 	UDEBUG("resolution=%f, xSize=%f, ySize=%f, xMin=%f, yMin=%f", resolution, xSize, ySize, xMin, yMin);
+#if VTK_MAJOR_VERSION >= 7
+	if(_visualizer->getShapeActorMap()->find("map") != _visualizer->getShapeActorMap()->end())
+	{
+		_visualizer->removeShape("map");
+	}
+#else
 	if(_visualizer->getCloudActorMap()->find("map") != _visualizer->getCloudActorMap()->end())
 	{
 		_visualizer->removePointCloud("map");
 	}
+#endif
 
 	if(xSize > 0.0f && ySize > 0.0f)
 	{
@@ -1448,10 +1507,17 @@ bool CloudViewer::addOccupancyGridMap(
 
 void CloudViewer::removeOccupancyGridMap()
 {
+#if VTK_MAJOR_VERSION >= 7
+	if(_visualizer->getShapeActorMap()->find("map") != _visualizer->getShapeActorMap()->end())
+	{
+		_visualizer->removeShape("map");
+	}
+#else
 	if(_visualizer->getCloudActorMap()->find("map") != _visualizer->getCloudActorMap()->end())
 	{
 		_visualizer->removePointCloud("map");
 	}
+#endif
 }
 
 void CloudViewer::addOrUpdateCoordinate(
@@ -2286,15 +2352,26 @@ void CloudViewer::resetCamera()
 
 void CloudViewer::removeAllClouds()
 {
-	_addedClouds.clear();
+	QMap<std::string, Transform> addedClouds = _addedClouds;
-	_locators.clear();
+	QList<std::string> ids = _addedClouds.keys();
-	_visualizer->removeAllPointClouds();
+	for(QList<std::string>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
+	{
+		removeCloud(*iter);
+	}
+	UASSERT(_addedClouds.empty());
+	UASSERT(_locators.empty());
 }
 
 
 bool CloudViewer::removeCloud(const std::string & id)
 {
 	bool success = _visualizer->removePointCloud(id);
+#if VTK_MAJOR_VERSION >= 7
+	if(!success)
+	{
+		success = _visualizer->removeShape(id);
+	}
+#endif
 	_visualizer->removePointCloud(id+"-normals");
 	_addedClouds.remove(id); // remove after visualizer
 	_addedClouds.remove(id+"-normals");
@@ -2441,6 +2518,18 @@ void CloudViewer::setBackfaceCulling(bool enabled, bool frontfaceCulling)
 		iter->second.actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
 		iter->second.actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
 	}
+#if VTK_MAJOR_VERSION >= 7
+	pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+	for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
+	{
+		vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+		if(actor)
+		{
+			actor->GetProperty()->SetBackfaceCulling(_aBackfaceCulling->isChecked());
+			actor->GetProperty()->SetFrontfaceCulling(_frontfaceCulling);
+		}
+	}
+#endif
 	this->update();
 }
 
@@ -2515,6 +2604,17 @@ void CloudViewer::setLighting(bool on)
 	{
 		iter->second.actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
 	}
+#if VTK_MAJOR_VERSION >= 7
+	pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+	for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
+	{
+		vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+		if(actor && _addedClouds.contains(iter->first))
+		{
+			actor->GetProperty()->SetLighting(_aSetLighting->isChecked());
+		}
+	}
+#endif
 	this->update();
 }
 
@@ -2526,6 +2626,17 @@ void CloudViewer::setShading(bool on)
 	{
 		iter->second.actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG); // VTK_FLAT - VTK_GOURAUD - VTK_PHONG
 	}
+#if VTK_MAJOR_VERSION >= 7
+	pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+	for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
+	{
+		vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+		if(actor && _addedClouds.contains(iter->first))
+		{
+			actor->GetProperty()->SetInterpolation(_aSetFlatShading->isChecked()?VTK_FLAT:VTK_PHONG); // VTK_FLAT - VTK_GOURAUD - VTK_PHONG
+		}
+	}
+#endif
 	this->update();
 }
 
@@ -2537,6 +2648,17 @@ void CloudViewer::setEdgeVisibility(bool visible)
 	{
 		iter->second.actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
 	}
+#if VTK_MAJOR_VERSION >= 7
+	pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+	for(pcl::visualization::ShapeActorMap::iterator iter=shapeActorMap->begin(); iter!=shapeActorMap->end(); ++iter)
+	{
+		vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+		if(actor && _addedClouds.contains(iter->first))
+		{
+			actor->GetProperty()->SetEdgeVisibility(_aSetEdgeVisibility->isChecked());
+		}
+	}
+#endif
 	this->update();
 }
 
@@ -2837,6 +2959,19 @@ void CloudViewer::setCloudVisibility(const std::string & id, bool isVisible)
 	}
 	else
 	{
+#if VTK_MAJOR_VERSION >= 7
+		pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+		pcl::visualization::ShapeActorMap::iterator iter = shapeActorMap->find(id);
+		if(iter != shapeActorMap->end())
+		{
+			vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+			if(actor)
+			{
+				actor->SetVisibility(isVisible?1:0);
+				return;
+			}
+		}
+#endif
 		UERROR("Cannot find actor named \"%s\".", id.c_str());
 	}
 }
@@ -2851,6 +2986,18 @@ bool CloudViewer::getCloudVisibility(const std::string & id)
 	}
 	else
 	{
+#if VTK_MAJOR_VERSION >= 7
+		pcl::visualization::ShapeActorMapPtr shapeActorMap = _visualizer->getShapeActorMap();
+		pcl::visualization::ShapeActorMap::iterator iter = shapeActorMap->find(id);
+		if(iter != shapeActorMap->end())
+		{
+			vtkActor* actor = vtkActor::SafeDownCast (iter->second);
+			if(actor)
+			{
+				return actor->GetVisibility() != 0;
+			}
+		}
+#endif
 		UERROR("Cannot find actor named \"%s\".", id.c_str());
 	}
 	return false;
@@ -2867,20 +3014,39 @@ void CloudViewer::setCloudColorIndex(const std::string & id, int index)
 void CloudViewer::setCloudOpacity(const std::string & id, double opacity)
 {
 	double lastOpacity;
-	_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, lastOpacity, id);
+	if(_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, lastOpacity, id))
-	if(lastOpacity != opacity)
 	{
-		_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id);
+		if(lastOpacity != opacity)
+		{
+			_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, id);
+		}
 	}
+#if VTK_MAJOR_VERSION >= 7
+	else
+	{
+		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)
+			{
+				actor->GetProperty ()->SetOpacity (opacity);
+				actor->Modified ();
+			}
+		}
+	}
+#endif
 }
 
 void CloudViewer::setCloudPointSize(const std::string & id, int size)
 {
 	double lastSize;
-	_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, lastSize, id);
+	if(_visualizer->getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, lastSize, id))
-	if((int)lastSize != size)
 	{
-		_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, (double)size, id);
+		if((int)lastSize != size)
+		{
+			_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, (double)size, id);
+		}
 	}
 }