From 200acfbd67a3ef72ba931d33f7cb3d2a7f4d5938 Mon Sep 17 00:00:00 2001
From: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Date: Thu, 21 Dec 2023 11:36:16 +0100
Subject: [PATCH] Simplify/Fix Isometry inverse benchmarks

---
 .../test/robot_state_benchmark.cpp            | 27 +++++++------------
 1 file changed, 9 insertions(+), 18 deletions(-)

diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp
index 5a171c6e08..18f3dcb100 100644
--- a/moveit_core/robot_state/test/robot_state_benchmark.cpp
+++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp
@@ -125,9 +125,9 @@ static void multiplyIsometryTimesIsometry(benchmark::State& st)
 static void inverseIsometry3d(benchmark::State& st)
 {
   Eigen::Isometry3d isometry = createTestIsometry();
+  Eigen::Isometry3d result;
   for (auto _ : st)
   {
-    Eigen::Isometry3d result;
     benchmark::DoNotOptimize(result = isometry.inverse());
     benchmark::ClobberMemory();
   }
@@ -136,14 +136,11 @@ static void inverseIsometry3d(benchmark::State& st)
 // Benchmark time to invert an Eigen::Affine3d(Eigen::Isometry).
 static void inverseAffineIsometry(benchmark::State& st)
 {
-  Eigen::Isometry3d isometry = createTestIsometry();
-  Eigen::Affine3d affine;
-  affine.matrix() = isometry.matrix();
-
+  Eigen::Affine3d affine = createTestIsometry();
+  Eigen::Affine3d result;
   for (auto _ : st)
   {
-    Eigen::Affine3d result;
-    benchmark::DoNotOptimize(result = affine.inverse(Eigen::Isometry).affine());
+    benchmark::DoNotOptimize(result = affine.inverse(Eigen::Isometry));
     benchmark::ClobberMemory();
   }
 }
@@ -151,14 +148,11 @@ static void inverseAffineIsometry(benchmark::State& st)
 // Benchmark time to invert an Eigen::Affine3d.
 static void inverseAffine(benchmark::State& st)
 {
-  Eigen::Isometry3d isometry = createTestIsometry();
-  Eigen::Affine3d affine;
-  affine.matrix() = isometry.matrix();
-
+  Eigen::Affine3d affine = createTestIsometry();
+  Eigen::Affine3d result;
   for (auto _ : st)
   {
-    Eigen::Affine3d result;
-    benchmark::DoNotOptimize(result = affine.inverse().affine());
+    benchmark::DoNotOptimize(result = affine.inverse());
     benchmark::ClobberMemory();
   }
 }
@@ -166,13 +160,10 @@ static void inverseAffine(benchmark::State& st)
 // Benchmark time to invert an Eigen::Matrix4d.
 static void inverseMatrix4d(benchmark::State& st)
 {
-  Eigen::Isometry3d isometry = createTestIsometry();
-  Eigen::Affine3d affine;
-  affine.matrix() = isometry.matrix();
-
+  Eigen::Affine3d affine = createTestIsometry();
+  Eigen::Affine3d result;
   for (auto _ : st)
   {
-    Eigen::Affine3d result;
     benchmark::DoNotOptimize(result = affine.matrix().inverse());
     benchmark::ClobberMemory();
   }