Add more detailed timers for pose graph copy

ut-amrl · Dec 8, 2023 · 101db42 · 101db42
1 parent d2b51a8
commit 101db42
Show file tree

Hide file tree

Showing 2 changed files with 44 additions and 3 deletions.
diff --git a/include/analysis/cumulative_timer_constants.h b/include/analysis/cumulative_timer_constants.h
@@ -64,6 +64,11 @@ const std::string kTimerNameGbaPoseGraphCopy = "gba_pose_graph_copy";
 const std::string kTimerNameMapMergeGbaPoseGraphCopy =
     "map_merge_gba_pose_graph_copy";
 
+const std::string kTimerNamePoseGraphCopyVfs = "pose_graph_copy_vfs";
+const std::string kTimerNamePoseGraphCopyPoses = "pose_graph_copy_poses";
+const std::string kTimerNamePoseGraphCopyObjs = "pose_graph_copy_objs";
+const std::string kTimerNamePoseGraphCopySetUpdatedVals = "pose_graph_copy_set_updated";
+
 const std::string kTimerNameIterationLoggerWrite = "iteration_logger_write";
 const std::string kTimerNameLbaOptLoggerWrite = "lba_opt_logger_write";
 const std::string kTimerNameGbaOptLoggerWrite = "gba_opt_logger_write";

diff --git a/include/refactoring/optimization/object_pose_graph.h b/include/refactoring/optimization/object_pose_graph.h
@@ -5,6 +5,8 @@
 #ifndef UT_VSLAM_OBJECT_POSE_GRAPH_H
 #define UT_VSLAM_OBJECT_POSE_GRAPH_H
 
+#include <analysis/cumulative_timer_constants.h>
+#include <analysis/cumulative_timer_factory.h>
 #include <base_lib/basic_utils.h>
 #include <glog/logging.h>
 #include <refactoring/optimization/low_level_feature_pose_graph.h>
@@ -1012,30 +1014,64 @@ class ObjectAndReprojectionFeaturePoseGraph
     // Reset the fields that we don't just want a shallow copy of
     std::unordered_map<ObjectId, EllipsoidEstimateNode>
         ellipsoid_estimates_copy;
+    {
+#ifdef RUN_TIMERS
+      CumulativeFunctionTimer::Invocation invoc(
+          CumulativeTimerFactory::getInstance()
+              .getOrCreateFunctionTimer(kTimerNamePoseGraphCopyObjs)
+              .get());
+#endif
     for (const auto &ellipsoid_est : this->ellipsoid_estimates_) {
       ellipsoid_estimates_copy[ellipsoid_est.first] =
           ellipsoid_est.second.makeDeepCopy();
     }
-    copy->setEllipsoidEstimatePtrs(ellipsoid_estimates_copy);
+
     // ObjectAndReprojectionFeaturePoseGraph::ellipsoid_estimates_ =
     //     ellipsoid_estimates_copy;
+    }
 
     std::unordered_map<FrameId, RobotPoseNode> robot_poses_copy;
+    {
+#ifdef RUN_TIMERS
+      CumulativeFunctionTimer::Invocation invoc(
+          CumulativeTimerFactory::getInstance()
+              .getOrCreateFunctionTimer(kTimerNamePoseGraphCopyPoses)
+              .get());
+#endif
     for (const auto &robot_pose_est : this->robot_poses_) {
       robot_poses_copy[robot_pose_est.first] =
           robot_pose_est.second.makeDeepCopy();
     }
-    copy->setRobotPosePtrs(robot_poses_copy);
     // ObjectAndReprojectionFeaturePoseGraph::robot_poses_ = robot_poses_copy;
+    }
 
     std::unordered_map<FeatureId, VisualFeatureNode> feature_positions_copy;
+    {
+#ifdef RUN_TIMERS
+      CumulativeFunctionTimer::Invocation invoc(
+          CumulativeTimerFactory::getInstance()
+              .getOrCreateFunctionTimer(kTimerNamePoseGraphCopyVfs)
+              .get());
+#endif
     for (const auto &feature_pos_est : this->feature_positions_) {
       feature_positions_copy[feature_pos_est.first] =
           feature_pos_est.second.makeDeepCopy();
     }
-    copy->setFeaturePositionPtrs(feature_positions_copy);
+
+    }
     // ObjectAndReprojectionFeaturePoseGraph::feature_positions_ =
     //     feature_positions_copy;
+    {
+#ifdef RUN_TIMERS
+      CumulativeFunctionTimer::Invocation invoc(
+          CumulativeTimerFactory::getInstance()
+              .getOrCreateFunctionTimer(kTimerNamePoseGraphCopySetUpdatedVals)
+              .get());
+#endif
+    copy->setEllipsoidEstimatePtrs(ellipsoid_estimates_copy);
+    copy->setRobotPosePtrs(robot_poses_copy);
+    copy->setFeaturePositionPtrs(feature_positions_copy);
+    }
     return copy;
   }