Fix for lgmath orthonormal bug when plotting edges.

See utiasASRL/lgmath#41
utiasASRL · May 19, 2021 · 86c7522 · 86c7522
1 parent 4cc8466
commit 86c7522
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 0 deletions.
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
@@ -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
@@ -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();
       }