Per-point colors in point clouds (#494)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
Signed-off-by: Ian Chen <ichen@osrfoundation.org>
Co-authored-by: Louise Poubel <louise@openrobotics.org>
Co-authored-by: Ian Chen <ichen@osrfoundation.org>

ogre2/include/ignition/rendering/ogre2/Ogre2DynamicRenderable.hh

@@ -137,6 +137,14 @@ namespace ignition
       private: void GenerateNormals(Ogre::OperationType _opType,
           const std::vector<math::Vector3d> &_vertices, float *_vbuffer);
 
+      /// \brief Helper function to generate colors per-vertex. Only applies
+      /// to points. The colors fill the normal slots on the vertex buffer.
+      /// \param[in] _opType Ogre render operation type
+      /// \param[in] _vertices a list of vertices
+      /// \param[in,out] _vbuffer vertex buffer to be filled
+      private: void GenerateColors(Ogre::OperationType _opType,
+          const std::vector<math::Vector3d> &_vertices, float *_vbuffer);
+
       /// \brief Destroy the vertex buffer
       private: void DestroyBuffer();
 

ogre2/src/Ogre2Camera.cc

@@ -278,7 +278,6 @@ VisualPtr Ogre2Camera::VisualAt(const ignition::math::Vector2i &_mousePos)
   ignition::math::Vector2i mousePos(
       static_cast<int>(std::rint(ratio * _mousePos.X())),
       static_cast<int>(std::rint(ratio * _mousePos.Y())));
-
   Ogre::Item *ogreItem = this->selectionBuffer->OnSelectionClick(
       mousePos.X(), mousePos.Y());
 

ogre2/src/Ogre2DynamicRenderable.cc

@@ -311,7 +311,8 @@ void Ogre2DynamicRenderable::UpdateBuffer()
 
   // fill the rest of the buffer with the position of the last vertex to avoid
   // the geometry connecting back to 0, 0, 0
-  if (vertexCount > 0 && vertexCount < this->dataPtr->vertexBufferCapacity)
+  if (vertexCount > 0 && vertexCount < this->dataPtr->vertexBufferCapacity &&
+      this->dataPtr->operationType != Ogre::OperationType::OT_POINT_LIST)
   {
     math::Vector3d lastVertex = this->dataPtr->vertices[vertexCount-1];
     for (unsigned int i = vertexCount; i < this->dataPtr->vertexBufferCapacity;
@@ -332,6 +333,10 @@ void Ogre2DynamicRenderable::UpdateBuffer()
   this->GenerateNormals(this->dataPtr->operationType, this->dataPtr->vertices,
       vertices);
 
+  // fill colors for points
+  this->GenerateColors(this->dataPtr->operationType, this->dataPtr->vertices,
+      vertices);
+
   // unmap buffer
   this->dataPtr->vertexBuffer->unmap(Ogre::UO_KEEP_PERSISTENT);
 
@@ -359,7 +364,7 @@ void Ogre2DynamicRenderable::UpdateBuffer()
       this->dataPtr->ogreItem->setCastShadows(
           this->dataPtr->material->CastShadows());
     }
-    else if (lowLevelMat)
+    if (lowLevelMat)
     {
       // the _initialise call above resets the ogre item properties so set
       // them again
@@ -448,7 +453,7 @@ void Ogre2DynamicRenderable::AddPoint(const ignition::math::Vector3d &_pt,
   this->dataPtr->vertices.push_back(_pt);
 
   // todo(anyone)
-  // setting material works but vertex coloring does not work yet.
+  // setting material works but vertex coloring only works for points
   // It requires using an unlit datablock:
   // https://forums.ogre3d.org/viewtopic.php?t=93627#p539276
   this->dataPtr->colors.push_back(_color);
@@ -492,12 +497,12 @@ void Ogre2DynamicRenderable::SetColor(unsigned int _index,
 
 
   // todo(anyone)
-  // vertex coloring does not work yet. It requires using an unlit datablock:
+  // vertex coloring only works for points.
+  // Full implementation requires using an unlit datablock:
   // https://forums.ogre3d.org/viewtopic.php?t=93627#p539276
   this->dataPtr->colors[_index] = _color;
 
-  // uncomment this line when colors are working
-  // this->dataPtr->dirty = true;
+  this->dataPtr->dirty = true;
 }
 
 /////////////////////////////////////////////////
@@ -713,3 +718,42 @@ void Ogre2DynamicRenderable::GenerateNormals(Ogre::OperationType _opType,
       break;
   }
 }
+
+//////////////////////////////////////////////////
+void Ogre2DynamicRenderable::GenerateColors(Ogre::OperationType _opType,
+  const std::vector<math::Vector3d> &_vertices, float *_vbuffer)
+{
+  unsigned int vertexCount = _vertices.size();
+  // Skip if colors haven't been setup per-vertex correctly.
+  if (vertexCount != this->dataPtr->colors.size())
+    return;
+
+  // Each vertex occupies 6 elements in the vbuffer float array. Normally,
+  // the last 3 are reserved for normals. But for types that don't use normals,
+  // we use them for per-vertex coloring.
+  // vbuffer[i]   : position x
+  // vbuffer[i+1] : position y
+  // vbuffer[i+2] : position z
+  // vbuffer[i+3] : color r
+  // vbuffer[i+4] : color g
+  // vbuffer[i+5] : color b
+  switch (_opType)
+  {
+    case Ogre::OperationType::OT_POINT_LIST:
+    {
+      for (unsigned int i = 0; i < vertexCount; ++i)
+      {
+        auto color = this->dataPtr->colors[i];
+
+        unsigned int idx = i * 6;
+        _vbuffer[idx+3] = color.R();
+        _vbuffer[idx+4] = color.G();
+        _vbuffer[idx+5] = color.B();
+      }
+
+      break;
+    }
+    default:
+      break;
+  }
+}

ogre2/src/Ogre2LidarVisual.cc

@@ -347,6 +347,8 @@ void Ogre2LidarVisual::Update()
       this->dataPtr->deadZoneRayFans[j]->SetPoint(0, this->offset.Pos());
     }
 
+    auto pointMat = this->Scene()->Material("Lidar/BlueRay");
+
     unsigned count = this->horizontalCount;
     // Process each ray in current scan
     for (unsigned int i = 0; i < count; ++i)
@@ -441,14 +443,16 @@ void Ogre2LidarVisual::Update()
         {
           if (this->displayNonHitting || !inf)
           {
-            this->dataPtr->points[j]->AddPoint(inf ? noHitPt: pt);
+            this->dataPtr->points[j]->AddPoint(inf ? noHitPt : pt,
+                pointMat->Diffuse());
           }
         }
         else
         {
           if (this->displayNonHitting || !inf)
           {
-            this->dataPtr->points[j]->SetPoint(i, inf ? noHitPt: pt);
+            this->dataPtr->points[j]->SetPoint(i, inf ? noHitPt : pt);
+            this->dataPtr->points[j]->SetColor(i, pointMat->Diffuse());
           }
         }
       }
@@ -483,12 +487,6 @@ void Ogre2LidarVisual::Update()
     auto pass = this->dataPtr->pointsMat->getTechnique(0)->getPass(0);
     auto vertParams = pass->getVertexProgramParameters();
     vertParams->setNamedConstant("size", static_cast<Ogre::Real>(this->size));
-
-    // support setting color only from diffuse for now
-    MaterialPtr mat = this->Scene()->Material("Lidar/BlueRay");
-    auto fragParams = pass->getFragmentProgramParameters();
-    fragParams->setNamedConstant("color",
-        Ogre2Conversions::Convert(mat->Diffuse()));
   }
 
   // The newly created dynamic lines are having default visibility as true.

ogre2/src/Ogre2Marker.cc

@@ -112,14 +112,6 @@ void Ogre2Marker::PreRender()
     auto pass = item->getSubItem(0)->getMaterial()->getTechnique(0)->getPass(0);
     auto vertParams = pass->getVertexProgramParameters();
     vertParams->setNamedConstant("size", static_cast<Ogre::Real>(this->size));
-
-    // support setting color only from diffuse for now
-    if (this->dataPtr->material)
-    {
-      auto fragParams = pass->getFragmentProgramParameters();
-      fragParams->setNamedConstant("color",
-          Ogre2Conversions::Convert(this->dataPtr->material->Diffuse()));
-    }
   }
 
   this->dataPtr->dynamicRenderable->Update();

ogre2/src/Ogre2RayQuery.cc

@@ -144,6 +144,10 @@ RayQueryResult Ogre2RayQuery::ClosestPoint(bool _forceSceneUpdate)
 //////////////////////////////////////////////////
 RayQueryResult Ogre2RayQuery::ClosestPointBySelectionBuffer()
 {
+  // update selection buffer dimension in case window is resized
+  this->dataPtr->camera->SelectionBuffer()->SetDimensions(
+    this->dataPtr->camera->ImageWidth(), this->dataPtr->camera->ImageHeight());
+
   RayQueryResult result;
   Ogre::Item *ogreItem = nullptr;
   math::Vector3d point;

ogre2/src/Ogre2SelectionBuffer.cc

@@ -88,6 +88,9 @@ class ignition::rendering::Ogre2SelectionBufferPrivate
   /// into a render target or render texture.
   public: Ogre::CompositorWorkspace *ogreCompositorWorkspace = nullptr;
 
+  /// \brief Name of the compositor workspace definition
+  public: std::string ogreCompWorkspaceDefName;
+
   /// \brief The selection buffer material
   public: Ogre::MaterialPtr selectionMaterial;
 };
@@ -142,6 +145,16 @@ Ogre2SelectionBuffer::~Ogre2SelectionBuffer()
 {
   this->DeleteRTTBuffer();
 
+  // remove selectionMaterial in destructor
+  // this does not need to be done in DeleteRTTBuffer as we do not need to
+  // reload the same material every time
+  if (!this->dataPtr->selectionMaterial.isNull())
+  {
+    Ogre::MaterialManager::getSingleton().remove(
+        this->dataPtr->selectionMaterial->getName());
+    this->dataPtr->selectionMaterial.setNull();
+  }
+
   // remove selection buffer camera
   this->dataPtr->sceneMgr->destroyCamera(this->dataPtr->selectionCamera);
 }
@@ -181,10 +194,17 @@ void Ogre2SelectionBuffer::Update()
 /////////////////////////////////////////////////
 void Ogre2SelectionBuffer::DeleteRTTBuffer()
 {
-  if (this->dataPtr->selectionMaterial)
+  if (this->dataPtr->ogreCompositorWorkspace)
   {
-    Ogre::MaterialManager::getSingleton().remove(
-        this->dataPtr->selectionMaterial->getName());
+    // TODO(ahcorde): Remove the workspace. Potential leak here
+    this->dataPtr->ogreCompMgr->removeWorkspace(
+        this->dataPtr->ogreCompositorWorkspace);
+
+    this->dataPtr->ogreCompMgr->removeWorkspaceDefinition(
+        this->dataPtr->ogreCompWorkspaceDefName);
+    this->dataPtr->ogreCompMgr->removeNodeDefinition(
+        this->dataPtr->ogreCompWorkspaceDefName + "/Node");
+    this->dataPtr->ogreCompositorWorkspace = nullptr;
   }
 
   auto engine = Ogre2RenderEngine::Instance();
@@ -222,11 +242,16 @@ void Ogre2SelectionBuffer::CreateRTTBuffer()
   // The SelectionBuffer material is defined in script
   // (selection_buffer.material).
   std::string matSelectionName = "SelectionBuffer";
-  Ogre::MaterialPtr matSelection =
-      Ogre::MaterialManager::getSingleton().getByName(matSelectionName);
-  this->dataPtr->selectionMaterial = matSelection->clone(
-      this->dataPtr->camera->getName() + "_" + matSelectionName);
-  this->dataPtr->selectionMaterial->load();
+  std::string matSelectionCloneName =
+      this->dataPtr->camera->getName() + "_" + matSelectionName;
+  if (this->dataPtr->selectionMaterial.isNull())
+  {
+    Ogre::MaterialPtr matSelection =
+        Ogre::MaterialManager::getSingleton().getByName(matSelectionName);
+    this->dataPtr->selectionMaterial = matSelection->clone(
+        matSelectionCloneName);
+    this->dataPtr->selectionMaterial->load();
+  }
   Ogre::Pass *p = this->dataPtr->selectionMaterial->getTechnique(0)->getPass(0);
   Ogre::GpuProgramParametersSharedPtr psParams =
       p->getFragmentProgramParameters();
@@ -252,13 +277,15 @@ void Ogre2SelectionBuffer::CreateRTTBuffer()
 
   // create compositor workspace for rendering
   // Setup the selection buffer compositor.
-  const Ogre::String workspaceName = "SelectionBufferWorkspace" +
+  this->dataPtr->ogreCompWorkspaceDefName = "SelectionBufferWorkspace" +
       this->dataPtr->camera->getName();
 
+  std::string nodeSpaceDefName =
+      this->dataPtr->ogreCompWorkspaceDefName + "/Node";
+
   Ogre::CompositorNodeDef *nodeDef =
       this->dataPtr->ogreCompMgr->addNodeDefinition(
-      "AutoGen " + Ogre::IdString(workspaceName +
-      "/Node").getReleaseText());
+      nodeSpaceDefName);
   Ogre::TextureDefinitionBase::TextureDefinition *depthTexDef =
       nodeDef->addTextureDefinition("depthTexture");
   depthTexDef->textureType = Ogre::TextureTypes::Type2D;
@@ -346,15 +373,16 @@ void Ogre2SelectionBuffer::CreateRTTBuffer()
   }
 
   Ogre::CompositorWorkspaceDef *workDef =
-      this->dataPtr->ogreCompMgr->addWorkspaceDefinition(workspaceName);
+      this->dataPtr->ogreCompMgr->addWorkspaceDefinition(
+      this->dataPtr->ogreCompWorkspaceDefName);
   workDef->connectExternal(0, nodeDef->getName(), 0);
 
   this->dataPtr->ogreCompositorWorkspace =
       this->dataPtr->ogreCompMgr->addWorkspace(
         this->dataPtr->scene->OgreSceneManager(),
         this->dataPtr->renderTexture,
         this->dataPtr->selectionCamera,
-        workspaceName,
+        this->dataPtr->ogreCompWorkspaceDefName,
         false);
 }
 
@@ -369,14 +397,6 @@ void Ogre2SelectionBuffer::SetDimensions(
   this->dataPtr->height = _height;
 
   this->DeleteRTTBuffer();
-
-  if (this->dataPtr->ogreCompositorWorkspace)
-  {
-    // TODO(ahcorde): Remove the workspace. Potential leak here
-    this->dataPtr->ogreCompMgr->removeWorkspace(
-        this->dataPtr->ogreCompositorWorkspace);
-  }
-
   this->CreateRTTBuffer();
 }
 /////////////////////////////////////////////////
@@ -398,15 +418,23 @@ bool Ogre2SelectionBuffer::ExecuteQuery(const int _x, const int _y,
   if (!this->dataPtr->camera)
     return false;
 
+  // check camera has valid projection matrix
+  // There could be nan values if camera was resized
+  Ogre::Matrix4 projectionMatrix =
+      this->dataPtr->camera->getProjectionMatrix();
+  if (projectionMatrix.getTrans().isNaN() ||
+      projectionMatrix.extractQuaternion().isNaN())
+    return false;
+
    const unsigned int targetWidth = this->dataPtr->width;
    const unsigned int targetHeight = this->dataPtr->height;
 
    if (_x < 0 || _y < 0 || _x >= static_cast<int>(targetWidth)
        || _y >= static_cast<int>(targetHeight))
      return false;
 
-  // // 1x1 selection buffer, adapted from rviz
-  // // http://docs.ros.org/indigo/api/rviz/html/c++/selection__manager_8cpp.html
+  // 1x1 selection buffer, adapted from rviz
+  // http://docs.ros.org/indigo/api/rviz/html/c++/selection__manager_8cpp.html
   unsigned int width = 1;
   unsigned int height = 1;
   float x1 = static_cast<float>(_x) /
@@ -425,10 +453,10 @@ bool Ogre2SelectionBuffer::ExecuteQuery(const int _x, const int _y,
   transMatrix[0][3] -= x1+x2;
   transMatrix[1][3] += y1+y2;
   Ogre::Matrix4 customProjectionMatrix =
-      scaleMatrix * transMatrix * this->dataPtr->camera->getProjectionMatrix();
-
-   this->dataPtr->selectionCamera->setCustomProjectionMatrix(true,
-        customProjectionMatrix);
+      scaleMatrix * transMatrix *
+      this->dataPtr->camera->getProjectionMatrix();
+  this->dataPtr->selectionCamera->setCustomProjectionMatrix(true,
+      customProjectionMatrix);
 
   this->dataPtr->selectionCamera->setPosition(
       this->dataPtr->camera->getDerivedPosition());
@@ -440,9 +468,8 @@ bool Ogre2SelectionBuffer::ExecuteQuery(const int _x, const int _y,
 
   Ogre::Image2 image;
   image.convertFromTexture(this->dataPtr->renderTexture, 0, 0);
-
   Ogre::ColourValue pixel = image.getColourAt(0, 0, 0, 0);
-  // Ogre::ColourValue pixel = image.getColourAt(_x, _y, 0, 0);
+
   float color = pixel[3];
   uint32_t *rgba = reinterpret_cast<uint32_t *>(&color);
   unsigned int r = *rgba >> 24 & 0xFF;

ogre2/src/media/materials/programs/GLSL/point_fs.glsl

@@ -18,13 +18,11 @@
 #version 330
 
 uniform vec4 color;
+in vec3 ptColor;
 
 out vec4 fragColor;
 
 void main()
 {
-  // todo(anyone) update Ogre2DynamicRenderable to support vertex coloring
-  // so we can set color using the line below
-  // fragColor = gl_Color;
-  fragColor = color;
+  fragColor = vec4(ptColor.x, ptColor.y, ptColor.z, 1);
 }

ogre2/src/media/materials/programs/GLSL/point_vs.glsl

@@ -18,8 +18,10 @@
 #version 330
 
 in vec4 vertex;
+in vec3 normal;
 uniform mat4 worldViewProj;
 uniform float size;
+out vec3 ptColor;
 
 out gl_PerVertex
 {
@@ -31,4 +33,6 @@ void main()
   // Calculate output position
   gl_Position = worldViewProj * vertex;
   gl_PointSize = size;
+  // We're abusing the normal variable to hold per-point colors
+  ptColor = normal;
 }