From acdfaf5109e975113a08634ca83874512c6dff25 Mon Sep 17 00:00:00 2001
From: Ryan Soussan <ryan.soussan@gmail.com>
Date: Sun, 2 Jun 2024 23:11:05 -0700
Subject: [PATCH] fixed plotting ar tag poses

---
 tools/localization_analysis/src/offline_replay.cc | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/tools/localization_analysis/src/offline_replay.cc b/tools/localization_analysis/src/offline_replay.cc
index 87c5076eab..0bf3591a1a 100644
--- a/tools/localization_analysis/src/offline_replay.cc
+++ b/tools/localization_analysis/src/offline_replay.cc
@@ -182,9 +182,9 @@ void OfflineReplay::Run() {
         pose_extrapolator_wrapper_.LocalizationStateCallback(*localization_msg);
         SaveMsg(*localization_msg, TOPIC_GRAPH_LOC_STATE, results_bag_);
       }
-      if (latest_ar_msg_) {
+      if (latest_ar_msg_ && graph_localizer_simulator_->WorldTDock()) {
         if (static_cast<int>(ar_msg->landmarks.size()) >= params_.ar_min_num_landmarks) {
-          const gtsam::Pose3 world_T_body =
+          const gtsam::Pose3 world_T_body = (*graph_localizer_simulator_->WorldTDock())*
             lc::PoseFromMsgWithExtrinsics(ar_msg->pose, params_.body_T_dock_cam.inverse());
           const lc::Time timestamp = lc::TimeFromHeader(ar_msg->header);
           SaveMsg(PoseMsg(world_T_body, timestamp), TOPIC_AR_TAG_POSE, results_bag_);