diff --git a/.clang-format b/.clang-format
index 89340932..815a04b1 100644
--- a/.clang-format
+++ b/.clang-format
@@ -4,7 +4,21 @@ BasedOnStyle: Google
 ColumnLimit: 80
 DerivePointerAlignment: false
 PointerAlignment: Left
-IncludeBlocks: Preserve
+IncludeBlocks: Regroup
+IncludeCategories:
+  # The the main header for a source file is automatically assigned priority 0
+  # C system headers
+  - Regex:    '^[<"](aio|arpa/inet|assert|complex|cpio|ctype|curses|dirent|dlfcn|errno|fcntl|fenv|float|fmtmsg|fnmatch|ftw|glob|grp|iconv|inttypes|iso646|langinfo|libgen|limits|locale|math|monetary|mqueue|ndbm|netdb|net/if|netinet/in|netinet/tcp|nl_types|poll|pthread|pwd|regex|sched|search|semaphore|setjmp|signal|spawn|stdalign|stdarg|stdatomic|stdbool|stddef|stdint|stdio|stdlib|stdnoreturn|string|strings|stropts|sys/ipc|syslog|sys/mman|sys/msg|sys/resource|sys/select|sys/sem|sys/shm|sys/socket|sys/stat|sys/statvfs|sys/time|sys/times|sys/types|sys/uio|sys/un|sys/utsname|sys/wait|tar|term|termios|tgmath|threads|time|trace|uchar|ulimit|uncntrl|unistd|utime|utmpx|wchar|wctype|wordexp)\.h[">]$'
+    Priority: 1
+  # C++ system headers
+  - Regex:    '^[<"](algorithm|any|array|atomic|bitset|cassert|ccomplex|cctype|cerrno|cfenv|cfloat|charconv|chrono|cinttypes|ciso646|climits|clocale|cmath|codecvt|complex|condition_variable|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype|deque|exception|execution|filesystem|forward_list|fstream|functional|future|initializer_list|iomanip|ios|iosfwd|iostream|istream|iterator|limits|list|locale|map|memory|memory_resource|mutex|new|numeric|optional|ostream|queue|random|ratio|regex|scoped_allocator|set|shared_mutex|sstream|stack|stdexcept|streambuf|string|string_view|strstream|system_error|thread|tuple|type_traits|typeindex|typeinfo|unordered_map|unordered_set|utility|valarray|variant|vector)[">]$'
+    Priority: 2
+  # Other libraries' .h files
+  - Regex:    '^<'
+    Priority: 3
+  # Your project's .h files
+  - Regex:    '^"'
+    Priority: 4
 ---
 Language: Proto
 BasedOnStyle: Google
diff --git a/voxgraph/CMakeLists.txt b/voxgraph/CMakeLists.txt
index ddba5c64..aff90271 100644
--- a/voxgraph/CMakeLists.txt
+++ b/voxgraph/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(voxgraph)
 
-add_definitions(-std=c++11 -Werror)
+add_definitions(-std=c++14 -Wall -Wextra -Wno-unused-parameter -Wno-sign-compare -Wno-deprecated-copy -fPIC -DEIGEN_INITIALIZE_MATRICES_BY_NAN -fdiagnostics-color=auto)
 
 find_package(catkin_simple REQUIRED)
 catkin_simple(ALL_DEPS_REQUIRED)
diff --git a/voxgraph/include/voxgraph/backend/constraint/constraint.h b/voxgraph/include/voxgraph/backend/constraint/constraint.h
index cee4f6ee..ee20d1b1 100644
--- a/voxgraph/include/voxgraph/backend/constraint/constraint.h
+++ b/voxgraph/include/voxgraph/backend/constraint/constraint.h
@@ -28,9 +28,7 @@ class Constraint {
   virtual void addToProblem(const NodeCollection& node_collection,
                             ceres::Problem* problem) = 0;
 
-  const ceres::ResidualBlockId getResidualBlockId() {
-    return residual_block_id_;
-  }
+  ceres::ResidualBlockId getResidualBlockId() { return residual_block_id_; }
 
  protected:
   static constexpr ceres::LossFunction* kNoRobustLossFunction = nullptr;
diff --git a/voxgraph/include/voxgraph/backend/constraint/constraint_collection.h b/voxgraph/include/voxgraph/backend/constraint/constraint_collection.h
index b3701e8b..a966223d 100644
--- a/voxgraph/include/voxgraph/backend/constraint/constraint_collection.h
+++ b/voxgraph/include/voxgraph/backend/constraint/constraint_collection.h
@@ -28,7 +28,7 @@ class ConstraintCollection {
 
  private:
   Constraint::ConstraintId constraint_id_counter_ = 0;
-  const Constraint::ConstraintId newConstraintId() {
+  Constraint::ConstraintId newConstraintId() {
     return constraint_id_counter_++;
   }
 
diff --git a/voxgraph/include/voxgraph/backend/constraint/cost_functions/relative_pose_cost_function.h b/voxgraph/include/voxgraph/backend/constraint/cost_functions/relative_pose_cost_function.h
index 1e4c369f..1e7ee59b 100644
--- a/voxgraph/include/voxgraph/backend/constraint/cost_functions/relative_pose_cost_function.h
+++ b/voxgraph/include/voxgraph/backend/constraint/cost_functions/relative_pose_cost_function.h
@@ -14,12 +14,12 @@ class RelativePoseCostFunction {
       const voxblox::Transformation& observed_relative_pose,
       const Constraint::InformationMatrix& sqrt_information_matrix,
       bool verbose = false)
-      : observed_relative_translation_(
+      : verbose_(verbose),
+        observed_relative_translation_(
             observed_relative_pose.getPosition().cast<double>()),
         observed_relative_yaw_(
             static_cast<double>(observed_relative_pose.log()[5])),
-        sqrt_information_matrix_(sqrt_information_matrix),
-        verbose_(verbose) {}
+        sqrt_information_matrix_(sqrt_information_matrix) {}
 
   template <typename T>
   bool operator()(const T* pose_A, const T* pose_B, T* residuals) const;
@@ -41,7 +41,7 @@ class RelativePoseCostFunction {
   const Constraint::InformationMatrix sqrt_information_matrix_;
 
   template <typename T>
-  const Eigen::Matrix<T, 3, 3> rotationMatrixFromYaw(T yaw_radians) const;
+  Eigen::Matrix<T, 3, 3> rotationMatrixFromYaw(T yaw_radians) const;
 };
 }  // namespace voxgraph
 
diff --git a/voxgraph/include/voxgraph/backend/constraint/cost_functions/relative_pose_cost_function_inl.h b/voxgraph/include/voxgraph/backend/constraint/cost_functions/relative_pose_cost_function_inl.h
index 6ddd1bfd..22797126 100644
--- a/voxgraph/include/voxgraph/backend/constraint/cost_functions/relative_pose_cost_function_inl.h
+++ b/voxgraph/include/voxgraph/backend/constraint/cost_functions/relative_pose_cost_function_inl.h
@@ -70,7 +70,7 @@ bool RelativePoseCostFunction::operator()(const T* const pose_A,
 }
 
 template <typename T>
-const Eigen::Matrix<T, 3, 3> RelativePoseCostFunction::rotationMatrixFromYaw(
+Eigen::Matrix<T, 3, 3> RelativePoseCostFunction::rotationMatrixFromYaw(
     T yaw_radians) const {
   const T cos_yaw = ceres::cos(yaw_radians);
   const T sin_yaw = ceres::sin(yaw_radians);
diff --git a/voxgraph/include/voxgraph/backend/node/node.h b/voxgraph/include/voxgraph/backend/node/node.h
index 89c344ea..22da4231 100644
--- a/voxgraph/include/voxgraph/backend/node/node.h
+++ b/voxgraph/include/voxgraph/backend/node/node.h
@@ -29,7 +29,7 @@ class Node {
   Pose* getPosePtr() { return &optimized_pose_; }
 
   void setConstant(bool constant) { config_.set_constant = constant; }
-  bool isConstant() { return config_.set_constant; }
+  bool isConstant() const { return config_.set_constant; }
 
   void addToProblem(ceres::Problem* problem,
                     ceres::LocalParameterization* local_parameterization);
diff --git a/voxgraph/include/voxgraph/backend/node/node_collection.h b/voxgraph/include/voxgraph/backend/node/node_collection.h
index fc22ad94..bbf4c359 100644
--- a/voxgraph/include/voxgraph/backend/node/node_collection.h
+++ b/voxgraph/include/voxgraph/backend/node/node_collection.h
@@ -31,7 +31,7 @@ class NodeCollection {
 
  private:
   Node::NodeId node_id_counter_ = 0;
-  const Node::NodeId newNodeId() { return node_id_counter_++; }
+  Node::NodeId newNodeId() { return node_id_counter_++; }
 
   SubmapNodeMap submap_nodes_;
   ReferenceFrameNodeMap reference_frame_nodes_;
diff --git a/voxgraph/include/voxgraph/backend/node/reference_frame_node.h b/voxgraph/include/voxgraph/backend/node/reference_frame_node.h
index 8c01a1f5..290e2b05 100644
--- a/voxgraph/include/voxgraph/backend/node/reference_frame_node.h
+++ b/voxgraph/include/voxgraph/backend/node/reference_frame_node.h
@@ -18,9 +18,7 @@ class ReferenceFrameNode : public Node {
   ReferenceFrameNode(const NodeId& node_id, const Config& config)
       : Node(node_id, config), config_(config) {}
 
-  const FrameId getReferenceFrameId() const {
-    return config_.reference_frame_id;
-  }
+  FrameId getReferenceFrameId() const { return config_.reference_frame_id; }
 
  private:
   Config config_;
diff --git a/voxgraph/include/voxgraph/backend/node/submap_node.h b/voxgraph/include/voxgraph/backend/node/submap_node.h
index 00a28255..b7d60c3c 100644
--- a/voxgraph/include/voxgraph/backend/node/submap_node.h
+++ b/voxgraph/include/voxgraph/backend/node/submap_node.h
@@ -20,7 +20,7 @@ class SubmapNode : public Node {
   SubmapNode(const NodeId& node_id, const Config& config)
       : Node(node_id, config), config_(config) {}
 
-  const cblox::SubmapID getSubmapId() const { return config_.submap_id; }
+  cblox::SubmapID getSubmapId() const { return config_.submap_id; }
 
  private:
   Config config_;
diff --git a/voxgraph/include/voxgraph/common.h b/voxgraph/include/voxgraph/common.h
index 0d1cf116..7d112d40 100644
--- a/voxgraph/include/voxgraph/common.h
+++ b/voxgraph/include/voxgraph/common.h
@@ -5,8 +5,6 @@
 
 #include <cblox/core/submap_collection.h>
 
-#include "voxgraph/frontend/submap_collection/voxgraph_submap.h"
-
 namespace voxgraph {
 using Transformation = voxblox::Transformation;
 using SubmapID = cblox::SubmapID;
diff --git a/voxgraph/include/voxgraph/frontend/frame_names.h b/voxgraph/include/voxgraph/frontend/frame_names.h
index af2ad916..2d6a94ef 100644
--- a/voxgraph/include/voxgraph/frontend/frame_names.h
+++ b/voxgraph/include/voxgraph/frontend/frame_names.h
@@ -21,8 +21,6 @@ struct FrameNames {
   //   - T_S_B corresponds to the pose of the robot in the current submap
   //   - T_B_C stores the transform from the pointcloud sensor
   //           to the base_link frame
-  // TODO(victorr): Update this documentation once Voxgraph's ICP has been
-  //                reimplemented to work with the new frame convention
 
   // Input frame names
   std::string input_odom_frame = "odom";
diff --git a/voxgraph/include/voxgraph/frontend/map_tracker/transformers/odometry_transformer.h b/voxgraph/include/voxgraph/frontend/map_tracker/transformers/odometry_transformer.h
index 639f1594..67aae494 100644
--- a/voxgraph/include/voxgraph/frontend/map_tracker/transformers/odometry_transformer.h
+++ b/voxgraph/include/voxgraph/frontend/map_tracker/transformers/odometry_transformer.h
@@ -6,6 +6,7 @@
 
 #include <minkindr_conversions/kindr_msg.h>
 #include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
 
 #include "voxgraph/common.h"
 
diff --git a/voxgraph/include/voxgraph/frontend/submap_collection/bounding_box.h b/voxgraph/include/voxgraph/frontend/submap_collection/bounding_box.h
index 3d01e655..3a8b185b 100644
--- a/voxgraph/include/voxgraph/frontend/submap_collection/bounding_box.h
+++ b/voxgraph/include/voxgraph/frontend/submap_collection/bounding_box.h
@@ -1,8 +1,8 @@
 #ifndef VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_BOUNDING_BOX_H_
 #define VOXGRAPH_FRONTEND_SUBMAP_COLLECTION_BOUNDING_BOX_H_
 
-#include <voxblox/core/common.h>
 #include <Eigen/Dense>
+#include <voxblox/core/common.h>
 
 namespace voxgraph {
 typedef Eigen::Matrix<voxblox::FloatingPoint, 3, 8> BoxCornerMatrix;
diff --git a/voxgraph/include/voxgraph/frontend/submap_collection/voxgraph_submap.h b/voxgraph/include/voxgraph/frontend/submap_collection/voxgraph_submap.h
index 401ea6bc..1dc1e33e 100644
--- a/voxgraph/include/voxgraph/frontend/submap_collection/voxgraph_submap.h
+++ b/voxgraph/include/voxgraph/frontend/submap_collection/voxgraph_submap.h
@@ -58,7 +58,7 @@ class VoxgraphSubmap : public cblox::TsdfEsdfSubmap {
 
   // Indicate that the submap is finished and generate all cached members
   // NOTE: These cached members are mainly used in the registration cost funcs
-  virtual void finishSubmap() override;
+  void finishSubmap() override;
 
   void transformSubmap(const voxblox::Transformation& T_new_old);
 
diff --git a/voxgraph/include/voxgraph/frontend/submap_collection/voxgraph_submap_collection.h b/voxgraph/include/voxgraph/frontend/submap_collection/voxgraph_submap_collection.h
index c71fdc05..42fb6cb7 100644
--- a/voxgraph/include/voxgraph/frontend/submap_collection/voxgraph_submap_collection.h
+++ b/voxgraph/include/voxgraph/frontend/submap_collection/voxgraph_submap_collection.h
@@ -27,11 +27,11 @@ class VoxgraphSubmapCollection
   explicit VoxgraphSubmapCollection(VoxgraphSubmap::Config submap_config,
                                     bool verbose = false)
       : SubmapCollection(submap_config),
-        submap_creation_interval_(20),
-        verbose_(verbose) {}
+        verbose_(verbose),
+        submap_creation_interval_(20) {}
 
   void setSubmapCreationInterval(ros::Duration submap_creation_interval) {
-    submap_creation_interval_ = std::move(submap_creation_interval);
+    submap_creation_interval_ = submap_creation_interval;
   }
 
   bool shouldCreateNewSubmap(const ros::Time& current_time);
diff --git a/voxgraph/src/backend/constraint/cost_functions/registration_cost_function.cpp b/voxgraph/src/backend/constraint/cost_functions/registration_cost_function.cpp
index c561930d..347fe674 100644
--- a/voxgraph/src/backend/constraint/cost_functions/registration_cost_function.cpp
+++ b/voxgraph/src/backend/constraint/cost_functions/registration_cost_function.cpp
@@ -13,14 +13,14 @@ RegistrationCostFunction::RegistrationCostFunction(
     VoxgraphSubmap::ConstPtr reference_submap_ptr,
     VoxgraphSubmap::ConstPtr reading_submap_ptr,
     const RegistrationCostFunction::Config& config)
-    : reading_submap_ptr_(reading_submap_ptr),
+    : config_(config),
+      reading_submap_ptr_(reading_submap_ptr),
       reading_tsdf_layer_(reading_submap_ptr->getTsdfMap().getTsdfLayer()),
       reading_esdf_layer_(reading_submap_ptr->getEsdfMap().getEsdfLayer()),
-      tsdf_interpolator_(&reading_submap_ptr->getTsdfMap().getTsdfLayer()),
-      esdf_interpolator_(&reading_submap_ptr->getEsdfMap().getEsdfLayer()),
       registration_points_(reference_submap_ptr->getRegistrationPoints(
           config.registration_point_type)),
-      config_(config) {
+      tsdf_interpolator_(&reading_submap_ptr->getTsdfMap().getTsdfLayer()),
+      esdf_interpolator_(&reading_submap_ptr->getEsdfMap().getEsdfLayer()) {
   // Ensure that the reference and reading submaps have gravity aligned Z-axes
   voxblox::Transformation::Vector6 T_vec_reference =
       reference_submap_ptr->getPose().log();
diff --git a/voxgraph/src/frontend/submap_collection/voxgraph_submap.cpp b/voxgraph/src/frontend/submap_collection/voxgraph_submap.cpp
index ad7d548d..23f602de 100644
--- a/voxgraph/src/frontend/submap_collection/voxgraph_submap.cpp
+++ b/voxgraph/src/frontend/submap_collection/voxgraph_submap.cpp
@@ -3,10 +3,10 @@
 #include <memory>
 #include <utility>
 
+#include <voxblox/integrator/esdf_integrator.h>
 #include <voxblox/integrator/merge_integration.h>
 #include <voxblox/interpolator/interpolator.h>
 #include <voxblox/mesh/mesh_integrator.h>
-#include <voxblox/integrator/esdf_integrator.h>
 
 namespace voxgraph {
 VoxgraphSubmap::VoxgraphSubmap(
@@ -136,6 +136,7 @@ const WeightedSampler<RegistrationPoint>& VoxgraphSubmap::getRegistrationPoints(
     case RegistrationPointType::kVoxels:
       return relevant_voxels_;
     case RegistrationPointType::kIsosurfacePoints:
+    default:
       return isosurface_vertices_;
   }
 }
diff --git a/voxgraph/src/tools/odometry_simulator/odometry_simulator.cpp b/voxgraph/src/tools/odometry_simulator/odometry_simulator.cpp
index d399341e..a85134b8 100644
--- a/voxgraph/src/tools/odometry_simulator/odometry_simulator.cpp
+++ b/voxgraph/src/tools/odometry_simulator/odometry_simulator.cpp
@@ -7,9 +7,9 @@
 namespace voxgraph {
 OdometrySimulator::OdometrySimulator(const ros::NodeHandle& nh,
                                      const ros::NodeHandle& nh_private)
-    : nh_(nh),
+    : debug_(false),
+      nh_(nh),
       nh_private_(nh_private),
-      debug_(false),
       subscriber_queue_length_(100),
       subscribe_to_odom_topic_("odometry"),
       published_mission_frame_("mission"),
diff --git a/voxgraph/src/tools/test_benches/registration_test_bench.cpp b/voxgraph/src/tools/test_benches/registration_test_bench.cpp
index de520a5a..21b5297c 100644
--- a/voxgraph/src/tools/test_benches/registration_test_bench.cpp
+++ b/voxgraph/src/tools/test_benches/registration_test_bench.cpp
@@ -3,6 +3,7 @@
 #include <string>
 #include <vector>
 
+#include <boost/filesystem.hpp>
 #include <cblox/core/submap_collection.h>
 #include <cblox/io/submap_io.h>
 #include <cblox/utils/quat_transformation_protobuf_utils.h>
@@ -11,7 +12,6 @@
 #include <voxblox/io/layer_io.h>
 #include <voxblox_ros/ptcloud_vis.h>
 #include <voxblox_ros/ros_params.h>
-#include <boost/filesystem.hpp>
 
 #include "voxgraph/frontend/submap_collection/voxgraph_submap.h"
 #include "voxgraph/tools/submap_registration_helper.h"