diff --git a/ros2/src/vtr_navigation/src/modules/optimization/window_optimization_module.cpp b/ros2/src/vtr_navigation/src/modules/optimization/window_optimization_module.cpp
index e85cb32f3..a129ae972 100644
--- a/ros2/src/vtr_navigation/src/modules/optimization/window_optimization_module.cpp
+++ b/ros2/src/vtr_navigation/src/modules/optimization/window_optimization_module.cpp
@@ -822,6 +822,8 @@ void WindowOptimizationModule::updateGraph(QueryCache &qdata, MapCache &mdata,
         pose_a_itr->second.tf_state_var->getValue();
     lgmath::se3::Transformation T_b_0 =
         pose_b_itr->second.tf_state_var->getValue();
+    T_a_0.reproject(true);    // quick fix for lgmath orthonormal issue
+    T_b_0.reproject(true);
     if (pose_b_itr->second.isLocked() == false) {
       if (pose_a_itr->first.majorId() != qdata.live_id->majorId()) {
         continue;
diff --git a/ros2/src/vtr_testing/include/vtr_testing/module_offline.hpp b/ros2/src/vtr_testing/include/vtr_testing/module_offline.hpp
index bdad07135..330c2ea11 100644
--- a/ros2/src/vtr_testing/include/vtr_testing/module_offline.hpp
+++ b/ros2/src/vtr_testing/include/vtr_testing/module_offline.hpp
@@ -98,6 +98,8 @@ class ModuleOffline {
 
     for (; path_itr != graph_->end(); ++path_itr) {
       T_curr = T_curr * path_itr->T();
+      T_curr.reproject(true);
+
       if (path_itr->from().isValid()) {
         LOG(INFO) << path_itr->e()->id();
       }