Improve voxel rendering by using multiple nodes instead of CompositeShape

CHANGELOG.md

@@ -28,6 +28,7 @@
   * Added shape type and color options to PointCloudShape: [#1314](https://github.com/dartsim/dart/pull/1314), [#1316](https://github.com/dartsim/dart/pull/1316)
   * Fixed incorrect transparency of MeshShape for MATERIAL_COLOR mode: [#1315](https://github.com/dartsim/dart/pull/1315)
   * Fixed incorrect PointCloudShape transparency: [#1330](https://github.com/dartsim/dart/pull/1330)
+  * Improved voxel rendering by using multiple nodes instead of CompositeShape: [#1334](https://github.com/dartsim/dart/pull/1334)
 
 * Examples and Tutorials
 

dart/dynamics/PointCloudShape.hpp

@@ -61,7 +61,7 @@ class PointCloudShape : public Shape
   enum PointShapeType
   {
     BOX = 0,          ///< 3D volumetric box
-    BILLBOARD_QUAD,   ///< 2D square always facing the screen
+    BILLBOARD_SQUARE, ///< 2D square always facing the screen
     BILLBOARD_CIRCLE, ///< 2D circle always facing the screen
   };
 

dart/gui/osg/GridVisual.cpp

@@ -521,6 +521,8 @@ void GridVisual::initialize()
   mAxisLineVertices = new ::osg::Vec3Array;
   mAxisLineGeom->setVertexArray(mAxisLineVertices);
   mAxisLineGeom->setDataVariance(::osg::Object::STATIC);
+  mAxisLineGeom->getOrCreateStateSet()->setMode(
+      GL_BLEND, ::osg::StateAttribute::ON);
 
   // Set grid color
   static const ::osg::Vec4 majorLineColor(0.4f, 0.4f, 0.4f, 1.0f);
@@ -540,12 +542,16 @@ void GridVisual::initialize()
   mMajorLineColor->at(0) = majorLineColor;
   mMajorLineGeom->setColorArray(mMajorLineColor);
   mMajorLineGeom->setColorBinding(::osg::Geometry::BIND_OVERALL);
+  mMajorLineGeom->getOrCreateStateSet()->setMode(
+      GL_BLEND, ::osg::StateAttribute::ON);
 
   mMinorLineColor = new ::osg::Vec4Array;
   mMinorLineColor->resize(1);
   mMinorLineColor->at(0) = minorLineColor;
   mMinorLineGeom->setColorArray(mMinorLineColor);
   mMinorLineGeom->setColorBinding(::osg::Geometry::BIND_OVERALL);
+  mMinorLineGeom->getOrCreateStateSet()->setMode(
+      GL_BLEND, ::osg::StateAttribute::ON);
 
   mMinorLineFaces = new ::osg::DrawElementsUInt(::osg::PrimitiveSet::LINES, 0);
   mMinorLineGeom->addPrimitiveSet(mMinorLineFaces);

dart/gui/osg/render/PointCloudShapeNode.cpp

@@ -82,7 +82,7 @@ class BoxDrawable final : public ::osg::ShapeDrawable
 };
 
 //==============================================================================
-class QuadDrawable : public ::osg::Geometry
+class QuadDrawable final : public ::osg::Geometry
 {
 public:
   QuadDrawable(double size, const Eigen::Vector4d& color)
@@ -129,7 +129,7 @@ class QuadDrawable : public ::osg::Geometry
 };
 
 //==============================================================================
-class CircleDrawable : public ::osg::Geometry
+class CircleDrawable final : public ::osg::Geometry
 {
 public:
   CircleDrawable(double size, const Eigen::Vector4d& color)
@@ -441,7 +441,7 @@ ::osg::ref_ptr<PointNode> PointCloudShapeNode::createPointNode(
   }
   else if (
       mPointShapeType
-      == dynamics::PointCloudShape::PointShapeType::BILLBOARD_QUAD)
+      == dynamics::PointCloudShape::PointShapeType::BILLBOARD_SQUARE)
   {
     return new BillboardPointNode<QuadDrawable>(point, size, color);
   }

dart/gui/osg/render/VoxelGridShapeNode.cpp

@@ -49,50 +49,73 @@ namespace osg {
 namespace render {
 
 //==============================================================================
-class VoxelGridShapeDrawable : public ::osg::ShapeDrawable
+class BoxDrawable final : public ::osg::ShapeDrawable
 {
 public:
-  VoxelGridShapeDrawable(
-      dynamics::VoxelGridShape* shape,
-      dynamics::VisualAspect* visualAspect,
-      VoxelGridShapeGeode* parent);
+  BoxDrawable(double size, const Eigen::Vector4d& color)
+  {
+    mShape = new ::osg::Box(::osg::Vec3(), static_cast<float>(size));
+    setColor(eigToOsgVec4f(color));
+    setShape(mShape);
+    setDataVariance(::osg::Object::DYNAMIC);
+    getOrCreateStateSet()->setMode(GL_BLEND, ::osg::StateAttribute::ON);
+  }
+
+  void updateSize(double size)
+  {
+    mShape->setHalfLengths(::osg::Vec3(
+        static_cast<float>(size * 0.5),
+        static_cast<float>(size * 0.5),
+        static_cast<float>(size * 0.5)));
+    dirtyBound();
+    dirtyDisplayList();
+  }
 
-  void refresh(bool firstTime);
+  void updateColor(const Eigen::Vector4d& color)
+  {
+    setColor(eigToOsgVec4f(color));
+  }
 
 protected:
-  ~VoxelGridShapeDrawable() override = default;
-
-  dynamics::VoxelGridShape* mVoxelGridShape;
-  dynamics::VisualAspect* mVisualAspect;
-  VoxelGridShapeGeode* mParent;
-  std::size_t mVoxelVersion;
-
-private:
-  void updateBoxes(
-      ::osg::CompositeShape* osgShape,
-      const octomap::OcTree* tree,
-      double threashold);
-  std::vector<::osg::ref_ptr<::osg::Box>> mBoxes;
+  ::osg::ref_ptr<::osg::Box> mShape;
 };
 
 //==============================================================================
-class VoxelGridShapeGeode : public ShapeNode, public ::osg::Geode
+class VoxelNode : public ::osg::MatrixTransform
 {
 public:
-  VoxelGridShapeGeode(
-      dynamics::VoxelGridShape* shape,
-      ShapeFrameNode* parentShapeFrame,
-      VoxelGridShapeNode* parentNode);
+  VoxelNode(
+      const Eigen::Vector3d& point, double size, const Eigen::Vector4d& color)
+  {
+    mDrawable = new BoxDrawable(size, color);
+    mGeode = new ::osg::Geode();
 
-  void refresh();
-  void extractData();
+    mGeode->addDrawable(mDrawable);
+    addChild(mGeode);
 
-protected:
-  virtual ~VoxelGridShapeGeode();
+    updateCenter(point);
+  }
 
-  VoxelGridShapeNode* mParentNode;
-  dynamics::VoxelGridShape* mVoxelGridShape;
-  VoxelGridShapeDrawable* mDrawable;
+  void updateCenter(const Eigen::Vector3d& point)
+  {
+    Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
+    tf.translation() = point;
+    setMatrix(eigToOsgMatrix(tf));
+  }
+
+  void updateSize(double size)
+  {
+    mDrawable->updateSize(size);
+  }
+
+  void updateColor(const Eigen::Vector4d& color)
+  {
+    mDrawable->updateColor(color);
+  }
+
+protected:
+  ::osg::ref_ptr<BoxDrawable> mDrawable;
+  ::osg::ref_ptr<::osg::Geode> mGeode;
 };
 
 //==============================================================================
@@ -126,156 +149,67 @@ void VoxelGridShapeNode::refresh()
 }
 
 //==============================================================================
-void VoxelGridShapeNode::extractData(bool /*firstTime*/)
-{
-  if (nullptr == mGeode)
-  {
-    mGeode = new VoxelGridShapeGeode(
-        mVoxelGridShape.get(), mParentShapeFrameNode, this);
-    addChild(mGeode);
-    return;
-  }
-
-  mGeode->refresh();
-}
-
-//==============================================================================
-VoxelGridShapeNode::~VoxelGridShapeNode()
+Eigen::Vector3d toVector3d(const octomap::point3d& point)
 {
-  // Do nothing
+  return Eigen::Vector3d(
+      static_cast<double>(point.x()),
+      static_cast<double>(point.y()),
+      static_cast<double>(point.z()));
 }
 
 //==============================================================================
-VoxelGridShapeGeode::VoxelGridShapeGeode(
-    dynamics::VoxelGridShape* shape,
-    ShapeFrameNode* parentShapeFrame,
-    VoxelGridShapeNode* parentNode)
-  : ShapeNode(parentNode->getShape(), parentShapeFrame, this),
-    mParentNode(parentNode),
-    mVoxelGridShape(shape),
-    mDrawable(nullptr)
-{
-  getOrCreateStateSet()->setMode(GL_BLEND, ::osg::StateAttribute::ON);
-  extractData();
-}
-
-//==============================================================================
-void VoxelGridShapeGeode::refresh()
-{
-  mUtilized = true;
-
-  extractData();
-}
-
-//==============================================================================
-void VoxelGridShapeGeode::extractData()
-{
-  if (nullptr == mDrawable)
-  {
-    mDrawable
-        = new VoxelGridShapeDrawable(mVoxelGridShape, mVisualAspect, this);
-    addDrawable(mDrawable);
-    return;
-  }
-
-  mDrawable->refresh(false);
-}
-
-//==============================================================================
-VoxelGridShapeGeode::~VoxelGridShapeGeode()
-{
-  // Do nothing
-}
-
-//==============================================================================
-VoxelGridShapeDrawable::VoxelGridShapeDrawable(
-    dynamics::VoxelGridShape* shape,
-    dynamics::VisualAspect* visualAspect,
-    VoxelGridShapeGeode* parent)
-  : mVoxelGridShape(shape),
-    mVisualAspect(visualAspect),
-    mParent(parent),
-    mVoxelVersion(dynamics::INVALID_INDEX)
-{
-  refresh(true);
-}
-
-//==============================================================================
-void VoxelGridShapeDrawable::refresh(bool /*firstTime*/)
-{
-  if (mVoxelGridShape->getDataVariance() == dynamics::Shape::STATIC)
-    setDataVariance(::osg::Object::STATIC);
-  else
-    setDataVariance(::osg::Object::DYNAMIC);
-
-  // This function is called whenever the voxel grid version is increased, and
-  // the voxel grid could be updated in the version up. So we always update the
-  // voxel grid.
-  {
-    if (mVoxelVersion != mVoxelGridShape->getVersion())
-    {
-      auto osgShape = new ::osg::CompositeShape();
-      auto octomap = mVoxelGridShape->getOctree();
-      updateBoxes(osgShape, octomap.get(), 0.75);
-
-      setShape(osgShape);
-      dirtyDisplayList();
-    }
-  }
-
-  // This function is called whenever the point voxel grid is increased, and
-  // the color could be updated in the version up. So we always update the
-  // color.
-  {
-    setColor(eigToOsgVec4d(mVisualAspect->getRGBA()));
-  }
-
-  mVoxelVersion = mVoxelGridShape->getVersion();
-}
-
-//==============================================================================
-::osg::Vec3 toVec3(const octomap::point3d& point)
-{
-  return ::osg::Vec3(point.x(), point.y(), point.z());
-}
-
-//==============================================================================
-void VoxelGridShapeDrawable::updateBoxes(
-    ::osg::CompositeShape* osgShape,
-    const octomap::OcTree* tree,
-    double threashold)
+void VoxelGridShapeNode::extractData(bool /*firstTime*/)
 {
-  const auto size = static_cast<float>(tree->getResolution());
-  const auto newNumBoxes = tree->getNumLeafNodes();
-  mBoxes.reserve(newNumBoxes);
+  auto tree = mVoxelGridShape->getOctree();
+  const auto visualSize = tree->getResolution();
+  const auto& color = mVisualAspect->getRGBA();
 
-  // TODO(JS): Use begin_leafs_bbx/end_leafs_bbx to only render voxels that
-  // are in the view sight. For this, camera view frustum would be required.
+  // Pre-allocate for the case that the size of new points are greater than
+  // previous update
+  const auto newVoxels = tree->getNumLeafNodes();
+  mVoxelNodes.reserve(newVoxels);
 
+  // Update position of cache boxes.
   std::size_t boxIndex = 0u;
   for (auto it = tree->begin_leafs(), end = tree->end_leafs(); it != end; ++it)
   {
-    threashold = tree->getOccupancyThres();
+    auto threashold = tree->getOccupancyThres();
 
     if (it->getOccupancy() < threashold)
       continue;
 
-    if (boxIndex < mBoxes.size())
+    if (boxIndex < mVoxelNodes.size())
     {
-      mBoxes[boxIndex]->setCenter(toVec3(it.getCoordinate()));
+      mVoxelNodes[boxIndex]->updateCenter(toVector3d(it.getCoordinate()));
+      mVoxelNodes[boxIndex]->updateSize(visualSize);
+      mVoxelNodes[boxIndex]->updateColor(color);
     }
     else
     {
-      auto osgSphere = new ::osg::Box(toVec3(it.getCoordinate()), size);
-      mBoxes.emplace_back(osgSphere);
+      ::osg::ref_ptr<VoxelNode> voxelNode
+          = new VoxelNode(toVector3d(it.getCoordinate()), visualSize, color);
+      mVoxelNodes.emplace_back(voxelNode);
+      addChild(mVoxelNodes.back());
     }
 
-    osgShape->addChild(mBoxes[boxIndex]);
-
     boxIndex++;
   }
 
-  mBoxes.resize(boxIndex);
+  // Fit the size of cache box list to the new points. No effect new boxes are
+  // added to the list.
+  if (mVoxelNodes.size() > boxIndex)
+  {
+    removeChildren(
+        static_cast<unsigned int>(boxIndex),
+        static_cast<unsigned int>(mVoxelNodes.size() - boxIndex));
+    mVoxelNodes.resize(boxIndex);
+  }
+}
+
+//==============================================================================
+VoxelGridShapeNode::~VoxelGridShapeNode()
+{
+  // Do nothing
 }
 
 } // namespace render

dart/gui/osg/render/VoxelGridShapeNode.hpp

@@ -53,8 +53,9 @@ namespace osg {
 namespace render {
 
 class VoxelGridShapeGeode;
+class VoxelNode;
 
-class VoxelGridShapeNode : public ShapeNode, public ::osg::MatrixTransform
+class VoxelGridShapeNode : public ShapeNode, public ::osg::Group
 {
 public:
   VoxelGridShapeNode(
@@ -68,6 +69,7 @@ class VoxelGridShapeNode : public ShapeNode, public ::osg::MatrixTransform
 
   std::shared_ptr<dynamics::VoxelGridShape> mVoxelGridShape;
   VoxelGridShapeGeode* mGeode;
+  std::vector<::osg::ref_ptr<VoxelNode>> mVoxelNodes;
   std::size_t mVoxelGridVersion;
 };