From dfc9df54ed1d79fe0e8849fcc65a2044aa49b7a1 Mon Sep 17 00:00:00 2001
From: Steve Nogar <stephen.m.nogar.civ@mail.mil>
Date: Thu, 10 Feb 2022 17:05:35 -0500
Subject: [PATCH] small tweaks to fix clang build errors/warnings

---
 ov_msckf/CMakeLists.txt              | 2 +-
 ov_msckf/src/ros/ROS2Visualizer.cpp  | 4 +++-
 ov_msckf/src/update/UpdaterMSCKF.cpp | 2 +-
 ov_msckf/src/update/UpdaterSLAM.cpp  | 4 ++--
 4 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt
index bcf045080..2e005304d 100644
--- a/ov_msckf/CMakeLists.txt
+++ b/ov_msckf/CMakeLists.txt
@@ -30,7 +30,7 @@ set(CMAKE_CXX_EXTENSIONS OFF)
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops")
 
 # Enable debug flags (use if you want to debug in gdb)
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -Wmaybe-uninitialized -fno-omit-frame-pointer")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -fno-omit-frame-pointer")
 
 # Find our ROS version!
 # NOTE: Default to using the ROS1 package if both are in our enviroment
diff --git a/ov_msckf/src/ros/ROS2Visualizer.cpp b/ov_msckf/src/ros/ROS2Visualizer.cpp
index f8b6f7cf9..925b4b773 100644
--- a/ov_msckf/src/ros/ROS2Visualizer.cpp
+++ b/ov_msckf/src/ros/ROS2Visualizer.cpp
@@ -656,7 +656,9 @@ void ROS2Visualizer::publish_groundtruth() {
   Eigen::Matrix<double, 6, 6> covariance = StateHelper::get_marginal_covariance(_app->get_state(), statevars);
 
   // Calculate NEES values
-  double ori_nees = 2 * quat_diff.block(0, 0, 3, 1).dot(covariance.block(0, 0, 3, 3).inverse() * 2 * quat_diff.block(0, 0, 3, 1));
+  Eigen::Vector3d quat_diff_vec = quat_diff.block(0, 0, 3, 1);
+  Eigen::Vector3d cov_vec = covariance.block(0, 0, 3, 3).inverse() * 2 * quat_diff.block(0, 0, 3, 1);
+  double ori_nees = 2 * quat_diff_vec.dot(cov_vec);
   Eigen::Vector3d errpos = state_ekf.block(4, 0, 3, 1) - state_gt.block(5, 0, 3, 1);
   double pos_nees = errpos.transpose() * covariance.block(3, 3, 3, 3).inverse() * errpos;
 
diff --git a/ov_msckf/src/update/UpdaterMSCKF.cpp b/ov_msckf/src/update/UpdaterMSCKF.cpp
index d1c2f2c3b..9de74691e 100644
--- a/ov_msckf/src/update/UpdaterMSCKF.cpp
+++ b/ov_msckf/src/update/UpdaterMSCKF.cpp
@@ -32,7 +32,7 @@ void UpdaterMSCKF::update(std::shared_ptr<State> state, std::vector<std::shared_
     return;
 
   // Start timing
-  boost::posix_time::ptime rT0, rT1, rT2, rT3, rT4, rT5, rT6, rT7;
+  boost::posix_time::ptime rT0, rT1, rT2, rT3, rT4, rT5;
   rT0 = boost::posix_time::microsec_clock::local_time();
 
   // 0. Get all timestamps our clones are at (and thus valid measurement times)
diff --git a/ov_msckf/src/update/UpdaterSLAM.cpp b/ov_msckf/src/update/UpdaterSLAM.cpp
index 401f8e821..d9c92918a 100644
--- a/ov_msckf/src/update/UpdaterSLAM.cpp
+++ b/ov_msckf/src/update/UpdaterSLAM.cpp
@@ -32,7 +32,7 @@ void UpdaterSLAM::delayed_init(std::shared_ptr<State> state, std::vector<std::sh
     return;
 
   // Start timing
-  boost::posix_time::ptime rT0, rT1, rT2, rT3, rT4, rT5, rT6, rT7;
+  boost::posix_time::ptime rT0, rT1, rT2, rT3;
   rT0 = boost::posix_time::microsec_clock::local_time();
 
   // 0. Get all timestamps our clones are at (and thus valid measurement times)
@@ -224,7 +224,7 @@ void UpdaterSLAM::update(std::shared_ptr<State> state, std::vector<std::shared_p
     return;
 
   // Start timing
-  boost::posix_time::ptime rT0, rT1, rT2, rT3, rT4, rT5, rT6, rT7;
+  boost::posix_time::ptime rT0, rT1, rT2, rT3;
   rT0 = boost::posix_time::microsec_clock::local_time();
 
   // 0. Get all timestamps our clones are at (and thus valid measurement times)