fixed plotting ar tag poses

nasa · Jun 3, 2024 · acdfaf5 · acdfaf5
1 parent 664e65f
commit acdfaf5
Showing 1 changed file with 2 additions and 2 deletions.
diff --git 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_);