diff --git a/fuse_constraints/src/variable_constraints.cpp b/fuse_constraints/src/variable_constraints.cpp
index 5a80953fd..8be6b3f49 100755
--- a/fuse_constraints/src/variable_constraints.cpp
+++ b/fuse_constraints/src/variable_constraints.cpp
@@ -52,9 +52,9 @@ bool VariableConstraints::empty() const
 size_t VariableConstraints::size() const
 {
   auto sum_edges = [](const size_t input, const ConstraintCollection & edges)
-  {
-    return input + edges.size();
-  };
+    {
+      return input + edges.size();
+    };
   return std::accumulate(variable_constraints_.begin(), variable_constraints_.end(), 0u, sum_edges);
 }
 
diff --git a/fuse_core/include/fuse_core/ceres_options.hpp b/fuse_core/include/fuse_core/ceres_options.hpp
index 6b7853f3c..e364b2118 100644
--- a/fuse_core/include/fuse_core/ceres_options.hpp
+++ b/fuse_core/include/fuse_core/ceres_options.hpp
@@ -54,10 +54,10 @@
  * For a given Ceres Solver Option <T>, the function ToString calls ceres::<T>ToString
  */
 #define CERES_OPTION_TO_STRING_DEFINITION(Option) \
-        static inline const char * ToString(ceres::Option value) \
-        { \
-          return ceres::Option ## ToString(value); \
-        }
+  static inline const char * ToString(ceres::Option value) \
+  { \
+    return ceres::Option ## ToString(value); \
+  }
 
 /**
  * Defines FromString overloaded function to Ceres Options.
@@ -65,10 +65,10 @@
  * For a given Ceres Solver Option <T>, the function FromString calls ceres::StringTo<T>
  */
 #define CERES_OPTION_FROM_STRING_DEFINITION(Option) \
-        static inline bool FromString(std::string string_value, ceres::Option * value) \
-        { \
-          return ceres::StringTo ## Option(string_value, value); \
-        }
+  static inline bool FromString(std::string string_value, ceres::Option * value) \
+  { \
+    return ceres::StringTo ## Option(string_value, value); \
+  }
 
 /**
  * Defines ToString and FromString overloaded functions for Ceres Options.
@@ -76,8 +76,8 @@
  * See CERES_OPTION_TO_STRING_DEFINITION and CERES_OPTION_FROM_STRING_DEFINITION.
  */
 #define CERES_OPTION_STRING_DEFINITIONS(Option) \
-        CERES_OPTION_TO_STRING_DEFINITION(Option) \
-        CERES_OPTION_FROM_STRING_DEFINITION(Option)
+  CERES_OPTION_TO_STRING_DEFINITION(Option) \
+  CERES_OPTION_FROM_STRING_DEFINITION(Option)
 
 #if !CERES_VERSION_AT_LEAST(2, 0, 0)
 /**
diff --git a/fuse_core/include/fuse_core/constraint.hpp b/fuse_core/include/fuse_core/constraint.hpp
index 553e3dfb6..1abd6d16d 100644
--- a/fuse_core/include/fuse_core/constraint.hpp
+++ b/fuse_core/include/fuse_core/constraint.hpp
@@ -67,10 +67,10 @@
  * @endcode
  */
 #define FUSE_CONSTRAINT_CLONE_DEFINITION(...) \
-        fuse_core::Constraint::UniquePtr clone() const override \
-        { \
-          return __VA_ARGS__::make_unique(*this); \
-        }
+  fuse_core::Constraint::UniquePtr clone() const override \
+  { \
+    return __VA_ARGS__::make_unique(*this); \
+  }
 
 /**
  * @brief Implementation of the serialize() and deserialize() member functions for derived classes
@@ -86,22 +86,22 @@
  * @endcode
  */
 #define FUSE_CONSTRAINT_SERIALIZE_DEFINITION(...) \
-        void serialize(fuse_core::BinaryOutputArchive & archive) const override \
-        { \
-          archive << *this; \
-        }  /* NOLINT */ \
-        void serialize(fuse_core::TextOutputArchive & archive) const override \
-        { \
-          archive << *this; \
-        }  /* NOLINT */ \
-        void deserialize(fuse_core::BinaryInputArchive & archive) override \
-        { \
-          archive >> *this; \
-        }  /* NOLINT */ \
-        void deserialize(fuse_core::TextInputArchive & archive) override \
-        { \
-          archive >> *this; \
-        }
+  void serialize(fuse_core::BinaryOutputArchive & archive) const override \
+  { \
+    archive << *this; \
+  }  /* NOLINT */ \
+  void serialize(fuse_core::TextOutputArchive & archive) const override \
+  { \
+    archive << *this; \
+  }  /* NOLINT */ \
+  void deserialize(fuse_core::BinaryInputArchive & archive) override \
+  { \
+    archive >> *this; \
+  }  /* NOLINT */ \
+  void deserialize(fuse_core::TextInputArchive & archive) override \
+  { \
+    archive >> *this; \
+  }
 
 /**
  * @brief Implements the type() member function using the suggested implementation
@@ -119,17 +119,17 @@
  * @endcode
  */
 #define FUSE_CONSTRAINT_TYPE_DEFINITION(...) \
-        struct detail \
-        { \
-          static std::string type() \
-          { \
-            return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \
-          }  /* NOLINT */ \
-        };  /* NOLINT */ \
-        std::string type() const override \
-        { \
-          return detail::type(); \
-        }
+  struct detail \
+  { \
+    static std::string type() \
+    { \
+      return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \
+    }  /* NOLINT */ \
+  };  /* NOLINT */ \
+  std::string type() const override \
+  { \
+    return detail::type(); \
+  }
 
 /**
  * @brief Convenience function that creates the required pointer aliases, clone() method, and type()
@@ -146,10 +146,10 @@
  * @endcode
  */
 #define FUSE_CONSTRAINT_DEFINITIONS(...) \
-        FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \
-        FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \
-        FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \
-        FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__)
+  FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \
+  FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \
+  FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \
+  FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__)
 
 /**
  * @brief Convenience function that creates the required pointer aliases, clone() method, and type()
@@ -166,10 +166,10 @@
  * @endcode
  */
 #define FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(...) \
-        FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \
-        FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \
-        FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \
-        FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__)
+  FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \
+  FUSE_CONSTRAINT_TYPE_DEFINITION(__VA_ARGS__) \
+  FUSE_CONSTRAINT_CLONE_DEFINITION(__VA_ARGS__) \
+  FUSE_CONSTRAINT_SERIALIZE_DEFINITION(__VA_ARGS__)
 
 
 namespace fuse_core
diff --git a/fuse_core/include/fuse_core/eigen_gtest.hpp b/fuse_core/include/fuse_core/eigen_gtest.hpp
index 7205470ff..5fe3d760c 100644
--- a/fuse_core/include/fuse_core/eigen_gtest.hpp
+++ b/fuse_core/include/fuse_core/eigen_gtest.hpp
@@ -108,23 +108,23 @@ AssertionResult AssertMatrixNearHelper(
 // Internal macro for implementing {EXPECT|ASSERT}_MATRIX_EQ.
 // Don't use this in your code.
 #define GTEST_MATRIX_EQUAL_(v1, v2, on_failure) \
-        GTEST_ASSERT_( \
-          ::testing::AssertMatrixEqualHelper( \
-            #v1, \
-            #v2, \
-            v1, \
-            v2), on_failure)
+  GTEST_ASSERT_( \
+    ::testing::AssertMatrixEqualHelper( \
+      #v1, \
+      #v2, \
+      v1, \
+      v2), on_failure)
 
 // Internal macro for implementing {EXPECT|ASSERT}_MATRIX_NEAR.
 // Don't use this in your code.
 #define GTEST_MATRIX_NEAR_(v1, v2, tol, on_failure) \
-        GTEST_ASSERT_( \
-          ::testing::AssertMatrixNearHelper( \
-            #v1, \
-            #v2, \
-            v1, \
-            v2, \
-            tol), on_failure)
+  GTEST_ASSERT_( \
+    ::testing::AssertMatrixNearHelper( \
+      #v1, \
+      #v2, \
+      v1, \
+      v2, \
+      tol), on_failure)
 
 // Define gtest macros for use with Eigen
 
@@ -137,7 +137,7 @@ AssertionResult AssertMatrixNearHelper(
  * @param[in] v2 The actual matrix
  */
 #define EXPECT_MATRIX_EQ(v1, v2) \
-        GTEST_MATRIX_EQUAL_(v1, v2, GTEST_NONFATAL_FAILURE_)
+  GTEST_MATRIX_EQUAL_(v1, v2, GTEST_NONFATAL_FAILURE_)
 
 /**
  * @brief Fatal check for exact equality of two Eigen matrix-like objects.
@@ -148,7 +148,7 @@ AssertionResult AssertMatrixNearHelper(
  * @param[in] v2 The actual matrix
  */
 #define ASSERT_MATRIX_EQ(v1, v2) \
-        GTEST_MATRIX_EQUAL_(v1, v2, GTEST_FATAL_FAILURE_)
+  GTEST_MATRIX_EQUAL_(v1, v2, GTEST_FATAL_FAILURE_)
 
 /**
  * @brief Non-fatal check for approximate equality of two Eigen matrix-like objects.
@@ -160,7 +160,7 @@ AssertionResult AssertMatrixNearHelper(
  * @param[in] tol The allowed tolerance between any entries in v1 and v2
  */
 #define EXPECT_MATRIX_NEAR(v1, v2, tol) \
-        GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_NONFATAL_FAILURE_)
+  GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_NONFATAL_FAILURE_)
 
 /**
  * @brief Fatal check for approximate equality of two Eigen matrix-like objects.
@@ -172,7 +172,7 @@ AssertionResult AssertMatrixNearHelper(
  * @param[in] tol The allowed tolerance between any entries in v1 and v2
  */
 #define ASSERT_MATRIX_NEAR(v1, v2, tol) \
-        GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_FATAL_FAILURE_)
+  GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_FATAL_FAILURE_)
 
 }  // namespace testing
 
diff --git a/fuse_core/include/fuse_core/fuse_macros.hpp b/fuse_core/include/fuse_core/fuse_macros.hpp
index a896a4c75..707fdf101 100644
--- a/fuse_core/include/fuse_core/fuse_macros.hpp
+++ b/fuse_core/include/fuse_core/fuse_macros.hpp
@@ -86,11 +86,11 @@
  * Use in the public section of the class.
  */
 #define FUSE_SMART_PTR_DEFINITIONS(...) \
-        __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \
-        __FUSE_MAKE_SHARED_DEFINITION(__VA_ARGS__) \
-        __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \
-        __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \
-        __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__)
+  __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \
+  __FUSE_MAKE_SHARED_DEFINITION(__VA_ARGS__) \
+  __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \
+  __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \
+  __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__)
 
 /**
  * Defines smart pointer aliases and static functions for a class that contains fixed-sized
@@ -104,15 +104,15 @@
  */
 #if __cpp_aligned_new
   #define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \
-          FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__)
+    FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__)
 #else
   #define FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \
-          EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
-          __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \
-          __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \
-          __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \
-          __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \
-          __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__)
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
+    __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \
+    __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \
+    __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \
+    __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__) \
+    __FUSE_MAKE_UNIQUE_DEFINITION(__VA_ARGS__)
 #endif
 
 /**
@@ -124,55 +124,55 @@
  * Use in the public section of the class.
  */
 #define FUSE_SMART_PTR_ALIASES_ONLY(...) \
-        __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \
-        __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \
-        __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__)
+  __FUSE_SHARED_PTR_ALIAS(__VA_ARGS__) \
+  __FUSE_WEAK_PTR_ALIAS(__VA_ARGS__) \
+  __FUSE_UNIQUE_PTR_ALIAS(__VA_ARGS__)
 
 #define __FUSE_SHARED_PTR_ALIAS(...) \
-        using SharedPtr = std::shared_ptr<__VA_ARGS__>; \
-        using ConstSharedPtr = std::shared_ptr<const __VA_ARGS__>;
+  using SharedPtr = std::shared_ptr<__VA_ARGS__>; \
+  using ConstSharedPtr = std::shared_ptr<const __VA_ARGS__>;
 
 #define __FUSE_MAKE_SHARED_DEFINITION(...) \
-        template<typename ... Args> \
-        static std::shared_ptr<__VA_ARGS__> \
-        make_shared(Args && ... args) \
-        { \
-          return std::make_shared<__VA_ARGS__>(std::forward<Args>(args) ...); \
-        }
+  template<typename ... Args> \
+  static std::shared_ptr<__VA_ARGS__> \
+  make_shared(Args && ... args) \
+  { \
+    return std::make_shared<__VA_ARGS__>(std::forward<Args>(args) ...); \
+  }
 
 #define __FUSE_MAKE_SHARED_ALIGNED_DEFINITION(...) \
-        template<typename ... Args> \
-        static std::shared_ptr<__VA_ARGS__> \
-        make_shared(Args && ... args) \
-        { \
-          return std::allocate_shared<__VA_ARGS__>( \
-            Eigen::aligned_allocator<__VA_ARGS__>(), \
-            std::forward<Args>(args) ...); \
-        }
+  template<typename ... Args> \
+  static std::shared_ptr<__VA_ARGS__> \
+  make_shared(Args && ... args) \
+  { \
+    return std::allocate_shared<__VA_ARGS__>( \
+      Eigen::aligned_allocator<__VA_ARGS__>(), \
+      std::forward<Args>(args) ...); \
+  }
 
 #define __FUSE_WEAK_PTR_ALIAS(...) \
-        using WeakPtr = std::weak_ptr<__VA_ARGS__>; \
-        using ConstWeakPtr = std::weak_ptr<const __VA_ARGS__>;
+  using WeakPtr = std::weak_ptr<__VA_ARGS__>; \
+  using ConstWeakPtr = std::weak_ptr<const __VA_ARGS__>;
 
 #define __FUSE_UNIQUE_PTR_ALIAS(...) \
-        using UniquePtr = std::unique_ptr<__VA_ARGS__>;
+  using UniquePtr = std::unique_ptr<__VA_ARGS__>;
 
 #if __cplusplus >= 201402L
   #define __FUSE_MAKE_UNIQUE_DEFINITION(...) \
-          template<typename ... Args> \
-          static std::unique_ptr<__VA_ARGS__> \
-          make_unique(Args && ... args) \
-          { \
-            return std::make_unique<__VA_ARGS__>(std::forward<Args>(args) ...); \
-          }
+    template<typename ... Args> \
+    static std::unique_ptr<__VA_ARGS__> \
+    make_unique(Args && ... args) \
+    { \
+      return std::make_unique<__VA_ARGS__>(std::forward<Args>(args) ...); \
+    }
 #else
   #define __FUSE_MAKE_UNIQUE_DEFINITION(...) \
-          template<typename ... Args> \
-          static std::unique_ptr<__VA_ARGS__> \
-          make_unique(Args && ... args) \
-          { \
-            return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward<Args>(args) ...)); \
-          }
+    template<typename ... Args> \
+    static std::unique_ptr<__VA_ARGS__> \
+    make_unique(Args && ... args) \
+    { \
+      return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward<Args>(args) ...)); \
+    }
 #endif
 
 #endif  // FUSE_CORE__FUSE_MACROS_HPP_
diff --git a/fuse_core/include/fuse_core/graph.hpp b/fuse_core/include/fuse_core/graph.hpp
index 4b9c96c0f..a9f366c84 100644
--- a/fuse_core/include/fuse_core/graph.hpp
+++ b/fuse_core/include/fuse_core/graph.hpp
@@ -71,22 +71,22 @@
  * @endcode
  */
 #define FUSE_GRAPH_SERIALIZE_DEFINITION(...) \
-        void serialize(fuse_core::BinaryOutputArchive & archive) const override \
-        { \
-          archive << *this; \
-        }  /* NOLINT */ \
-        void serialize(fuse_core::TextOutputArchive & archive) const override \
-        { \
-          archive << *this; \
-        }  /* NOLINT */ \
-        void deserialize(fuse_core::BinaryInputArchive & archive) override \
-        { \
-          archive >> *this; \
-        }  /* NOLINT */ \
-        void deserialize(fuse_core::TextInputArchive & archive) override \
-        { \
-          archive >> *this; \
-        }
+  void serialize(fuse_core::BinaryOutputArchive & archive) const override \
+  { \
+    archive << *this; \
+  }  /* NOLINT */ \
+  void serialize(fuse_core::TextOutputArchive & archive) const override \
+  { \
+    archive << *this; \
+  }  /* NOLINT */ \
+  void deserialize(fuse_core::BinaryInputArchive & archive) override \
+  { \
+    archive >> *this; \
+  }  /* NOLINT */ \
+  void deserialize(fuse_core::TextInputArchive & archive) override \
+  { \
+    archive >> *this; \
+  }
 
 /**
  * @brief Implements the type() member function using the suggested implementation
@@ -104,17 +104,17 @@
  * @endcode
  */
 #define FUSE_GRAPH_TYPE_DEFINITION(...) \
-        struct detail \
-        { \
-          static std::string type() \
-          { \
-            return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \
-          }  /* NOLINT */ \
-        };  /* NOLINT */ \
-        std::string type() const override \
-        { \
-          return detail::type(); \
-        }
+  struct detail \
+  { \
+    static std::string type() \
+    { \
+      return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \
+    }  /* NOLINT */ \
+  };  /* NOLINT */ \
+  std::string type() const override \
+  { \
+    return detail::type(); \
+  }
 
 /**
 * @brief Convenience function that creates the required pointer aliases, and type() method
@@ -130,9 +130,9 @@
 * @endcode
 */
 #define FUSE_GRAPH_DEFINITIONS(...) \
-        FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \
-        FUSE_GRAPH_TYPE_DEFINITION(__VA_ARGS__) \
-        FUSE_GRAPH_SERIALIZE_DEFINITION(__VA_ARGS__)
+  FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \
+  FUSE_GRAPH_TYPE_DEFINITION(__VA_ARGS__) \
+  FUSE_GRAPH_SERIALIZE_DEFINITION(__VA_ARGS__)
 
 
 namespace fuse_core
diff --git a/fuse_core/include/fuse_core/loss.hpp b/fuse_core/include/fuse_core/loss.hpp
index 71812a5bf..8465e574f 100644
--- a/fuse_core/include/fuse_core/loss.hpp
+++ b/fuse_core/include/fuse_core/loss.hpp
@@ -59,10 +59,10 @@
  * @endcode
  */
 #define FUSE_LOSS_CLONE_DEFINITION(...) \
-        fuse_core::Loss::UniquePtr clone() const override \
-        { \
-          return __VA_ARGS__::make_unique(*this); \
-        }
+  fuse_core::Loss::UniquePtr clone() const override \
+  { \
+    return __VA_ARGS__::make_unique(*this); \
+  }
 
 /**
  * @brief Implementation of the serialize() and deserialize() member functions for derived classes
@@ -78,22 +78,22 @@
  * @endcode
  */
 #define FUSE_LOSS_SERIALIZE_DEFINITION(...) \
-        void serialize(fuse_core::BinaryOutputArchive & archive) const override \
-        { \
-          archive << *this; \
-        }  /* NOLINT */ \
-        void serialize(fuse_core::TextOutputArchive & archive) const override \
-        { \
-          archive << *this; \
-        }  /* NOLINT */ \
-        void deserialize(fuse_core::BinaryInputArchive & archive) override \
-        { \
-          archive >> *this; \
-        }  /* NOLINT */ \
-        void deserialize(fuse_core::TextInputArchive & archive) override \
-        { \
-          archive >> *this; \
-        }
+  void serialize(fuse_core::BinaryOutputArchive & archive) const override \
+  { \
+    archive << *this; \
+  }  /* NOLINT */ \
+  void serialize(fuse_core::TextOutputArchive & archive) const override \
+  { \
+    archive << *this; \
+  }  /* NOLINT */ \
+  void deserialize(fuse_core::BinaryInputArchive & archive) override \
+  { \
+    archive >> *this; \
+  }  /* NOLINT */ \
+  void deserialize(fuse_core::TextInputArchive & archive) override \
+  { \
+    archive >> *this; \
+  }
 
 /**
  * @brief Implements the type() member function using the suggested implementation
@@ -111,17 +111,17 @@
  * @endcode
  */
 #define FUSE_LOSS_TYPE_DEFINITION(...) \
-        struct detail \
-        { \
-          static std::string type() \
-          { \
-            return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \
-          }  /* NOLINT */ \
-        };  /* NOLINT */ \
-        std::string type() const override \
-        { \
-          return detail::type(); \
-        }
+  struct detail \
+  { \
+    static std::string type() \
+    { \
+      return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \
+    }  /* NOLINT */ \
+  };  /* NOLINT */ \
+  std::string type() const override \
+  { \
+    return detail::type(); \
+  }
 
 /**
  * @brief Convenience function that creates the required pointer aliases, clone() method, and type()
@@ -138,10 +138,10 @@
  * @endcode
  */
 #define FUSE_LOSS_DEFINITIONS(...) \
-        FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \
-        FUSE_LOSS_TYPE_DEFINITION(__VA_ARGS__) \
-        FUSE_LOSS_CLONE_DEFINITION(__VA_ARGS__) \
-        FUSE_LOSS_SERIALIZE_DEFINITION(__VA_ARGS__)
+  FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \
+  FUSE_LOSS_TYPE_DEFINITION(__VA_ARGS__) \
+  FUSE_LOSS_CLONE_DEFINITION(__VA_ARGS__) \
+  FUSE_LOSS_SERIALIZE_DEFINITION(__VA_ARGS__)
 
 
 namespace fuse_core
diff --git a/fuse_core/include/fuse_core/macros.hpp b/fuse_core/include/fuse_core/macros.hpp
index c78eb0966..3af81587a 100644
--- a/fuse_core/include/fuse_core/macros.hpp
+++ b/fuse_core/include/fuse_core/macros.hpp
@@ -90,11 +90,11 @@
  * Use in the public section of the class.
  */
 #define SMART_PTR_DEFINITIONS(...) \
-        __SHARED_PTR_ALIAS(__VA_ARGS__) \
-        __MAKE_SHARED_DEFINITION(__VA_ARGS__) \
-        __WEAK_PTR_ALIAS(__VA_ARGS__) \
-        __UNIQUE_PTR_ALIAS(__VA_ARGS__) \
-        __MAKE_UNIQUE_DEFINITION(__VA_ARGS__)
+  __SHARED_PTR_ALIAS(__VA_ARGS__) \
+  __MAKE_SHARED_DEFINITION(__VA_ARGS__) \
+  __WEAK_PTR_ALIAS(__VA_ARGS__) \
+  __UNIQUE_PTR_ALIAS(__VA_ARGS__) \
+  __MAKE_UNIQUE_DEFINITION(__VA_ARGS__)
 
 /**
  * Defines smart pointer aliases and static functions for a class that contains fixed-sized
@@ -108,15 +108,15 @@
  */
 #if __cpp_aligned_new
   #define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \
-          SMART_PTR_DEFINITIONS(__VA_ARGS__)
+    SMART_PTR_DEFINITIONS(__VA_ARGS__)
 #else
   #define SMART_PTR_DEFINITIONS_WITH_EIGEN(...) \
-          EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
-          __SHARED_PTR_ALIAS(__VA_ARGS__) \
-          __MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \
-          __WEAK_PTR_ALIAS(__VA_ARGS__) \
-          __UNIQUE_PTR_ALIAS(__VA_ARGS__) \
-          __MAKE_UNIQUE_DEFINITION(__VA_ARGS__)
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW \
+    __SHARED_PTR_ALIAS(__VA_ARGS__) \
+    __MAKE_SHARED_ALIGNED_DEFINITION(__VA_ARGS__) \
+    __WEAK_PTR_ALIAS(__VA_ARGS__) \
+    __UNIQUE_PTR_ALIAS(__VA_ARGS__) \
+    __MAKE_UNIQUE_DEFINITION(__VA_ARGS__)
 #endif
 
 /**
@@ -128,55 +128,55 @@
  * Use in the public section of the class.
  */
 #define SMART_PTR_ALIASES_ONLY(...) \
-        __SHARED_PTR_ALIAS(__VA_ARGS__) \
-        __WEAK_PTR_ALIAS(__VA_ARGS__) \
-        __UNIQUE_PTR_ALIAS(__VA_ARGS__)
+  __SHARED_PTR_ALIAS(__VA_ARGS__) \
+  __WEAK_PTR_ALIAS(__VA_ARGS__) \
+  __UNIQUE_PTR_ALIAS(__VA_ARGS__)
 
 #define __SHARED_PTR_ALIAS(...) \
-        using SharedPtr = std::shared_ptr<__VA_ARGS__>; \
-        using ConstSharedPtr = std::shared_ptr<const __VA_ARGS__>;
+  using SharedPtr = std::shared_ptr<__VA_ARGS__>; \
+  using ConstSharedPtr = std::shared_ptr<const __VA_ARGS__>;
 
 #define __MAKE_SHARED_DEFINITION(...) \
-        template<typename ... Args> \
-        static std::shared_ptr<__VA_ARGS__> \
-        make_shared(Args && ... args) \
-        { \
-          return std::make_shared<__VA_ARGS__>(std::forward<Args>(args) ...); \
-        }
+  template<typename ... Args> \
+  static std::shared_ptr<__VA_ARGS__> \
+  make_shared(Args && ... args) \
+  { \
+    return std::make_shared<__VA_ARGS__>(std::forward<Args>(args) ...); \
+  }
 
 #define __MAKE_SHARED_ALIGNED_DEFINITION(...) \
-        template<typename ... Args> \
-        static std::shared_ptr<__VA_ARGS__> \
-        make_shared(Args && ... args) \
-        { \
-          return std::allocate_shared<__VA_ARGS__>( \
-            Eigen::aligned_allocator<__VA_ARGS__>(), \
-            std::forward<Args>(args) ...); \
-        }
+  template<typename ... Args> \
+  static std::shared_ptr<__VA_ARGS__> \
+  make_shared(Args && ... args) \
+  { \
+    return std::allocate_shared<__VA_ARGS__>( \
+      Eigen::aligned_allocator<__VA_ARGS__>(), \
+      std::forward<Args>(args) ...); \
+  }
 
 #define __WEAK_PTR_ALIAS(...) \
-        using WeakPtr = std::weak_ptr<__VA_ARGS__>; \
-        using ConstWeakPtr = std::weak_ptr<const __VA_ARGS__>;
+  using WeakPtr = std::weak_ptr<__VA_ARGS__>; \
+  using ConstWeakPtr = std::weak_ptr<const __VA_ARGS__>;
 
 #define __UNIQUE_PTR_ALIAS(...) \
-        using UniquePtr = std::unique_ptr<__VA_ARGS__>;
+  using UniquePtr = std::unique_ptr<__VA_ARGS__>;
 
 #if __cplusplus >= 201402L
   #define __MAKE_UNIQUE_DEFINITION(...) \
-          template<typename ... Args> \
-          static std::unique_ptr<__VA_ARGS__> \
-          make_unique(Args && ... args) \
-          { \
-            return std::make_unique<__VA_ARGS__>(std::forward<Args>(args) ...); \
-          }
+    template<typename ... Args> \
+    static std::unique_ptr<__VA_ARGS__> \
+    make_unique(Args && ... args) \
+    { \
+      return std::make_unique<__VA_ARGS__>(std::forward<Args>(args) ...); \
+    }
 #else
   #define __MAKE_UNIQUE_DEFINITION(...) \
-          template<typename ... Args> \
-          static std::unique_ptr<__VA_ARGS__> \
-          make_unique(Args && ... args) \
-          { \
-            return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward<Args>(args) ...)); \
-          }
+    template<typename ... Args> \
+    static std::unique_ptr<__VA_ARGS__> \
+    make_unique(Args && ... args) \
+    { \
+      return std::unique_ptr<__VA_ARGS__>(new __VA_ARGS__(std::forward<Args>(args) ...)); \
+    }
 #endif
 
 #endif  // FUSE_CORE__MACROS_HPP_
diff --git a/fuse_core/include/fuse_core/message_buffer_impl.hpp b/fuse_core/include/fuse_core/message_buffer_impl.hpp
index 408bbeea0..218f89ae0 100644
--- a/fuse_core/include/fuse_core/message_buffer_impl.hpp
+++ b/fuse_core/include/fuse_core/message_buffer_impl.hpp
@@ -96,9 +96,9 @@ typename MessageBuffer<Message>::message_range MessageBuffer<Message>::query(
   // Find the entry that is strictly greater than the requested beginning stamp. If the extended
   // range flag is true, we will then back up one entry.
   auto upper_bound_comparison = [](const auto & stamp, const auto & element) -> bool
-  {
-    return element.first > stamp;
-  };
+    {
+      return element.first > stamp;
+    };
   auto beginning_iter = std::upper_bound(
     buffer_.begin(),
     buffer_.end(), beginning_stamp, upper_bound_comparison);
@@ -108,9 +108,9 @@ typename MessageBuffer<Message>::message_range MessageBuffer<Message>::query(
   // Find the entry that is greater than or equal to the ending stamp. If the extended range flag is
   // false, we will back up one entry.
   auto lower_bound_comparison = [](const auto & element, const auto & stamp) -> bool
-  {
-    return element.first < stamp;
-  };
+    {
+      return element.first < stamp;
+    };
   auto ending_iter = std::lower_bound(
     buffer_.begin(),
     buffer_.end(), ending_stamp, lower_bound_comparison);
@@ -155,9 +155,9 @@ void MessageBuffer<Message>::purgeHistory()
   //  - at least two entries remains at all times
   //  - the buffer covers *at least* until the expiration time. Longer is acceptable.
   auto is_greater = [](const auto & stamp, const auto & element) -> bool
-  {
-    return element.first > stamp;
-  };
+    {
+      return element.first > stamp;
+    };
   auto expiration_iter = std::upper_bound(
     buffer_.begin(),
     buffer_.end(), expiration_time, is_greater);
diff --git a/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp b/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp
index 8d38d644a..897e568ed 100644
--- a/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp
+++ b/fuse_core/include/fuse_core/node_interfaces/node_interfaces.hpp
@@ -31,16 +31,16 @@
 
 
 #define ALL_FUSE_CORE_NODE_INTERFACES \
-        fuse_core::node_interfaces::Base, \
-        fuse_core::node_interfaces::Clock, \
-        fuse_core::node_interfaces::Graph, \
-        fuse_core::node_interfaces::Logging, \
-        fuse_core::node_interfaces::Parameters, \
-        fuse_core::node_interfaces::Services, \
-        fuse_core::node_interfaces::TimeSource, \
-        fuse_core::node_interfaces::Timers, \
-        fuse_core::node_interfaces::Topics, \
-        fuse_core::node_interfaces::Waitables
+  fuse_core::node_interfaces::Base, \
+  fuse_core::node_interfaces::Clock, \
+  fuse_core::node_interfaces::Graph, \
+  fuse_core::node_interfaces::Logging, \
+  fuse_core::node_interfaces::Parameters, \
+  fuse_core::node_interfaces::Services, \
+  fuse_core::node_interfaces::TimeSource, \
+  fuse_core::node_interfaces::Timers, \
+  fuse_core::node_interfaces::Topics, \
+  fuse_core::node_interfaces::Waitables
 
 namespace fuse_core
 {
diff --git a/fuse_core/include/fuse_core/variable.hpp b/fuse_core/include/fuse_core/variable.hpp
index 016b344b0..91570d966 100644
--- a/fuse_core/include/fuse_core/variable.hpp
+++ b/fuse_core/include/fuse_core/variable.hpp
@@ -62,10 +62,10 @@
  * @endcode
  */
 #define FUSE_VARIABLE_CLONE_DEFINITION(...) \
-        fuse_core::Variable::UniquePtr clone() const override \
-        { \
-          return __VA_ARGS__::make_unique(*this); \
-        }
+  fuse_core::Variable::UniquePtr clone() const override \
+  { \
+    return __VA_ARGS__::make_unique(*this); \
+  }
 
 /**
  * @brief Implementation of the serialize() and deserialize() member functions for derived classes
@@ -81,22 +81,22 @@
  * @endcode
  */
 #define FUSE_VARIABLE_SERIALIZE_DEFINITION(...) \
-        void serialize(fuse_core::BinaryOutputArchive & archive) const override \
-        { \
-          archive << *this; \
-        }  /* NOLINT */ \
-        void serialize(fuse_core::TextOutputArchive & archive) const override \
-        { \
-          archive << *this; \
-        }  /* NOLINT */ \
-        void deserialize(fuse_core::BinaryInputArchive & archive) override \
-        { \
-          archive >> *this; \
-        }  /* NOLINT */ \
-        void deserialize(fuse_core::TextInputArchive & archive) override \
-        { \
-          archive >> *this; \
-        }
+  void serialize(fuse_core::BinaryOutputArchive & archive) const override \
+  { \
+    archive << *this; \
+  }  /* NOLINT */ \
+  void serialize(fuse_core::TextOutputArchive & archive) const override \
+  { \
+    archive << *this; \
+  }  /* NOLINT */ \
+  void deserialize(fuse_core::BinaryInputArchive & archive) override \
+  { \
+    archive >> *this; \
+  }  /* NOLINT */ \
+  void deserialize(fuse_core::TextInputArchive & archive) override \
+  { \
+    archive >> *this; \
+  }
 
 /**
  * @brief Implements the type() member function using the suggested implementation
@@ -114,17 +114,17 @@
  * @endcode
  */
 #define FUSE_VARIABLE_TYPE_DEFINITION(...) \
-        struct detail \
-        { \
-          static std::string type() \
-          { \
-            return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \
-          }  /* NOLINT */ \
-        };  /* NOLINT */ \
-        std::string type() const override \
-        { \
-          return detail::type(); \
-        }
+  struct detail \
+  { \
+    static std::string type() \
+    { \
+      return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \
+    }  /* NOLINT */ \
+  };  /* NOLINT */ \
+  std::string type() const override \
+  { \
+    return detail::type(); \
+  }
 
 /**
  * @brief Convenience function that creates the required pointer aliases, clone() method, and type()
@@ -141,10 +141,10 @@
  * @endcode
  */
 #define FUSE_VARIABLE_DEFINITIONS(...) \
-        FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \
-        FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \
-        FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \
-        FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__)
+  FUSE_SMART_PTR_DEFINITIONS(__VA_ARGS__) \
+  FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \
+  FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \
+  FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__)
 
 /**
  * @brief Convenience function that creates the required pointer aliases, clone() method, and type()
@@ -161,10 +161,10 @@
  * @endcode
  */
 #define FUSE_VARIABLE_DEFINITIONS_WITH_EIGEN(...) \
-        FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \
-        FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \
-        FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \
-        FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__)
+  FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(__VA_ARGS__) \
+  FUSE_VARIABLE_TYPE_DEFINITION(__VA_ARGS__) \
+  FUSE_VARIABLE_CLONE_DEFINITION(__VA_ARGS__) \
+  FUSE_VARIABLE_SERIALIZE_DEFINITION(__VA_ARGS__)
 
 
 namespace fuse_core
diff --git a/fuse_core/src/transaction.cpp b/fuse_core/src/transaction.cpp
index d394b4be9..cbee6c839 100644
--- a/fuse_core/src/transaction.cpp
+++ b/fuse_core/src/transaction.cpp
@@ -92,9 +92,9 @@ void Transaction::addConstraint(Constraint::SharedPtr constraint, bool overwrite
 
   // Also don't add the same constraint twice
   auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr & added_constraint)
-  {
-    return constraint_uuid == added_constraint->uuid();
-  };
+    {
+      return constraint_uuid == added_constraint->uuid();
+    };
   auto added_constraints_iter = std::find_if(
     added_constraints_.begin(),
     added_constraints_.end(), is_constraint_added);
@@ -110,9 +110,9 @@ void Transaction::removeConstraint(const UUID & constraint_uuid)
   // If the constraint being removed is in the 'added' container, then delete it from the 'added'
   // container instead of adding it to the 'removed' container.
   auto is_constraint_added = [&constraint_uuid](const Constraint::SharedPtr & added_constraint)
-  {
-    return constraint_uuid == added_constraint->uuid();
-  };
+    {
+      return constraint_uuid == added_constraint->uuid();
+    };
   auto added_constraints_iter = std::find_if(
     added_constraints_.begin(),
     added_constraints_.end(), is_constraint_added);
@@ -166,9 +166,9 @@ void Transaction::addVariable(Variable::SharedPtr variable, bool overwrite)
 
   // Also don't add the same variable twice
   auto is_variable_added = [&variable_uuid](const Variable::SharedPtr & added_variable)
-  {
-    return variable_uuid == added_variable->uuid();
-  };
+    {
+      return variable_uuid == added_variable->uuid();
+    };
   auto added_variables_iter = std::find_if(
     added_variables_.begin(),
     added_variables_.end(), is_variable_added);
@@ -184,9 +184,9 @@ void Transaction::removeVariable(const UUID & variable_uuid)
   // If the variable being removed is in the 'added' container, then delete it from the 'added'
   // container instead of adding it to the 'removed' container.
   auto is_variable_added = [&variable_uuid](const Variable::SharedPtr & added_variable)
-  {
-    return variable_uuid == added_variable->uuid();
-  };
+    {
+      return variable_uuid == added_variable->uuid();
+    };
   auto added_variables_iter = std::find_if(
     added_variables_.begin(),
     added_variables_.end(), is_variable_added);
diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp
index 412c21696..0f14bb7fc 100644
--- a/fuse_graphs/src/hash_graph.cpp
+++ b/fuse_graphs/src/hash_graph.cpp
@@ -321,10 +321,10 @@ void HashGraph::getCovariance(
   // an expression to test a pair of data pointers such that (A,B) == (A,B) OR (B,A)
   auto symmetric_equal = [](const std::pair<const double *, const double *> & x,
     const std::pair<const double *, const double *> & y)
-  {
-    return ((x.first == y.first) && (x.second == y.second)) ||
-           ((x.first == y.second) && (x.second == y.first));
-  };
+    {
+      return ((x.first == y.first) && (x.second == y.second)) ||
+             ((x.first == y.second) && (x.second == y.first));
+    };
   // Convert the covariance requests into the input structure needed by Ceres. Namely, we must
   // convert the variable UUIDs into memory addresses. We create two containers of covariance
   // blocks: one only contains the unique variable pairs that we give to Ceres, and a second that
diff --git a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp
index 52f9a9302..8a3ca5d4f 100644
--- a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp
+++ b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.hpp
@@ -115,9 +115,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase
                 std::to_string(sigma_vector.size()));
       }
       auto is_sigma_valid = [](const double sigma)
-      {
-        return std::isfinite(sigma) && (sigma > 0);
-      };
+        {
+          return std::isfinite(sigma) && (sigma > 0);
+        };
       if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) {
         throw std::invalid_argument(
                 "The supplied initial_sigma parameter must contain valid floating point values. "
@@ -139,9 +139,9 @@ struct Unicycle2DIgnitionParams : public ParameterBase
                 std::to_string(state_vector.size()));
       }
       auto is_state_valid = [](const double state)
-      {
-        return std::isfinite(state);
-      };
+        {
+          return std::isfinite(state);
+        };
       if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) {
         throw std::invalid_argument(
                 "The supplied initial_state parameter must contain valid floating point values. "
diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp
index 9ebda7089..87de2e8fe 100644
--- a/fuse_optimizers/src/fixed_lag_smoother.cpp
+++ b/fuse_optimizers/src/fixed_lag_smoother.cpp
@@ -185,13 +185,12 @@ void FixedLagSmoother::postprocessMarginalization(
 void FixedLagSmoother::optimizationLoop()
 {
   auto exit_wait_condition = [this]()
-  {
-    return
-      this->optimization_request_ ||
-      !this->optimization_running_ ||
-      !interfaces_.get_node_base_interface()->get_context()->is_valid()
-    ;
-  };
+    {
+      return
+        this->optimization_request_ ||
+        !this->optimization_running_ ||
+        !interfaces_.get_node_base_interface()->get_context()->is_valid();
+    };
   // Optimize constraints until told to exit
   while (interfaces_.get_node_base_interface()->get_context()->is_valid() &&
     optimization_running_)
@@ -519,9 +518,9 @@ void FixedLagSmoother::transactionCallback(
     // The pending set is arranged "smallest stamp last" to making popping off the back more
     // efficient
     auto comparator = [](const rclcpp::Time & value, const TransactionQueueElement & element)
-    {
-      return value >= element.stamp();
-    };
+      {
+        return value >= element.stamp();
+      };
     auto position = std::upper_bound(
       pending_transactions_.begin(),
       pending_transactions_.end(),
diff --git a/fuse_optimizers/src/optimizer.cpp b/fuse_optimizers/src/optimizer.cpp
index 16c3cfa09..e23bd3353 100644
--- a/fuse_optimizers/src/optimizer.cpp
+++ b/fuse_optimizers/src/optimizer.cpp
@@ -502,9 +502,9 @@ void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & sta
   status.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Optimizer exists");
 
   auto print_key = [](const std::string & result, const auto & entry)
-  {
-    return result + entry.first + ' ';
-  };
+    {
+      return result + entry.first + ' ';
+    };
 
   status.add(
     "Sensor Models",
diff --git a/fuse_optimizers/src/variable_stamp_index.cpp b/fuse_optimizers/src/variable_stamp_index.cpp
index 2a5b71775..60038053d 100644
--- a/fuse_optimizers/src/variable_stamp_index.cpp
+++ b/fuse_optimizers/src/variable_stamp_index.cpp
@@ -47,9 +47,9 @@ namespace fuse_optimizers
 rclcpp::Time VariableStampIndex::currentStamp() const
 {
   auto compare_stamps = [](const StampedMap::value_type & lhs, const StampedMap::value_type & rhs)
-  {
-    return lhs.second < rhs.second;
-  };
+    {
+      return lhs.second < rhs.second;
+    };
   auto iter = std::max_element(stamped_index_.begin(), stamped_index_.end(), compare_stamps);
   if (iter != stamped_index_.end()) {
     return iter->second;