Visual::SetPose performance improvement / minor fixes (#3350)

Cache sdf pose element in Visual::SetPose to speed up the
poseMsgs profiling block in Scene::PreRender by 10x for
some use cases.

Also fix undefined behavior in Scene.cc

Signed-off-by: Janosch Machowinski <J.Machowinski@cellumation.com>
Co-authored-by: Janosch Machowinski <J.Machowinski@cellumation.com>

gazebo/rendering/Scene.cc

@@ -2142,8 +2142,7 @@ void Scene::PreRender()
           ignition::math::Pose3d pose = msgs::ConvertIgn(pIter->second);
           GZ_ASSERT(iter->second, "Visual pointer is NULL");
           iter->second->SetPose(pose);
-          PoseMsgs_M::iterator prev = pIter++;
-          this->dataPtr->poseMsgs.erase(prev);
+          pIter = this->dataPtr->poseMsgs.erase(pIter);
         }
         else
           ++pIter;
@@ -2157,8 +2156,7 @@ void Scene::PreRender()
           ignition::math::Pose3d pose = msgs::ConvertIgn(pIter->second);
           lIter->second->SetPosition(pose.Pos());
           lIter->second->SetRotation(pose.Rot());
-          auto prev = pIter++;
-          this->dataPtr->poseMsgs.erase(prev);
+          pIter = this->dataPtr->poseMsgs.erase(pIter);
         }
         else
           ++pIter;

gazebo/rendering/Visual.cc

@@ -243,6 +243,8 @@ void Visual::Fini()
   }
   this->dataPtr->scene.reset();
 
+  dataPtr->poseElem.reset();
+
   if (this->dataPtr->sdf)
     this->dataPtr->sdf->Reset();
   this->dataPtr->sdf.reset();
@@ -458,6 +460,7 @@ void Visual::Load()
   }
 
   // Set the pose of the scene node
+  this->dataPtr->poseElem.reset();
   this->SetPose(pose);
   this->dataPtr->initialRelativePose = pose;
 
@@ -1984,14 +1987,25 @@ void Visual::SetRotation(const ignition::math::Quaterniond &_rot)
   this->dataPtr->sceneNode->setOrientation(
       Ogre::Quaternion(_rot.W(), _rot.X(), _rot.Y(), _rot.Z()));
 
-  this->dataPtr->sdf->GetElement("pose")->Set(this->Pose());
+  if(!dataPtr->poseElem)
+  {
+    dataPtr->poseElem = this->dataPtr->sdf->GetElement("pose");
+  }
+
+  dataPtr->poseElem->Set(this->Pose());
 }
 
 //////////////////////////////////////////////////
 void Visual::SetPose(const ignition::math::Pose3d &_pose)
 {
-  this->SetPosition(_pose.Pos());
-  this->SetRotation(_pose.Rot());
+  this->dataPtr->sceneNode->setPosition(_pose.Pos().X(), _pose.Pos().Y(), _pose.Pos().Z());
+  this->dataPtr->sceneNode->setOrientation(
+      Ogre::Quaternion(_pose.Rot().W(), _pose.Rot().X(), _pose.Rot().Y(), _pose.Rot().Z()));
+  if(!dataPtr->poseElem)
+  {
+    dataPtr->poseElem = this->dataPtr->sdf->GetElement("pose");
+  }
+  dataPtr->poseElem->Set(this->Pose());
 }
 
 //////////////////////////////////////////////////

gazebo/rendering/VisualPrivate.hh

@@ -93,6 +93,7 @@ namespace gazebo
 
       /// \brief The SDF element for the visual.
       public: sdf::ElementPtr sdf;
+      public: sdf::ElementPtr poseElem;
 
       /// \brief The unique name for the visual's material.
       public: std::string myMaterialName;