Use pose multiplication instead of subtraction (#231)

Part of gz-math#60. Signed-off-by: Steve Peters <scpeters@openrobotics.org>
gazebosim · Jun 7, 2022 · 9302a7d · 9302a7d
1 parent 3359a0f
commit 9302a7d
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 3 deletions.
diff --git a/src/LogicalCameraSensor.cc b/src/LogicalCameraSensor.cc
@@ -155,7 +155,7 @@ bool LogicalCameraSensor::Update(
       msgs::LogicalCameraImage::Model *modelMsg =
           this->dataPtr->msg.add_model();
       modelMsg->set_name(it.first);
-      msgs::Set(modelMsg->mutable_pose(), it.second - this->Pose());
+      msgs::Set(modelMsg->mutable_pose(), this->Pose().Inverse() * it.second);
     }
   }
   *this->dataPtr->msg.mutable_header()->mutable_stamp() = msgs::Convert(_now);

diff --git a/test/integration/logical_camera.cc b/test/integration/logical_camera.cc
@@ -183,7 +183,7 @@ TEST_F(LogicalCameraSensorTest, DetectBox)
   EXPECT_EQ(sensorPose, gz::msgs::Convert(img.pose()));
   EXPECT_EQ(1, img.model().size());
   EXPECT_EQ(boxName, img.model(0).name());
-  gz::math::Pose3d boxPoseCameraFrame = boxPose - sensorPose;
+  gz::math::Pose3d boxPoseCameraFrame = sensorPose.Inverse() * boxPose;
   EXPECT_EQ(boxPoseCameraFrame, gz::msgs::Convert(img.model(0).pose()));
 
   // 2. test box outside of frustum
@@ -221,7 +221,7 @@ TEST_F(LogicalCameraSensorTest, DetectBox)
   EXPECT_EQ(sensorPose3, gz::msgs::Convert(img.pose()));
   EXPECT_EQ(1, img.model().size());
   EXPECT_EQ(boxName, img.model(0).name());
-  gz::math::Pose3d boxPose3CameraFrame = boxPose3 - sensorPose3;
+  gz::math::Pose3d boxPose3CameraFrame = sensorPose3.Inverse() * boxPose3;
   EXPECT_EQ(boxPose3CameraFrame, gz::msgs::Convert(img.model(0).pose()));
 
   // 4. rotate camera away and image should be empty