Improve voxel grid and point cloud rendering performance

CHANGELOG.md

@@ -4,9 +4,9 @@
 
 #### Changes
 
-* XXX
+* GUI
 
-  * XXX: [#XXXX](https://github.com/dartsim/dart/pull/XXXX)
+  * Improved voxel grid and point cloud rendering performance: [#1294](https://github.com/dartsim/dart/pull/1294)
 
 #### Compilers Tested
 

dart/dynamics/PointCloudShape.cpp

@@ -53,7 +53,7 @@ Eigen::Vector3d toVector3d(const octomap::point3d& point)
 PointCloudShape::PointCloudShape(double visualSize)
   : Shape(), mVisualSize(visualSize)
 {
-  mVariance = DYNAMIC_ELEMENTS;
+  // Do nothing
 }
 
 //==============================================================================
@@ -154,6 +154,12 @@ double PointCloudShape::getVisualSize() const
   return mVisualSize;
 }
 
+//==============================================================================
+void PointCloudShape::notifyColorUpdated(const Eigen::Vector4d& /*color*/)
+{
+  incrementVersion();
+}
+
 //==============================================================================
 void PointCloudShape::updateVolume() const
 {

dart/dynamics/PointCloudShape.hpp

@@ -98,6 +98,9 @@ class PointCloudShape : public Shape
   /// Returns size of visual object that represents each point.
   double getVisualSize() const;
 
+  // Documentation inherited.
+  void notifyColorUpdated(const Eigen::Vector4d& color) override;
+
 protected:
   // Documentation inherited.
   void updateVolume() const override;

dart/dynamics/VoxelGridShape.cpp

@@ -96,8 +96,6 @@ VoxelGridShape::VoxelGridShape(fcl_shared_ptr<octomap::OcTree> octree) : Shape()
   }
 
   setOctree(std::move(octree));
-
-  mVariance = DYNAMIC_ELEMENTS;
 }
 
 //==============================================================================
@@ -212,6 +210,12 @@ Eigen::Matrix3d VoxelGridShape::computeInertia(double /*mass*/) const
   return Eigen::Matrix3d::Identity();
 }
 
+//==============================================================================
+void VoxelGridShape::notifyColorUpdated(const Eigen::Vector4d& /*color*/)
+{
+  incrementVersion();
+}
+
 //==============================================================================
 void VoxelGridShape::updateBoundingBox() const
 {

dart/dynamics/VoxelGridShape.hpp

@@ -143,6 +143,9 @@ class VoxelGridShape : public Shape
   // Documentation inherited.
   Eigen::Matrix3d computeInertia(double mass) const override;
 
+  // Documentation inherited.
+  void notifyColorUpdated(const Eigen::Vector4d& color) override;
+
 protected:
   // Documentation inherited.
   void updateBoundingBox() const override;

dart/gui/osg/render/PointCloudShapeNode.cpp

@@ -73,7 +73,6 @@ class PointCloudShapeDrawable : public ::osg::ShapeDrawable
       ::osg::CompositeShape* osgShape,
       const std::vector<Eigen::Vector3d>& points,
       double size);
-  std::size_t mPointCloudVersion;
   std::vector<::osg::ref_ptr<::osg::Box>> mBoxes;
 };
 
@@ -99,24 +98,32 @@ class PointCloudShapeGeode : public ShapeNode, public ::osg::Geode
 PointCloudShapeNode::PointCloudShapeNode(
     std::shared_ptr<dart::dynamics::PointCloudShape> shape,
     ShapeFrameNode* parent)
-  : ShapeNode(shape, parent, this), mPointCloudShape(shape), mGeode(nullptr)
+  : ShapeNode(shape, parent, this),
+    mPointCloudShape(shape),
+    mGeode(nullptr),
+    mPointCloudVersion(dynamics::INVALID_INDEX)
 {
   mNode = this;
   extractData(true);
-  setNodeMask(mVisualAspect->isHidden() ? 0x0 : ~0x0);
+  setNodeMask(mVisualAspect->isHidden() ? 0x0u : ~0x0u);
 }
 
 //==============================================================================
 void PointCloudShapeNode::refresh()
 {
   mUtilized = true;
 
-  setNodeMask(mVisualAspect->isHidden() ? 0x0 : ~0x0);
+  setNodeMask(mVisualAspect->isHidden() ? 0x0u : ~0x0u);
 
-  if (mShape->getDataVariance() == dart::dynamics::Shape::STATIC)
+  if (mShape->getDataVariance() == dart::dynamics::Shape::STATIC
+      && mPointCloudVersion == mPointCloudShape->getVersion())
+  {
     return;
+  }
 
   extractData(false);
+
+  mPointCloudVersion = mPointCloudShape->getVersion();
 }
 
 //==============================================================================
@@ -184,52 +191,41 @@ PointCloudShapeDrawable::PointCloudShapeDrawable(
     mVisualAspect(visualAspect),
     mVertices(new ::osg::Vec3Array),
     mColors(new ::osg::Vec4Array),
-    mOsgShape(new ::osg::CompositeShape()),
-    mPointCloudVersion(dynamics::INVALID_INDEX)
+    mOsgShape(new ::osg::CompositeShape())
 {
   refresh(true);
 }
 
 //==============================================================================
-void PointCloudShapeDrawable::refresh(bool firstTime)
+void PointCloudShapeDrawable::refresh(bool /*firstTime*/)
 {
   if (mPointCloudShape->getDataVariance() == dart::dynamics::Shape::STATIC)
     setDataVariance(::osg::Object::STATIC);
   else
     setDataVariance(::osg::Object::DYNAMIC);
 
-  if (mPointCloudShape->checkDataVariance(dynamics::Shape::DYNAMIC_ELEMENTS)
-      || firstTime)
+  // This function is called whenever the point cloud version is increased, and
+  // the point cloud could be updated in the version up. So we always update the
+  // point cloud.
   {
-    if (mPointCloudVersion != mPointCloudShape->getVersion())
-    {
-      const auto size = mPointCloudShape->getVisualSize();
-      const auto& points = mPointCloudShape->getPoints();
+    const auto size = mPointCloudShape->getVisualSize();
+    const auto& points = mPointCloudShape->getPoints();
 
-      auto osgShape = new ::osg::CompositeShape();
-      addBoxes(osgShape, points, size);
+    auto osgShape = new ::osg::CompositeShape();
+    addBoxes(osgShape, points, size);
 
-      setShape(osgShape);
-      dirtyDisplayList();
-    }
+    setShape(osgShape);
+    dirtyDisplayList();
   }
 
-  if (mPointCloudShape->checkDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR)
-      || firstTime)
+  // This function is called whenever the point cloud version is increased, and
+  // the color could be updated in the version up. So we always update the
+  // color.
   {
     setColor(eigToOsgVec4(mVisualAspect->getRGBA()));
   }
 }
 
-//==============================================================================
-::osg::Vec3 toVec3(const Eigen::Vector3d& point)
-{
-  return ::osg::Vec3(
-      static_cast<float>(point.x()),
-      static_cast<float>(point.y()),
-      static_cast<float>(point.z()));
-}
-
 //==============================================================================
 void PointCloudShapeDrawable::addBoxes(
     ::osg::CompositeShape* osgShape,
@@ -254,7 +250,7 @@ void PointCloudShapeDrawable::addBoxes(
   for (auto i = mBoxes.size(); i < points.size(); ++i)
   {
     auto osgSphere
-        = new ::osg::Box(toVec3(points[i]), static_cast<float>(size));
+        = new ::osg::Box(eigToOsgVec3(points[i]), static_cast<float>(size));
     mBoxes.emplace_back(osgSphere);
     osgShape->addChild(mBoxes.back());
   }

dart/gui/osg/render/PointCloudShapeNode.hpp

@@ -64,6 +64,7 @@ class PointCloudShapeNode : public ShapeNode, public ::osg::Group
 
   std::shared_ptr<dart::dynamics::PointCloudShape> mPointCloudShape;
   PointCloudShapeGeode* mGeode;
+  std::size_t mPointCloudVersion;
 };
 
 } // namespace render

dart/gui/osg/render/VoxelGridShapeNode.cpp

@@ -98,7 +98,10 @@ class VoxelGridShapeGeode : public ShapeNode, public ::osg::Geode
 //==============================================================================
 VoxelGridShapeNode::VoxelGridShapeNode(
     std::shared_ptr<dynamics::VoxelGridShape> shape, ShapeFrameNode* parent)
-  : ShapeNode(shape, parent, this), mVoxelGridShape(shape), mGeode(nullptr)
+  : ShapeNode(shape, parent, this),
+    mVoxelGridShape(shape),
+    mGeode(nullptr),
+    mVoxelGridVersion(dynamics::INVALID_INDEX)
 {
   extractData(true);
   setNodeMask(mVisualAspect->isHidden() ? 0x0u : ~0x0u);
@@ -111,10 +114,15 @@ void VoxelGridShapeNode::refresh()
 
   setNodeMask(mVisualAspect->isHidden() ? 0x0u : ~0x0u);
 
-  if (mShape->getDataVariance() == dynamics::Shape::STATIC)
+  if (mShape->getDataVariance() == dart::dynamics::Shape::STATIC
+      && mVoxelGridVersion == mVoxelGridShape->getVersion())
+  {
     return;
+  }
 
   extractData(false);
+
+  mVoxelGridVersion = mVoxelGridShape->getVersion();
 }
 
 //==============================================================================
@@ -193,15 +201,16 @@ VoxelGridShapeDrawable::VoxelGridShapeDrawable(
 }
 
 //==============================================================================
-void VoxelGridShapeDrawable::refresh(bool firstTime)
+void VoxelGridShapeDrawable::refresh(bool /*firstTime*/)
 {
   if (mVoxelGridShape->getDataVariance() == dynamics::Shape::STATIC)
     setDataVariance(::osg::Object::STATIC);
   else
     setDataVariance(::osg::Object::DYNAMIC);
 
-  if (mVoxelGridShape->checkDataVariance(dynamics::Shape::DYNAMIC_ELEMENTS)
-      || firstTime)
+  // 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())
     {
@@ -214,8 +223,9 @@ void VoxelGridShapeDrawable::refresh(bool firstTime)
     }
   }
 
-  if (mVoxelGridShape->checkDataVariance(dynamics::Shape::DYNAMIC_COLOR)
-      || firstTime)
+  // 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(eigToOsgVec4(mVisualAspect->getRGBA()));
   }

dart/gui/osg/render/VoxelGridShapeNode.hpp

@@ -68,6 +68,7 @@ class VoxelGridShapeNode : public ShapeNode, public ::osg::MatrixTransform
 
   std::shared_ptr<dynamics::VoxelGridShape> mVoxelGridShape;
   VoxelGridShapeGeode* mGeode;
+  std::size_t mVoxelGridVersion;
 };
 
 } // namespace render

examples/osgExamples/osgPointCloud/main.cpp

@@ -43,7 +43,7 @@ using namespace dart::common;
 using namespace dart::dynamics;
 using namespace dart::math;
 
-const std::string& robotName = "KR5";
+static const std::string& robotName = "KR5";
 
 class PointCloudWorld : public gui::osg::WorldNode
 {
@@ -72,13 +72,16 @@ class PointCloudWorld : public gui::osg::WorldNode
     if (!mRobot)
       return;
 
+    if (!mUpdate)
+      return;
+
     // Set robot pose
     Eigen::VectorXd pos = mRobot->getPositions();
     pos += 0.01 * Eigen::VectorXd::Random(pos.size());
     mRobot->setPositions(pos);
 
     // Generate point cloud from robot meshes
-    auto pointCloud = generatePointCloud(100);
+    auto pointCloud = generatePointCloud(500);
 
     // Update sensor position
     static double time = 0.0;
@@ -105,11 +108,22 @@ class PointCloudWorld : public gui::osg::WorldNode
   {
     return mPointCloudVisualAspect;
   }
+
   dynamics::VisualAspect* getVoxelGridVisualAspect()
   {
     return mVoxelGridVisualAspect;
   }
 
+  void setUpdate(bool update)
+  {
+    mUpdate = update;
+  }
+
+  bool getUpdate() const
+  {
+    return mUpdate;
+  }
+
 protected:
   octomap::Pointcloud generatePointCloud(std::size_t numPoints)
   {
@@ -173,6 +187,8 @@ class PointCloudWorld : public gui::osg::WorldNode
 
   dynamics::VisualAspect* mPointCloudVisualAspect;
   dynamics::VisualAspect* mVoxelGridVisualAspect;
+
+  bool mUpdate{true};
 };
 
 class PointCloudWidget : public dart::gui::osg::ImGuiWidget
@@ -247,6 +263,15 @@ class PointCloudWidget : public dart::gui::osg::ImGuiWidget
         if (ImGui::RadioButton("Pause", &e, 1) && mViewer->isSimulating())
           mViewer->simulate(false);
       }
+
+      int robotUpdate = mNode->getUpdate() ? 0 : 1;
+      if (ImGui::RadioButton("Run Robot Updating", &robotUpdate, 0)
+          && mNode->getUpdate())
+        mNode->setUpdate(true);
+      ImGui::SameLine();
+      if (ImGui::RadioButton("Stop Robot Updating", &robotUpdate, 1)
+          && mNode->getUpdate())
+        mNode->setUpdate(false);
     }
 
     if (ImGui::CollapsingHeader("View", ImGuiTreeNodeFlags_DefaultOpen))
@@ -329,26 +354,26 @@ class PointCloudWidget : public dart::gui::osg::ImGuiWidget
           minorLinesPerMajorLine
               = static_cast<int>(mGrid->getNumMinorLinesPerMajorLine());
           if (ImGui::InputInt(
-                "Minor Lines per Major Line", &minorLinesPerMajorLine, 1, 5))
+                  "Minor Lines per Major Line", &minorLinesPerMajorLine, 1, 5))
           {
             if (minorLinesPerMajorLine < 0)
               minorLinesPerMajorLine = 0;
             mGrid->setNumMinorLinesPerMajorLine(
-                  static_cast<std::size_t>(minorLinesPerMajorLine));
+                static_cast<std::size_t>(minorLinesPerMajorLine));
           }
 
           static float axisLineWidth;
           axisLineWidth = mGrid->getAxisLineWidth();
           if (ImGui::InputFloat(
-                "Axis Line Width", &axisLineWidth, 1.f, 2.f, "%.0f"))
+                  "Axis Line Width", &axisLineWidth, 1.f, 2.f, "%.0f"))
           {
             mGrid->setAxisLineWidth(axisLineWidth);
           }
 
           static float majorLineWidth;
           majorLineWidth = mGrid->getMajorLineWidth();
           if (ImGui::InputFloat(
-                "Major Line Width", &majorLineWidth, 1.f, 2.f, "%.0f"))
+                  "Major Line Width", &majorLineWidth, 1.f, 2.f, "%.0f"))
           {
             mGrid->setMajorLineWidth(majorLineWidth);
           }
@@ -369,7 +394,7 @@ class PointCloudWidget : public dart::gui::osg::ImGuiWidget
           static float minorLineWidth;
           minorLineWidth = mGrid->getMinorLineWidth();
           if (ImGui::InputFloat(
-                "Minor Line Width", &minorLineWidth, 1.f, 2.f, "%.0f"))
+                  "Minor Line Width", &minorLineWidth, 1.f, 2.f, "%.0f"))
           {
             mGrid->setMinorLineWidth(minorLineWidth);
           }