Initial Commit (tested training, testing, and TRT conversion)

flightlib/third_party/arc_utilities/include/arc_utilities/abb_irb1600_145_fk_fast.hpp

@@ -0,0 +1,168 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <vector>
+#include <map>
+#include <Eigen/Geometry>
+#include <arc_utilities/arc_helpers.hpp>
+#include <arc_utilities/eigen_helpers.hpp>
+
+#ifndef ABB_IRB1600_145_FK_FAST_HPP
+#define ABB_IRB1600_145_FK_FAST_HPP
+
+namespace ABB_IRB1600_145_FK_FAST
+{
+    const size_t ABB_IRB1600_145_NUM_ACTIVE_JOINTS = 6;
+    const size_t ABB_IRB1600_145_NUM_LINKS = 8;
+
+    const std::string ABB_IRB1600_145_ACTIVE_JOINT_1_NAME = "joint_1";
+    const std::string ABB_IRB1600_145_ACTIVE_JOINT_2_NAME = "joint_2";
+    const std::string ABB_IRB1600_145_ACTIVE_JOINT_3_NAME = "joint_3";
+    const std::string ABB_IRB1600_145_ACTIVE_JOINT_4_NAME = "joint_4";
+    const std::string ABB_IRB1600_145_ACTIVE_JOINT_5_NAME = "joint_5";
+    const std::string ABB_IRB1600_145_ACTIVE_JOINT_6_NAME = "joint_6";
+
+    const std::string ABB_IRB1600_145_LINK_1_NAME = "link_0";
+    const std::string ABB_IRB1600_145_LINK_2_NAME = "link_1";
+    const std::string ABB_IRB1600_145_LINK_3_NAME = "link_2";
+    const std::string ABB_IRB1600_145_LINK_4_NAME = "link_3";
+    const std::string ABB_IRB1600_145_LINK_5_NAME = "link_4";
+    const std::string ABB_IRB1600_145_LINK_6_NAME = "link_5";
+    const std::string ABB_IRB1600_145_LINK_7_NAME = "link_6";
+    const std::string ABB_IRB1600_145_LINK_8_NAME = "link_7";
+
+    typedef std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d>> VectorIsometry3d;
+
+    inline Eigen::Isometry3d Get_base_joint1_LinkJointTransform(const double joint_val)
+    {
+        Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.1245);
+        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
+        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_1_joint_2_LinkJointTransform(const double joint_val)
+    {
+        Eigen::Translation3d pre_joint_translation(0.15, -0.1395, 0.362);
+        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
+        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
+        Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_2_joint_3_LinkJointTransform(const double joint_val)
+    {
+        Eigen::Translation3d pre_joint_translation(0.0, 0.028, 0.7);
+        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
+        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
+        Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_3_joint_4_LinkJointTransform(const double joint_val)
+    {
+        Eigen::Translation3d pre_joint_translation(0.314, 0.107, 0.0);
+        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
+        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitX()));
+        Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_4_joint_5_LinkJointTransform(const double joint_val)
+    {
+        Eigen::Translation3d pre_joint_translation(0.286, 0.0, 0.0);
+        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
+        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitY()));
+        Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_5_joint_6_LinkJointTransform(const double joint_val)
+    {
+        Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.0);
+        Eigen::Quaterniond pre_joint_rotation(1.0, 0.0, 0.0, 0.0);
+        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitX()));
+        Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_Fixed_link_6_joint_tool_LinkJointTransform(void)
+    {
+        Eigen::Translation3d pre_joint_translation(0.065, 0.0, 0.0);
+        Eigen::Quaterniond pre_joint_rotation(0.7071067811865476, 0.0, 0.7071067811865476, 0.0);
+        Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        return pre_joint_transform;
+    }
+
+    inline VectorIsometry3d GetLinkTransforms(const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
+    {
+        assert(configuration.size() == ABB_IRB1600_145_NUM_ACTIVE_JOINTS);
+        VectorIsometry3d link_transforms(ABB_IRB1600_145_NUM_LINKS);
+        link_transforms[0] = base_transform;
+        link_transforms[1] = link_transforms[0] * Get_base_joint1_LinkJointTransform(configuration[0]);
+        link_transforms[2] = link_transforms[1] * Get_link_1_joint_2_LinkJointTransform(configuration[1]);
+        link_transforms[3] = link_transforms[2] * Get_link_2_joint_3_LinkJointTransform(configuration[2]);
+        link_transforms[4] = link_transforms[3] * Get_link_3_joint_4_LinkJointTransform(configuration[3]);
+        link_transforms[5] = link_transforms[4] * Get_link_4_joint_5_LinkJointTransform(configuration[4]);
+        link_transforms[6] = link_transforms[5] * Get_link_5_joint_6_LinkJointTransform(configuration[5]);
+        link_transforms[7] = link_transforms[6] * Get_Fixed_link_6_joint_tool_LinkJointTransform();
+        return link_transforms;
+    }
+
+    inline VectorIsometry3d GetLinkTransforms(std::map<std::string, double> configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
+    {
+        std::vector<double> configuration_vector(ABB_IRB1600_145_NUM_ACTIVE_JOINTS);
+        configuration_vector[0] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_1_NAME];
+        configuration_vector[1] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_2_NAME];
+        configuration_vector[2] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_3_NAME];
+        configuration_vector[3] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_4_NAME];
+        configuration_vector[4] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_5_NAME];
+        configuration_vector[5] = configuration[ABB_IRB1600_145_ACTIVE_JOINT_6_NAME];
+        return GetLinkTransforms(configuration_vector, base_transform);
+    }
+
+    inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
+    {
+        const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
+        EigenHelpers::MapStringIsometry3d link_transforms_map;
+        link_transforms_map[ABB_IRB1600_145_LINK_1_NAME] = link_transforms[0];
+        link_transforms_map[ABB_IRB1600_145_LINK_2_NAME] = link_transforms[1];
+        link_transforms_map[ABB_IRB1600_145_LINK_3_NAME] = link_transforms[2];
+        link_transforms_map[ABB_IRB1600_145_LINK_4_NAME] = link_transforms[3];
+        link_transforms_map[ABB_IRB1600_145_LINK_5_NAME] = link_transforms[4];
+        link_transforms_map[ABB_IRB1600_145_LINK_6_NAME] = link_transforms[5];
+        link_transforms_map[ABB_IRB1600_145_LINK_7_NAME] = link_transforms[6];
+        link_transforms_map[ABB_IRB1600_145_LINK_8_NAME] = link_transforms[7];
+        return link_transforms_map;
+    }
+
+    inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(const std::map<std::string, double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
+    {
+        const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
+        EigenHelpers::MapStringIsometry3d link_transforms_map;
+        link_transforms_map[ABB_IRB1600_145_LINK_1_NAME] = link_transforms[0];
+        link_transforms_map[ABB_IRB1600_145_LINK_2_NAME] = link_transforms[1];
+        link_transforms_map[ABB_IRB1600_145_LINK_3_NAME] = link_transforms[2];
+        link_transforms_map[ABB_IRB1600_145_LINK_4_NAME] = link_transforms[3];
+        link_transforms_map[ABB_IRB1600_145_LINK_5_NAME] = link_transforms[4];
+        link_transforms_map[ABB_IRB1600_145_LINK_6_NAME] = link_transforms[5];
+        link_transforms_map[ABB_IRB1600_145_LINK_7_NAME] = link_transforms[6];
+        link_transforms_map[ABB_IRB1600_145_LINK_8_NAME] = link_transforms[7];
+        return link_transforms_map;
+    }
+}
+
+#endif // ABB_IRB1600_145_FK_FAST_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/aligned_eigen_types.hpp

@@ -0,0 +1,229 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Gael Guennebaud <g.gael@free.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef ALIGNED_EIGEN_TYPES_HPP
+#define ALIGNED_EIGEN_TYPES_HPP
+
+#include <Eigen/Geometry>
+
+namespace Eigen {
+
+/**
+  * \defgroup Aligned4Vector3_Module Aligned vector3 module
+  *
+  * \code
+  * #include <unsupported/Eigen/Aligned4Vector3>
+  * \endcode
+  */
+  //@{
+
+
+/** \class Aligned4Vector3
+  *
+  * \brief A vectorization friendly 3D vector
+  *
+  * This class represents a 3D vector internally using a 4D vector
+  * such that vectorization can be seamlessly enabled. Of course,
+  * the same result can be achieved by directly using a 4D vector.
+  * This class makes this process simpler.
+  *
+  */
+// TODO specialize Cwise
+template<typename _Scalar> class Aligned4Vector3;
+
+namespace internal {
+template<typename _Scalar> struct traits<Aligned4Vector3<_Scalar> >
+  : traits<Matrix<_Scalar,3,1,0,3,1> >
+{
+};
+}
+
+template<typename _Scalar> class Aligned4Vector3
+  : public MatrixBase<Aligned4Vector3<_Scalar> >
+{
+    typedef Matrix<_Scalar,4,1> CoeffType;
+    CoeffType m_coeffs;
+  public:
+
+    typedef MatrixBase<Aligned4Vector3<_Scalar> > Base;
+    EIGEN_DENSE_PUBLIC_INTERFACE(Aligned4Vector3)
+    using Base::operator*;
+
+    inline Index rows() const { return 3; }
+    inline Index cols() const { return 1; }
+
+    Scalar* data() { return m_coeffs.data(); }
+    const Scalar* data() const { return m_coeffs.data(); }
+    Index innerStride() const { return 1; }
+    Index outerStride() const { return m_coeffs.outerStride(); }
+
+    inline const Scalar& coeff(Index row, Index col) const
+    { return m_coeffs.coeff(row, col); }
+
+    inline Scalar& coeffRef(Index row, Index col)
+    { return m_coeffs.coeffRef(row, col); }
+
+    inline const Scalar& coeff(Index index) const
+    { return m_coeffs.coeff(index); }
+
+    inline Scalar& coeffRef(Index index)
+    { return m_coeffs.coeffRef(index);}
+
+
+    inline Aligned4Vector3(const Scalar& x, const Scalar& y, const Scalar& z)
+      : m_coeffs(x, y, z, Scalar(1))
+    {}
+
+    inline Aligned4Vector3()
+      : m_coeffs(Scalar(0), Scalar(0), Scalar(0), Scalar(1))
+    {}
+
+    inline Aligned4Vector3(const Aligned4Vector3& other)
+      : Base(), m_coeffs(other.m_coeffs)
+    {}
+
+    template<typename XprType, int Size=XprType::SizeAtCompileTime>
+    struct generic_assign_selector {};
+
+    template<typename XprType> struct generic_assign_selector<XprType,4>
+    {
+      inline static void run(Aligned4Vector3& dest, const XprType& src)
+      {
+        dest.m_coeffs = src;
+        dest.m_coeffs.w() = Scalar(1);
+      }
+    };
+
+    template<typename XprType> struct generic_assign_selector<XprType,3>
+    {
+      inline static void run(Aligned4Vector3& dest, const XprType& src)
+      {
+        dest.m_coeffs.template head<3>() = src;
+        dest.m_coeffs.w() = Scalar(1);
+      }
+    };
+
+    template<typename Derived>
+    inline Aligned4Vector3(const MatrixBase<Derived>& other)
+    {
+      generic_assign_selector<Derived>::run(*this,other.derived());
+    }
+
+    inline Aligned4Vector3& operator=(const Aligned4Vector3& other)
+    { m_coeffs = other.m_coeffs; return *this; }
+
+    template <typename Derived>
+    inline Aligned4Vector3& operator=(const MatrixBase<Derived>& other)
+    {
+      generic_assign_selector<Derived>::run(*this,other.derived());
+      return *this;
+    }
+
+    inline Aligned4Vector3 operator+(const Aligned4Vector3& other) const
+    {return Aligned4Vector3(m_coeffs + other.m_coeffs); }
+
+    inline Aligned4Vector3& operator+=(const Aligned4Vector3& other)
+    { m_coeffs += other.m_coeffs; return *this; }
+
+    inline Aligned4Vector3 operator-(const Aligned4Vector3& other) const
+    { return Aligned4Vector3(m_coeffs - other.m_coeffs); }
+
+    inline Aligned4Vector3 operator-=(const Aligned4Vector3& other)
+    { m_coeffs -= other.m_coeffs; return *this; }
+
+    inline Aligned4Vector3 operator*(const Scalar& s) const
+    { return Aligned4Vector3(m_coeffs * s); }
+
+    inline friend Aligned4Vector3 operator*(const Scalar& s,const Aligned4Vector3& vec)
+    { return Aligned4Vector3(s * vec.m_coeffs); }
+
+    inline Aligned4Vector3& operator*=(const Scalar& s)
+    { m_coeffs *= s; return *this; }
+
+    inline Aligned4Vector3 operator/(const Scalar& s) const
+    { return Aligned4Vector3(m_coeffs / s); }
+
+    inline Aligned4Vector3& operator/=(const Scalar& s)
+    { m_coeffs /= s; return *this; }
+
+    inline Scalar dot(const Aligned4Vector3& other) const
+    {
+      eigen_assert(m_coeffs.w()==Scalar(1));
+      eigen_assert(other.m_coeffs.w()==Scalar(1));
+      return m_coeffs.dot(other.m_coeffs) - Scalar(1);
+    }
+
+    inline void normalize()
+    {
+      m_coeffs /= norm();
+    }
+
+    inline Aligned4Vector3 normalized() const
+    {
+      return Aligned4Vector3(m_coeffs / norm());
+    }
+
+    inline Scalar sum() const
+    {
+      eigen_assert(m_coeffs.w()==Scalar(1));
+      return m_coeffs.sum() - Scalar(1);
+    }
+
+    inline Scalar squaredNorm() const
+    {
+      eigen_assert(m_coeffs.w()==Scalar(1));
+      return m_coeffs.squaredNorm() - Scalar(1);
+    }
+
+    inline Scalar norm() const
+    {
+      using std::sqrt;
+      return sqrt(squaredNorm());
+    }
+
+    inline Aligned4Vector3 cross(const Aligned4Vector3& other) const
+    {
+      return Aligned4Vector3(m_coeffs.cross3(other.m_coeffs));
+    }
+
+    template<typename Derived>
+    inline bool isApprox(const MatrixBase<Derived>& other, RealScalar eps=NumTraits<Scalar>::dummy_precision()) const
+    {
+      return m_coeffs.template head<3>().isApprox(other,eps);
+    }
+
+    CoeffType& coeffs() { return m_coeffs; }
+    const CoeffType& coeffs() const { return m_coeffs; }
+};
+
+namespace internal {
+
+template<typename _Scalar>
+struct eval<Aligned4Vector3<_Scalar>, Dense>
+{
+ typedef const Aligned4Vector3<_Scalar>& type;
+};
+
+template<typename Scalar>
+struct evaluator<Aligned4Vector3<Scalar> >
+  : evaluator<Matrix<Scalar,4,1> >
+{
+  typedef Aligned4Vector3<Scalar> XprType;
+  typedef evaluator<Matrix<Scalar,4,1> > Base;
+
+  evaluator(const XprType &m) : Base(m.coeffs()) {}
+};
+
+}
+
+//@}
+
+}
+
+#endif // ALIGNED_EIGEN_TYPES

flightlib/third_party/arc_utilities/include/arc_utilities/arc_exceptions.hpp

@@ -0,0 +1,21 @@
+#include <stdexcept>
+#include <string>
+#include <sstream>
+
+#ifndef ARC_EXCEPTIONS_HPP
+#define ARC_EXCEPTIONS_HPP
+
+#define throw_arc_exception(type, ...) arc_exceptions::ArcException<type>(__FILE__, __LINE__, __VA_ARGS__)
+
+namespace arc_exceptions
+{
+    template <typename ExceptionType>
+    inline void ArcException(const char* file, const std::size_t line, const std::string& message)
+    {
+       std::ostringstream stream;
+       stream << message << ": " << file << ": " << line;
+       throw ExceptionType(stream.str());
+    }
+}
+
+#endif // ARC_EXCEPTIONS_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/base64_helpers.hpp

@@ -0,0 +1,21 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <vector>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <fstream>
+#include <stdexcept>
+#include <zlib.h>
+
+#ifndef BASE64_HELPERS_HPP
+#define BASE64_HELPERS_HPP
+
+namespace Base64Helpers
+{
+    std::vector<uint8_t> Decode(const std::string& encoded);
+
+    std::string Encode(const std::vector<uint8_t>& binary);
+}
+
+#endif // BASE64_HELPERS_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/dijkstras.hpp

@@ -0,0 +1,984 @@
+#ifndef DIJKSTRAS_HPP
+#define DIJKSTRAS_HPP
+
+#include <random>
+#include <cstdint>
+#include <functional>
+#include <limits>
+#include <queue>
+#include <stdexcept>
+#include <unordered_map>
+#include <vector>
+#include <Eigen/Geometry>
+#include <arc_utilities/arc_helpers.hpp>
+#include <arc_utilities/serialization.hpp>
+
+namespace arc_dijkstras
+{
+    class GraphEdge
+    {
+    protected:
+
+        int64_t from_index_;
+        int64_t to_index_;
+        double weight_;
+
+    public:
+
+        static uint64_t Serialize(const GraphEdge& edge, std::vector<uint8_t>& buffer)
+        {
+            return edge.SerializeSelf(buffer);
+        }
+
+        static std::pair<GraphEdge, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current)
+        {
+            GraphEdge temp_edge;
+            const uint64_t bytes_read = temp_edge.DeserializeSelf(buffer, current);
+            return std::make_pair(temp_edge, bytes_read);
+        }
+
+        GraphEdge(const int64_t from_index, const int64_t to_index, const double weight)
+            : from_index_(from_index), to_index_(to_index), weight_(weight)
+        {}
+
+        GraphEdge()
+            : from_index_(-1), to_index_(-1), weight_(0.0)
+        {}
+
+        uint64_t SerializeSelf(std::vector<uint8_t>& buffer) const
+        {
+            const uint64_t start_buffer_size = buffer.size();
+            arc_utilities::SerializeFixedSizePOD<int64_t>(from_index_, buffer);
+            arc_utilities::SerializeFixedSizePOD<int64_t>(to_index_, buffer);
+            arc_utilities::SerializeFixedSizePOD<double>(weight_, buffer);
+            // Figure out how many bytes were written
+            const uint64_t end_buffer_size = buffer.size();
+            const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+            return bytes_written;
+        }
+
+        uint64_t DeserializeSelf(const std::vector<uint8_t>& buffer, const uint64_t current)
+        {
+            assert(current < buffer.size());
+            uint64_t current_position = current;
+            const std::pair<int64_t, uint64_t> deserialized_from_index = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
+            from_index_ = deserialized_from_index.first;
+            current_position += deserialized_from_index.second;
+            const std::pair<int64_t, uint64_t> deserialized_to_index = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
+            to_index_ = deserialized_to_index.first;
+            current_position += deserialized_to_index.second;
+            const std::pair<double, uint64_t> deserialized_weight = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+            weight_ = deserialized_weight.first;
+            current_position += deserialized_weight.second;
+            // Figure out how many bytes were read
+            const uint64_t bytes_read = current_position - current;
+            return bytes_read;
+        }
+
+        bool operator==(const GraphEdge& other) const
+        {
+            return (from_index_ == other.GetFromIndex() && to_index_ == other.GetToIndex() && weight_ == other.GetWeight());
+        }
+
+        std::string Print() const
+        {
+            return std::string("(" + std::to_string(from_index_) + "->" + std::to_string(to_index_) + ") : " + std::to_string(weight_));
+        }
+
+        int64_t GetFromIndex() const
+        {
+            return from_index_;
+        }
+
+        int64_t GetToIndex() const
+        {
+            return to_index_;
+        }
+
+        double GetWeight() const
+        {
+            return weight_;
+        }
+
+        void SetFromIndex(const int64_t new_from_index)
+        {
+            from_index_ = new_from_index;
+        }
+
+        void SetToIndex(const int64_t new_to_index)
+        {
+            to_index_ = new_to_index;
+        }
+
+        void SetWeight(const double new_weight)
+        {
+            weight_ = new_weight;
+        }
+    };
+
+    inline std::ostream& operator<< (std::ostream& stream, const GraphEdge& edge)
+    {
+        stream << edge.Print();
+        return stream;
+    }
+
+    template<typename NodeValueType, typename Allocator = std::allocator<NodeValueType>>
+    class GraphNode
+    {
+    protected:
+
+        NodeValueType value_;
+        double distance_;
+        std::vector<GraphEdge> in_edges_;
+        std::vector<GraphEdge> out_edges_;
+
+    public:
+
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        static uint64_t Serialize(
+                const GraphNode<NodeValueType, Allocator>& node,
+                std::vector<uint8_t>& buffer,
+                const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer)
+        {
+            return node.SerializeSelf(buffer, value_serializer);
+        }
+
+        static std::pair<GraphNode<NodeValueType, Allocator>, uint64_t> Deserialize(
+                const std::vector<uint8_t>& buffer,
+                const uint64_t current,
+                const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
+        {
+            GraphNode<NodeValueType, Allocator> temp_node;
+            const uint64_t bytes_read = temp_node.DeserializeSelf(buffer, current, value_deserializer);
+            return std::make_pair(temp_node, bytes_read);
+        }
+
+        GraphNode(
+                const NodeValueType& value,
+                const double distance,
+                const std::vector<GraphEdge>& new_in_edges,
+                const std::vector<GraphEdge>& new_out_edges)
+            : value_(value)
+            , distance_(distance)
+            , in_edges_(new_in_edges)
+            , out_edges_(new_out_edges)
+        {}
+
+        GraphNode(const NodeValueType& value)
+            : value_(value)
+            , distance_(std::numeric_limits<double>::infinity())
+        {}
+
+        GraphNode()
+            : distance_(std::numeric_limits<double>::infinity())
+        {}
+
+        uint64_t SerializeSelf(
+                std::vector<uint8_t>& buffer,
+                const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer) const
+        {
+            const uint64_t start_buffer_size = buffer.size();
+            // Serialize the value
+            value_serializer(value_, buffer);
+            // Serialize the distance
+            arc_utilities::SerializeFixedSizePOD<double>(distance_, buffer);
+            // Serialize the in edges
+            arc_utilities::SerializeVector<GraphEdge>(in_edges_, buffer, GraphEdge::Serialize);
+            // Serialize the in edges
+            arc_utilities::SerializeVector<GraphEdge>(out_edges_, buffer, GraphEdge::Serialize);
+            // Figure out how many bytes were written
+            const uint64_t end_buffer_size = buffer.size();
+            const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+            return bytes_written;
+        }
+
+        uint64_t DeserializeSelf(
+                const std::vector<uint8_t>& buffer,
+                const uint64_t current,
+                const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
+        {
+            assert(current < buffer.size());
+            uint64_t current_position = current;
+            // Deserialize the value
+            const std::pair<NodeValueType, uint64_t> value_deserialized = value_deserializer(buffer, current_position);
+            value_ = value_deserialized.first;
+            current_position += value_deserialized.second;
+            // Deserialize the distace
+            const std::pair<double, uint64_t> distance_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+            distance_ = distance_deserialized.first;
+            current_position += distance_deserialized.second;
+            // Deserialize the in edges
+            const std::pair<std::vector<GraphEdge>, uint64_t> in_edges_deserialized = arc_utilities::DeserializeVector<GraphEdge>(buffer, current_position, GraphEdge::Deserialize);
+            in_edges_ = in_edges_deserialized.first;
+            current_position += in_edges_deserialized.second;
+            // Deserialize the out edges
+            const std::pair<std::vector<GraphEdge>, uint64_t> out_edges_deserialized = arc_utilities::DeserializeVector<GraphEdge>(buffer, current_position, GraphEdge::Deserialize);
+            out_edges_ = out_edges_deserialized.first;
+            current_position += out_edges_deserialized.second;
+            // Figure out how many bytes were read
+            const uint64_t bytes_read = current_position - current;
+            return bytes_read;
+        }
+
+        std::string Print() const
+        {
+            std::ostringstream strm;
+            strm << "Node : " << distance_ << " In Edges : ";
+            if (in_edges_.size() > 0)
+            {
+                strm << in_edges_[0].Print();
+                for (size_t idx = 1; idx < in_edges_.size(); idx++)
+                {
+                    strm << ", " << in_edges_[idx].Print();
+                }
+            }
+            strm << " Out Edges : ";
+            if (out_edges_.size() > 0)
+            {
+                strm << out_edges_[0].Print();
+                for (size_t idx = 1; idx < out_edges_.size(); idx++)
+                {
+                    strm << ", " << out_edges_[idx].Print();
+                }
+            }
+            return strm.str();
+        }
+
+        const NodeValueType& GetValueImmutable() const
+        {
+            return value_;
+        }
+
+        NodeValueType& GetValueMutable()
+        {
+            return value_;
+        }
+
+        void AddInEdge(const GraphEdge& new_in_edge)
+        {
+            in_edges_.push_back(new_in_edge);
+        }
+
+        void AddOutEdge(const GraphEdge& new_out_edge)
+        {
+            out_edges_.push_back(new_out_edge);
+        }
+
+        void AddEdgePair(const GraphEdge& new_in_edge, const GraphEdge& new_out_edge)
+        {
+            AddInEdge(new_in_edge);
+            AddOutEdge(new_out_edge);
+        }
+
+        double GetDistance() const
+        {
+            return distance_;
+        }
+
+        void SetDistance(const double distance)
+        {
+            distance_ = distance;
+        }
+
+        const std::vector<GraphEdge>& GetInEdgesImmutable() const
+        {
+            return in_edges_;
+        }
+
+        std::vector<GraphEdge>& GetInEdgesMutable()
+        {
+            return in_edges_;
+        }
+
+        const std::vector<GraphEdge>& GetOutEdgesImmutable() const
+        {
+            return out_edges_;
+        }
+
+        std::vector<GraphEdge>& GetOutEdgesMutable()
+        {
+            return out_edges_;
+        }
+
+        void SetInEdges(const std::vector<GraphEdge>& new_in_edges)
+        {
+            in_edges_ = new_in_edges;
+        }
+
+        void SetOutEdges(const std::vector<GraphEdge>& new_out_edges)
+        {
+            out_edges_ = new_out_edges;
+        }
+    };
+
+    template<typename NodeValueType, typename Allocator = std::allocator<NodeValueType>>
+    class Graph
+    {
+    protected:
+
+        std::vector<GraphNode<NodeValueType, Allocator>> nodes_;
+
+        size_t MarkConnectedComponentUndirected(
+                const int64_t starting_node_idx,
+                std::vector<uint32_t>& components,
+                const uint32_t component_id) const
+        {
+            // When we push into the queue, we mark as connected, so we don't need a separate "queued" tracker to
+            // avoid duplication, unlike what is used in CollisionMapGrid::MarkConnectedComponent
+            std::queue<int64_t> working_queue;
+            working_queue.push(starting_node_idx);
+            size_t num_marked = 1;
+
+            while (working_queue.size() > 0)
+            {
+                const auto next_node_idx = working_queue.front();
+                working_queue.pop();
+
+                const auto& out_edges = nodes_[next_node_idx].GetOutEdgesImmutable();
+                for (const auto& edge : out_edges)
+                {
+                    const auto neighbour_idx = edge.GetToIndex();
+                    if (components[neighbour_idx] == 0)
+                    {
+                        components[neighbour_idx] = component_id;
+                        ++num_marked;
+                        working_queue.push(neighbour_idx);
+                    }
+                }
+            }
+
+            return num_marked;
+        }
+
+    public:
+
+        static uint64_t Serialize(
+                const Graph<NodeValueType,
+                Allocator>& graph, std::vector<uint8_t>& buffer,
+                const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer)
+        {
+            return graph.SerializeSelf(buffer, value_serializer);
+        }
+
+        static std::pair<Graph<NodeValueType, Allocator>, uint64_t> Deserialize(
+                const std::vector<uint8_t>& buffer, const uint64_t current,
+                const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
+        {
+            Graph<NodeValueType, Allocator> temp_graph;
+            const uint64_t bytes_read = temp_graph.DeserializeSelf(buffer, current, value_deserializer);
+            return std::make_pair(temp_graph, bytes_read);
+        }
+
+        Graph(const std::vector<GraphNode<NodeValueType, Allocator>>& nodes)
+        {
+            if (CheckGraphLinkage(nodes))
+            {
+                nodes_ = nodes;
+            }
+            else
+            {
+                throw std::invalid_argument("Invalid graph linkage");
+            }
+        }
+
+        Graph(const size_t expected_size)
+        {
+            nodes_.reserve(expected_size);
+        }
+
+        Graph()
+        {}
+
+        uint64_t SerializeSelf(
+                std::vector<uint8_t>& buffer,
+                const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer) const
+        {
+            const uint64_t start_buffer_size = buffer.size();
+            const auto graph_state_serializer = std::bind(GraphNode<NodeValueType, Allocator>::Serialize, std::placeholders::_1, std::placeholders::_2, value_serializer);
+            arc_utilities::SerializeVector<GraphNode<NodeValueType, Allocator>>(nodes_, buffer, graph_state_serializer);
+            // Figure out how many bytes were written
+            const uint64_t end_buffer_size = buffer.size();
+            const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+            return bytes_written;
+        }
+
+        uint64_t DeserializeSelf(
+                const std::vector<uint8_t>& buffer,
+                const uint64_t current,
+                const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
+        {
+            const auto graph_state_deserializer = std::bind(GraphNode<NodeValueType, Allocator>::Deserialize, std::placeholders::_1, std::placeholders::_2, value_deserializer);
+            const auto deserialized_nodes = arc_utilities::DeserializeVector<GraphNode<NodeValueType, Allocator>>(buffer, current, graph_state_deserializer);
+            nodes_ = deserialized_nodes.first;
+            return deserialized_nodes.second;
+        }
+
+        std::string Print() const
+        {
+            std::ostringstream strm;
+            strm << "Graph - Nodes : ";
+            if (nodes_.size() > 0)
+            {
+                strm << nodes_[0].Print();
+                for (size_t idx = 1; idx < nodes_.size(); idx++)
+                {
+                    strm << "\n" << nodes_[idx].Print();
+                }
+            }
+            return strm.str();
+        }
+
+        void ShrinkToFit()
+        {
+            nodes_.shrink_to_fit();
+        }
+
+        bool CheckGraphLinkage() const
+        {
+            return CheckGraphLinkage(GetNodesImmutable());
+        }
+
+        static bool CheckGraphLinkage(const Graph<NodeValueType, Allocator>& graph)
+        {
+            return CheckGraphLinkage(graph.GetNodesImmutable());
+        }
+
+        static bool CheckGraphLinkage(const std::vector<GraphNode<NodeValueType, Allocator>>& nodes)
+        {
+            // Go through every node and make sure the edges are valid
+            for (size_t idx = 0; idx < nodes.size(); idx++)
+            {
+                const GraphNode<NodeValueType, Allocator>& current_node = nodes[idx];
+                // Check the in edges first
+                const std::vector<GraphEdge>& in_edges = current_node.GetInEdgesImmutable();
+                for (size_t in_edge_idx = 0; in_edge_idx < in_edges.size(); in_edge_idx++)
+                {
+                    const GraphEdge& current_edge = in_edges[in_edge_idx];
+                    // Check from index to make sure it's in bounds
+                    const int64_t from_index = current_edge.GetFromIndex();
+                    if (from_index < 0 || from_index >= (int64_t)nodes.size())
+                    {
+                        return false;
+                    }
+                    // Check to index to make sure it matches our own index
+                    const int64_t to_index = current_edge.GetToIndex();
+                    if (to_index != (int64_t)idx)
+                    {
+                        return false;
+                    }
+                    // Check edge validity (edges to ourself are not allowed)
+                    if (from_index == to_index)
+                    {
+                        return false;
+                    }
+                    // Check to make sure that the from index node is linked to us
+                    const GraphNode<NodeValueType, Allocator>& from_node = nodes[(size_t)from_index];
+                    const std::vector<GraphEdge>& from_node_out_edges = from_node.GetOutEdgesImmutable();
+                    bool from_node_connection_valid = false;
+                    // Make sure at least one out edge of the from index node corresponds to the current node
+                    for (size_t from_node_out_edge_idx = 0; from_node_out_edge_idx < from_node_out_edges.size(); from_node_out_edge_idx++)
+                    {
+                        const GraphEdge& current_from_node_out_edge = from_node_out_edges[from_node_out_edge_idx];
+                        if (current_from_node_out_edge.GetToIndex() == (int64_t)idx)
+                        {
+                            from_node_connection_valid = true;
+                        }
+                    }
+                    if (from_node_connection_valid == false)
+                    {
+                        return false;
+                    }
+                }
+                // Check the out edges second
+                const std::vector<GraphEdge>& out_edges = current_node.GetOutEdgesImmutable();
+                for (size_t out_edge_idx = 0; out_edge_idx < out_edges.size(); out_edge_idx++)
+                {
+                    const GraphEdge& current_edge = out_edges[out_edge_idx];
+                    // Check from index to make sure it matches our own index
+                    const int64_t from_index = current_edge.GetFromIndex();
+                    if (from_index != (int64_t)idx)
+                    {
+                        return false;
+                    }
+                    // Check to index to make sure it's in bounds
+                    const int64_t to_index = current_edge.GetToIndex();
+                    if (to_index < 0 || to_index >= (int64_t)nodes.size())
+                    {
+                        return false;
+                    }
+                    // Check edge validity (edges to ourself are not allowed)
+                    if (from_index == to_index)
+                    {
+                        return false;
+                    }
+                    // Check to make sure that the to index node is linked to us
+                    const GraphNode<NodeValueType, Allocator>& to_node = nodes[(size_t)to_index];
+                    const std::vector<GraphEdge>& to_node_in_edges = to_node.GetInEdgesImmutable();
+                    bool to_node_connection_valid = false;
+                    // Make sure at least one in edge of the to index node corresponds to the current node
+                    for (size_t to_node_in_edge_idx = 0; to_node_in_edge_idx < to_node_in_edges.size(); to_node_in_edge_idx++)
+                    {
+                        const GraphEdge& current_to_node_in_edge = to_node_in_edges[to_node_in_edge_idx];
+                        if (current_to_node_in_edge.GetFromIndex() == (int64_t)idx)
+                        {
+                            to_node_connection_valid = true;
+                        }
+                    }
+                    if (to_node_connection_valid == false)
+                    {
+                        return false;
+                    }
+                }
+            }
+            return true;
+        }
+
+        const std::vector<GraphNode<NodeValueType, Allocator>>& GetNodesImmutable() const
+        {
+            return nodes_;
+        }
+
+        std::vector<GraphNode<NodeValueType, Allocator>>& GetNodesMutable()
+        {
+            return nodes_;
+        }
+
+        const GraphNode<NodeValueType, Allocator>& GetNodeImmutable(const int64_t index) const
+        {
+            assert(index >= 0);
+            assert(index < (int64_t)nodes_.size());
+            return nodes_[(size_t)index];
+        }
+
+        GraphNode<NodeValueType, Allocator>& GetNodeMutable(const int64_t index)
+        {
+            assert(index >= 0);
+            assert(index < (int64_t)nodes_.size());
+            return nodes_[(size_t)index];
+        }
+
+        int64_t AddNode(const GraphNode<NodeValueType, Allocator>& new_node)
+        {
+            nodes_.push_back(new_node);
+            return (int64_t)(nodes_.size() - 1);
+        }
+
+        int64_t AddNode(const NodeValueType& new_value)
+        {
+            nodes_.push_back(GraphNode<NodeValueType, Allocator>(new_value));
+            return (int64_t)(nodes_.size() - 1);
+        }
+
+        void AddEdgeBetweenNodes(const int64_t from_index, const int64_t to_index, const double edge_weight)
+        {
+            assert(from_index >= 0);
+            assert(from_index < (int64_t)nodes_.size());
+            assert(to_index >= 0);
+            assert(to_index < (int64_t)nodes_.size());
+            assert(from_index != to_index);
+            const GraphEdge new_edge(from_index, to_index, edge_weight);
+            GetNodeMutable(from_index).AddOutEdge(new_edge);
+            GetNodeMutable(to_index).AddInEdge(new_edge);
+        }
+
+        void AddEdgesBetweenNodes(const int64_t first_index, const int64_t second_index, const double edge_weight)
+        {
+            assert(first_index >= 0);
+            assert(first_index < (int64_t)nodes_.size());
+            assert(second_index >= 0);
+            assert(second_index < (int64_t)nodes_.size());
+            assert(first_index != second_index);
+            const GraphEdge first_edge(first_index, second_index, edge_weight);
+            GetNodeMutable(first_index).AddOutEdge(first_edge);
+            GetNodeMutable(second_index).AddInEdge(first_edge);
+            const GraphEdge second_edge(second_index, first_index, edge_weight);
+            GetNodeMutable(second_index).AddOutEdge(second_edge);
+            GetNodeMutable(first_index).AddInEdge(second_edge);
+        }
+
+        /**
+         * @brief GetConnectedComponentsUndirected
+         * @return A vector of the component ids for each node, and the total number of components
+         */
+        std::pair<std::vector<uint32_t>, uint32_t> GetConnectedComponentsUndirected() const
+        {
+            size_t total_num_marked = 0;
+            auto connected_components = std::make_pair(std::vector<uint32_t>(nodes_.size(), 0), 0u);
+
+            for (size_t node_idx = 0; node_idx < nodes_.size() && total_num_marked < nodes_.size(); ++node_idx)
+            {
+                // If we have not yet marked this node, then mark it and anything it can reach
+                if (connected_components.first[node_idx] == 0)
+                {
+                    connected_components.second++;
+                    size_t num_marked = MarkConnectedComponentUndirected(node_idx, connected_components.first, connected_components.second);
+                    total_num_marked += num_marked;
+                }
+            }
+            return connected_components;
+        }
+    };
+
+    template<typename NodeValueType, typename Allocator = std::allocator<NodeValueType>>
+    class SimpleDijkstrasAlgorithm
+    {
+    protected:
+
+        class CompareIndexFn
+        {
+            public:
+
+                constexpr bool operator()(const std::pair<int64_t, double>& lhs, const std::pair<int64_t, double>& rhs) const
+                {
+                    return lhs.second > rhs.second;
+                }
+        };
+
+        SimpleDijkstrasAlgorithm() {}
+    public:
+
+        typedef std::pair<Graph<NodeValueType, Allocator>, std::pair<std::vector<int64_t>, std::vector<double>>> DijkstrasResult;
+
+        static DijkstrasResult PerformDijkstrasAlgorithm(
+                const Graph<NodeValueType, Allocator>& graph,
+                const int64_t start_index)
+        {
+            if ((start_index < 0) && (start_index >= (int64_t)graph.GetNodesImmutable().size()))
+            {
+                throw std::invalid_argument("Start index out of range");
+            }
+            Graph<NodeValueType, Allocator> working_copy = graph;
+            // Setup
+            std::vector<int64_t> previous_index_map(working_copy.GetNodesImmutable().size(), -1);
+            std::vector<double> distances(working_copy.GetNodesImmutable().size(), std::numeric_limits<double>::infinity());
+            std::priority_queue<std::pair<int64_t, double>, std::vector<std::pair<int64_t, double>>, CompareIndexFn> queue;
+            std::unordered_map<int64_t, uint32_t> explored(graph.GetNodesImmutable().size());
+            for (size_t idx = 0; idx < working_copy.GetNodesImmutable().size(); idx++)
+            {
+                working_copy.GetNodeMutable((int64_t)idx).SetDistance(std::numeric_limits<double>::infinity());
+                queue.push(std::make_pair((int64_t)idx, std::numeric_limits<double>::infinity()));
+            }
+            working_copy.GetNodeMutable(start_index).SetDistance(0.0);
+            previous_index_map[(size_t)start_index] = start_index;
+            distances[(size_t)start_index] = 0.0;
+            queue.push(std::make_pair(start_index, 0.0));
+            while (queue.size() > 0)
+            {
+                const std::pair<int64_t, double> top_node = queue.top();
+                const int64_t& top_node_index = top_node.first;
+                const double& top_node_distance = top_node.second;
+                queue.pop();
+                if (explored[top_node.first] > 0)
+                {
+                    // We've already been here
+                    continue;
+                }
+                else
+                {
+                    // Note that we've been here
+                    explored[top_node.first] = 1;
+                    // Get our neighbors
+                    const std::vector<GraphEdge>& neighbor_edges = working_copy.GetNodeImmutable(top_node_index).GetInEdgesImmutable();
+                    // Go through our neighbors
+                    for (size_t neighbor_idx = 0; neighbor_idx < neighbor_edges.size(); neighbor_idx++)
+                    {
+                        const int64_t neighbor_index = neighbor_edges[neighbor_idx].GetFromIndex();
+                        const double neighbor_edge_weight = neighbor_edges[neighbor_idx].GetWeight();
+                        const double new_neighbor_distance = top_node_distance + neighbor_edge_weight;
+                        // Check against the neighbor
+                        const double stored_neighbor_distance = working_copy.GetNodeImmutable(neighbor_index).GetDistance();
+                        if (new_neighbor_distance < stored_neighbor_distance)
+                        {
+                            // We've found a better way to get to this node
+                            // Check if it's already been explored
+                            if (explored[neighbor_index] > 0)
+                            {
+                                // If it's already been explored, we just update it in place
+                                working_copy.GetNodeMutable(neighbor_index).SetDistance(new_neighbor_distance);
+                            }
+                            else
+                            {
+                                // If it hasn't been explored, we need to update it and add it to the queue
+                                working_copy.GetNodeMutable(neighbor_index).SetDistance(new_neighbor_distance);
+                                queue.push(std::make_pair(neighbor_index, new_neighbor_distance));
+                            }
+                            // Update that we're the best previous node
+                            previous_index_map[(size_t)neighbor_index] = top_node_index;
+                            distances[(size_t)neighbor_index] = new_neighbor_distance;
+                        }
+                        else
+                        {
+                            // Do nothing
+                            continue;
+                        }
+                    }
+                }
+            }
+            return std::make_pair(working_copy, std::make_pair(previous_index_map, distances));
+        }
+
+        // These functions have not been tested.  Use with care.
+        static uint64_t SerializeDijstrasResult(
+                const DijkstrasResult& result,
+                std::vector<uint8_t>& buffer,
+                const std::function<uint64_t(const NodeValueType&, std::vector<uint8_t>&)>& value_serializer)
+        {
+            const uint64_t start_buffer_size = buffer.size();
+            // Serialize the graph
+            result.first.SerializeSelf(buffer, value_serializer);
+            // Serialize the previous index
+            const auto index_serializer = std::bind(arc_utilities::SerializeFixedSizePOD<int64_t>, std::placeholders::_1, std::placeholders::_2);
+            SerializeVector(result.second.first, index_serializer);
+            // Serialze the distances
+            const auto distance_serializer = std::bind(arc_utilities::SerializeFixedSizePOD<double>, std::placeholders::_1, std::placeholders::_2);
+            SerializeVector(result.second.second, distance_serializer);
+            // Figure out how many bytes were written
+            const uint64_t end_buffer_size = buffer.size();
+            const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+            return bytes_written;
+        }
+
+        // These functions have not been tested.  Use with care.
+        static std::pair<DijkstrasResult, uint64_t> DijstrasResult(
+                const std::vector<uint8_t>& buffer,
+                const uint64_t current,
+                const std::function<std::pair<NodeValueType, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
+        {
+            assert(current < buffer.size());
+            uint64_t current_position = current;
+            // Deserialize the graph itself
+            std::pair<DijkstrasResult, uint64_t> deserialized;
+            const auto graph_deserialized = Graph<NodeValueType, Allocator>::Deserialize(buffer, current_position, value_deserializer);
+            deserialized.first.first = graph_deserialized.first;
+            current_position += graph_deserialized.second;
+            // Deserialize the previous index
+            const auto index_deserializer = std::bind(arc_utilities::DeserializeFixedSizePOD<int64_t>, std::placeholders::_1, std::placeholders::_2);
+            const auto prev_index_deserialized = arc_utilities::DeserializeVector<int64_t>(buffer, current_position, index_deserializer);
+            deserialized.first.second.first = prev_index_deserialized.first;
+            current_position += prev_index_deserialized.second;
+            // Deserialize the distances
+            const auto distance_deserializer = std::bind(arc_utilities::DeserializeFixedSizePOD<double>, std::placeholders::_1, std::placeholders::_2);
+            const auto distance_deserialized = arc_utilities::DeserializeVector<double>(buffer, current_position, distance_deserializer);
+            deserialized.first.second.second = distance_deserialized.first;
+            current_position += distance_deserialized.second;
+            // Figure out how many bytes were read
+            deserialized.second = current_position - current;
+            return deserialized;
+        }
+    };
+
+    template<typename NodeValueType, typename Allocator = std::allocator<NodeValueType>>
+    class SimpleGraphAstar
+    {
+    protected:
+
+        SimpleGraphAstar() {}
+
+    public:
+
+        static arc_helpers::AstarResult PerformLazyAstar(
+                const Graph<NodeValueType, Allocator>& graph,
+                const int64_t start_index,
+                const int64_t goal_index,
+                const std::function<bool(const Graph<NodeValueType, Allocator>&, const GraphEdge&)>& edge_validity_check_fn,
+                const std::function<double(const Graph<NodeValueType, Allocator>&, const GraphEdge&)>& distance_fn,
+                const std::function<double(const NodeValueType&, const NodeValueType&)>& heuristic_fn,
+                const bool limit_pqueue_duplicates)
+        {
+            // Enforced sanity checks
+            if ((start_index < 0) && (start_index >= (int64_t)graph.GetNodesImmutable().size()))
+            {
+                throw std::invalid_argument("Start index out of range");
+            }
+            if ((goal_index < 0) && (goal_index >= (int64_t)graph.GetNodesImmutable().size()))
+            {
+                throw std::invalid_argument("Goal index out of range");
+            }
+            if (start_index == goal_index)
+            {
+                throw std::invalid_argument("Start and goal indices must be different");
+            }
+            // Make helper function
+            const auto heuristic_function = [&] (const int64_t node_index)
+            {
+                return heuristic_fn(graph.GetNodeImmutable(node_index).GetValueImmutable(), graph.GetNodeImmutable(goal_index).GetValueImmutable());
+            };
+            // Setup
+            std::priority_queue<arc_helpers::AstarPQueueElement, std::vector<arc_helpers::AstarPQueueElement>, arc_helpers::CompareAstarPQueueElementFn> queue;
+            // Optional map to reduce the number of duplicate items added to the pqueue
+            // Key is the node index in the provided graph
+            // Value is cost-to-come
+            std::unordered_map<int64_t, double> queue_members_map;
+            // Key is the node index in the provided graph
+            // Value is a pair<backpointer, cost-to-come>
+            // backpointer is the parent index in the provided graph
+            std::unordered_map<int64_t, std::pair<int64_t, double>> explored;
+            // Initialize
+            queue.push(arc_helpers::AstarPQueueElement(start_index, -1, 0.0, heuristic_function(start_index)));
+            if (limit_pqueue_duplicates)
+            {
+                queue_members_map[start_index] = 0.0;
+            }
+            // Search
+            while (queue.size() > 0)
+            {
+                // Get the top of the priority queue
+                const arc_helpers::AstarPQueueElement top_node = queue.top();
+                queue.pop();
+                // Remove from queue map if necessary
+                if (limit_pqueue_duplicates)
+                {
+                    queue_members_map.erase(top_node.NodeID());
+                }
+                // Check if the node has already been discovered
+                const auto explored_itr = explored.find(top_node.NodeID());
+                // We have not been here before, or it is cheaper now
+                const bool in_explored = (explored_itr != explored.end());
+                const bool explored_is_better = (in_explored) ? (top_node.CostToCome() >= explored_itr->second.second) : false;
+                if (!explored_is_better)
+                {
+                    // Add to the explored list
+                    explored[top_node.NodeID()] = std::make_pair(top_node.Backpointer(), top_node.CostToCome());
+                    // Check if we have reached the goal
+                    if (top_node.NodeID() == goal_index)
+                    {
+                        break;
+                    }
+                    // Explore and add the children
+                    const std::vector<GraphEdge>& out_edges = graph.GetNodeImmutable(top_node.NodeID()).GetOutEdgesImmutable();
+                    for (size_t out_edge_idx = 0; out_edge_idx < out_edges.size(); out_edge_idx++)
+                    {
+                        // Get the next potential child node
+                        const GraphEdge& current_out_edge = out_edges[out_edge_idx];
+                        const int64_t child_node_index = current_out_edge.GetToIndex();
+                        // Check if the top node->child edge is valid
+                        if (edge_validity_check_fn(graph, current_out_edge))
+                        {
+                            // Compute the cost-to-come for the new child
+                            const double parent_cost_to_come = top_node.CostToCome();
+                            const double parent_to_child_cost = distance_fn(graph, current_out_edge);
+                            const double child_cost_to_come = parent_cost_to_come + parent_to_child_cost;
+                            // Check if the child state has already been explored
+                            const auto explored_itr = explored.find(child_node_index);
+                            // It is not in the explored list, or is there with a higher cost-to-come
+                            const bool in_explored = (explored_itr != explored.end());
+                            const bool explored_is_better = (in_explored) ? (child_cost_to_come >= explored_itr->second.second) : false;
+                            // Check if the child state is already in the queue
+                            bool queue_is_better = false;
+                            if (limit_pqueue_duplicates)
+                            {
+                                const auto queue_members_map_itr = queue_members_map.find(child_node_index);
+                                const bool in_queue = (queue_members_map_itr != queue_members_map.end());
+                                queue_is_better = (in_queue) ? (child_cost_to_come >= queue_members_map_itr->second) : false;
+                            }
+                            // Only add the new state if we need to
+                            if (!explored_is_better && !queue_is_better)
+                            {
+                                // Compute the heuristic for the child
+                                const double child_heuristic = heuristic_function(child_node_index);
+                                // Compute the child value
+                                const double child_value = child_cost_to_come + child_heuristic;
+                                queue.push(arc_helpers::AstarPQueueElement(child_node_index, top_node.NodeID(), child_cost_to_come, child_value));
+                            }
+                        }
+                    }
+                }
+            }
+            return arc_helpers::ExtractAstarResult(explored, start_index, goal_index);
+        }
+
+        static arc_helpers::AstarResult PerformLazyAstar(
+                const Graph<NodeValueType, Allocator>& graph,
+                const int64_t start_index,
+                const int64_t goal_index,
+                const std::function<bool(const NodeValueType&, const NodeValueType&)>& edge_validity_check_fn,
+                const std::function<double(const NodeValueType&, const NodeValueType&)>& distance_fn,
+                const std::function<double(const NodeValueType&, const NodeValueType&)>& heuristic_fn,
+                const bool limit_pqueue_duplicates)
+        {
+            const auto edge_validity_check_function = [&] (const Graph<NodeValueType, Allocator>& graph, const GraphEdge& edge)
+            {
+                return edge_validity_check_fn(graph.GetNodeImmutable(edge.GetFromIndex()).GetValueImmutable(), graph.GetNodeImmutable(edge.GetToIndex()).GetValueImmutable());
+            };
+            const auto distance_function = [&] (const Graph<NodeValueType, Allocator>& graph, const GraphEdge& edge)
+            {
+                return distance_fn(graph.GetNodeImmutable(edge.GetFromIndex()).GetValueImmutable(), graph.GetNodeImmutable(edge.GetToIndex()).GetValueImmutable());
+            };
+            return PerformLazyAstar(graph, start_index, goal_index, edge_validity_check_function, distance_function, heuristic_fn, limit_pqueue_duplicates);
+        }
+
+        static arc_helpers::AstarResult PerformAstar(
+                const Graph<NodeValueType, Allocator>& graph,
+                const int64_t start_index,
+                const int64_t goal_index,
+                const std::function<double(const NodeValueType&, const NodeValueType&)>& heuristic_fn,
+                const bool limit_pqueue_duplicates)
+        {
+            const auto edge_validity_check_function = [&] (const Graph<NodeValueType, Allocator>& graph, const GraphEdge& edge)
+            {
+                UNUSED(graph);
+                if (edge.GetWeight() < std::numeric_limits<double>::infinity())
+                {
+                    return true;
+                }
+                else
+                {
+                    return false;
+                }
+            };
+            const auto distance_function = [&] (const Graph<NodeValueType, Allocator>& graph, const GraphEdge& edge)
+            {
+                UNUSED(graph);
+                return edge.GetWeight();
+            };
+            return PerformLazyAstar(graph, start_index, goal_index, edge_validity_check_function, distance_function, heuristic_fn, limit_pqueue_duplicates);
+        }
+    };
+
+    template<typename NodeValueType, typename Allocator = std::allocator<NodeValueType>>
+    class GraphRandomWalk
+    {
+    protected:
+        GraphRandomWalk() {}
+
+    public:
+
+        template <typename Generator>
+        static std::vector<int64_t> PerformRandomWalk(
+                const Graph<NodeValueType, Allocator>& graph,
+                const int64_t start_index,
+                const int64_t goal_index,
+                Generator& generator)
+        {
+            std::uniform_int_distribution<int64_t> uniform_int_distribution;
+
+            std::vector<int64_t> path(1, start_index);
+
+            while (path.back() != goal_index)
+            {
+                // Collect data from the current node
+                const int64_t curr_index = path.back();
+                const auto& out_edges = graph.GetNodeImmutable(curr_index).GetOutEdgesImmutable();
+                const auto num_edges = out_edges.size();
+
+                // Determine which node to step to next
+                std::uniform_int_distribution<int64_t>::param_type params(0, (int64_t)(num_edges - 1));
+                uniform_int_distribution.param(params);
+                const int64_t next_step = uniform_int_distribution(generator);
+                const auto next_index = out_edges.at(next_step).GetToIndex();
+
+                // If the next index is somewhere we've been already, then "trim" the loop off
+                const auto it = std::find(path.begin(), path.end(), next_index);
+                path.erase(it, path.end());
+
+                // (Re)add the new index to the path
+                path.push_back(next_index);
+            }
+
+            return path;
+        }
+    };
+}
+
+#endif // DIJKSTRAS_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/eigen_helpers_conversions.hpp

@@ -0,0 +1,285 @@
+#include <Eigen/Geometry>
+#include <Eigen/Jacobi>
+#include <Eigen/SVD>
+#include <stdio.h>
+#include <iostream>
+#include <arc_utilities/pretty_print.hpp>
+#include <functional>
+#include <ros/ros.h>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Transform.h>
+#include <geometry_msgs/Wrench.h>
+#include <arc_utilities/eigen_helpers.hpp>
+
+#ifndef EIGEN_HELPERS_CONVERSIONS_HPP
+#define EIGEN_HELPERS_CONVERSIONS_HPP
+
+namespace EigenHelpersConversions
+{
+    inline Eigen::Vector3d GeometryPointToEigenVector3d(const geometry_msgs::Point& point)
+    {
+        Eigen::Vector3d eigen_point(point.x, point.y, point.z);
+        return eigen_point;
+    }
+
+    inline geometry_msgs::Point EigenVector3dToGeometryPoint(const Eigen::Vector3d& point)
+    {
+        geometry_msgs::Point geom_point;
+        geom_point.x = point.x();
+        geom_point.y = point.y();
+        geom_point.z = point.z();
+        return geom_point;
+    }
+
+    inline Eigen::Vector4d GeometryPointToEigenVector4d(const geometry_msgs::Point& point)
+    {
+        Eigen::Vector4d eigen_point(point.x, point.y, point.z, 1.0);
+        return eigen_point;
+    }
+
+    inline geometry_msgs::Point EigenVector4dToGeometryPoint(const Eigen::Vector4d& point)
+    {
+        geometry_msgs::Point geom_point;
+        geom_point.x = point(0);
+        geom_point.y = point(1);
+        geom_point.z = point(2);
+        return geom_point;
+    }
+
+    inline geometry_msgs::PointStamped EigenVector3dToGeometryPointStamped(const Eigen::Vector3d& point, const std::string& frame_id)
+    {
+        geometry_msgs::PointStamped point_stamped;
+        point_stamped.header.frame_id = frame_id;
+        point_stamped.point = EigenVector3dToGeometryPoint(point);
+        return point_stamped;
+    }
+
+    inline Eigen::Vector3d GeometryVector3ToEigenVector3d(const geometry_msgs::Vector3& vector)
+    {
+        Eigen::Vector3d eigen_vector(vector.x, vector.y, vector.z);
+        return eigen_vector;
+    }
+
+    inline geometry_msgs::Vector3 EigenVector3dToGeometryVector3(const Eigen::Vector3d& vector)
+    {
+        geometry_msgs::Vector3 geom_vector;
+        geom_vector.x = vector.x();
+        geom_vector.y = vector.y();
+        geom_vector.z = vector.z();
+        return geom_vector;
+    }
+
+    inline Eigen::Quaterniond GeometryQuaternionToEigenQuaterniond(const geometry_msgs::Quaternion& quat)
+    {
+        Eigen::Quaterniond eigen_quaternion(quat.w, quat.x, quat.y, quat.z);
+        return eigen_quaternion;
+    }
+
+    inline geometry_msgs::Quaternion EigenQuaterniondToGeometryQuaternion(const Eigen::Quaterniond& quat)
+    {
+        geometry_msgs::Quaternion geom_quaternion;
+        geom_quaternion.w = quat.w();
+        geom_quaternion.x = quat.x();
+        geom_quaternion.y = quat.y();
+        geom_quaternion.z = quat.z();
+        return geom_quaternion;
+    }
+
+    inline Eigen::Isometry3d GeometryPoseToEigenIsometry3d(const geometry_msgs::Pose& pose)
+    {
+        const Eigen::Translation3d trans(pose.position.x, pose.position.y, pose.position.z);
+        const Eigen::Quaterniond quat(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
+        const Eigen::Isometry3d eigen_pose = trans * quat;
+        return eigen_pose;
+    }
+
+    inline geometry_msgs::Pose EigenIsometry3dToGeometryPose(const Eigen::Isometry3d& transform)
+    {
+        const Eigen::Vector3d trans = transform.translation();
+        const Eigen::Quaterniond quat(transform.rotation());
+        geometry_msgs::Pose geom_pose;
+        geom_pose.position.x = trans.x();
+        geom_pose.position.y = trans.y();
+        geom_pose.position.z = trans.z();
+        geom_pose.orientation.w = quat.w();
+        geom_pose.orientation.x = quat.x();
+        geom_pose.orientation.y = quat.y();
+        geom_pose.orientation.z = quat.z();
+        return geom_pose;
+    }
+
+    inline geometry_msgs::PoseStamped EigenIsometry3dToGeometryPoseStamped(const Eigen::Isometry3d& transform, const std::string& frame_id)
+    {
+        geometry_msgs::PoseStamped pose_stamped;
+        pose_stamped.header.frame_id = frame_id;
+        pose_stamped.pose = EigenIsometry3dToGeometryPose(transform);
+        return pose_stamped;
+    }
+
+    inline Eigen::Isometry3d GeometryTransformToEigenIsometry3d(const geometry_msgs::Transform& transform)
+    {
+        const Eigen::Translation3d trans(transform.translation.x, transform.translation.y, transform.translation.z);
+        const Eigen::Quaterniond quat(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z);
+        const Eigen::Isometry3d eigen_transform = trans * quat;
+        return eigen_transform;
+    }
+
+    inline geometry_msgs::Transform EigenIsometry3dToGeometryTransform(const Eigen::Isometry3d& transform)
+    {
+        const Eigen::Vector3d trans = transform.translation();
+        const Eigen::Quaterniond quat(transform.rotation());
+        geometry_msgs::Transform geom_transform;
+        geom_transform.translation.x = trans.x();
+        geom_transform.translation.y = trans.y();
+        geom_transform.translation.z = trans.z();
+        geom_transform.rotation.w = quat.w();
+        geom_transform.rotation.x = quat.x();
+        geom_transform.rotation.y = quat.y();
+        geom_transform.rotation.z = quat.z();
+        return geom_transform;
+    }
+
+    inline Eigen::Matrix3Xd VectorGeometryPointToEigenMatrix3Xd(const std::vector<geometry_msgs::Point>& vector_geom)
+    {
+        Eigen::Matrix3Xd eigen_matrix = Eigen::MatrixXd(3, vector_geom.size());
+        for (size_t idx = 0; idx < vector_geom.size(); idx++)
+        {
+            eigen_matrix.block<3,1>(0, idx) = GeometryPointToEigenVector3d(vector_geom[idx]);
+        }
+        return eigen_matrix;
+    }
+
+    inline std::vector<geometry_msgs::Point> EigenMatrix3XdToVectorGeometryPoint(const Eigen::Matrix3Xd& eigen_matrix)
+    {
+        std::vector<geometry_msgs::Point> vector_geom(eigen_matrix.cols());
+        for (size_t idx = 0; idx < vector_geom.size(); idx++)
+        {
+            vector_geom[idx] = EigenVector3dToGeometryPoint(eigen_matrix.block<3,1>(0,idx));
+        }
+        return vector_geom;
+    }
+
+    inline std::vector<geometry_msgs::Point> VectorEigenVector3dToVectorGeometryPoint(const EigenHelpers::VectorVector3d& vector_eigen)
+    {
+        std::vector<geometry_msgs::Point> vector_geom(vector_eigen.size());
+        for (size_t idx = 0; idx < vector_eigen.size(); idx++)
+        {
+            vector_geom[idx] = EigenVector3dToGeometryPoint(vector_eigen[idx]);
+        }
+        return vector_geom;
+    }
+
+    inline Eigen::Matrix3Xd VectorEigenVector3dToEigenMatrix3Xd(const EigenHelpers::VectorVector3d& vector_eigen)
+    {
+        Eigen::Matrix3Xd eigen_matrix(3, vector_eigen.size());
+        for (size_t idx = 0; idx < vector_eigen.size(); idx++)
+        {
+            eigen_matrix.col(idx) = vector_eigen[idx];
+        }
+        return eigen_matrix;
+    }
+
+    inline EigenHelpers::VectorVector3d VectorGeometryPointToVectorEigenVector3d(const std::vector<geometry_msgs::Point>& vector_geom)
+    {
+        EigenHelpers::VectorVector3d vector_eigen(vector_geom.size());
+        for (size_t idx = 0; idx < vector_geom.size(); idx++)
+        {
+            vector_eigen[idx] = GeometryPointToEigenVector3d(vector_geom[idx]);
+        }
+        return vector_eigen;
+    }
+
+    inline EigenHelpers::VectorVector3d VectorGeometryVector3ToEigenVector3d(const std::vector<geometry_msgs::Vector3>& vector_geom)
+    {
+        EigenHelpers::VectorVector3d vector_eigen(vector_geom.size());
+        for (size_t idx = 0; idx < vector_geom.size(); idx++)
+        {
+            vector_eigen[idx] = GeometryVector3ToEigenVector3d(vector_geom[idx]);
+        }
+        return vector_eigen;
+    }
+
+    inline EigenHelpers::VectorIsometry3d VectorGeometryPoseToVectorIsometry3d(const std::vector<geometry_msgs::Pose>& vector_geom)
+    {
+        EigenHelpers::VectorIsometry3d vector_eigen(vector_geom.size());
+        for (size_t idx = 0; idx < vector_geom.size(); idx++)
+        {
+            vector_eigen[idx] = GeometryPoseToEigenIsometry3d(vector_geom[idx]);
+        }
+        return vector_eigen;
+    }
+
+    inline EigenHelpers::VectorIsometry3d VectorGeometryPoseToVectorIsometry3d(const std::vector<geometry_msgs::Transform>& vector_geom)
+    {
+        EigenHelpers::VectorIsometry3d vector_eigen(vector_geom.size());
+        for (size_t idx = 0; idx < vector_geom.size(); idx++)
+        {
+            vector_eigen[idx] = GeometryTransformToEigenIsometry3d(vector_geom[idx]);
+        }
+        return vector_eigen;
+    }
+
+    inline std::vector<geometry_msgs::Pose> VectorIsometry3dToVectorGeometryPose(const EigenHelpers::VectorIsometry3d& vector_eigen)
+    {
+        std::vector<geometry_msgs::Pose> vector_geom(vector_eigen.size());
+        for (size_t idx = 0; idx < vector_eigen.size(); idx++)
+        {
+            vector_geom[idx] = EigenIsometry3dToGeometryPose(vector_eigen[idx]);
+        }
+        return vector_geom;
+    }
+
+    inline std::vector<geometry_msgs::Transform> VectorIsometry3dToVectorGeometryTransform(const EigenHelpers::VectorIsometry3d& vector_eigen)
+    {
+        std::vector<geometry_msgs::Transform> vector_geom(vector_eigen.size());
+        for (size_t idx = 0; idx < vector_eigen.size(); idx++)
+        {
+            vector_geom[idx] = EigenIsometry3dToGeometryTransform(vector_eigen[idx]);
+        }
+        return vector_geom;
+    }
+
+    // Convert wrench (force and torque) ROS message to Eigen typed data
+    inline std::pair<Eigen::Vector3d, Eigen::Vector3d> GeometryWrenchToEigenPairVector(const geometry_msgs::Wrench& wrench)
+    {
+        const Eigen::Vector3d eigen_force(wrench.force.x, wrench.force.y, wrench.force.z);
+        const Eigen::Vector3d eigen_torque(wrench.torque.x, wrench.torque.y, wrench.torque.z);
+        const std::pair<Eigen::Vector3d, Eigen::Vector3d> eigen_wrench = std::make_pair(eigen_force, eigen_torque);
+        return eigen_wrench;
+    }
+
+    template<typename data_type, int LENGTH>
+    inline Eigen::Matrix<data_type, Eigen::Dynamic, 1> VectorEigenVectorToEigenVectorX(const std::vector<Eigen::Matrix<data_type, LENGTH, 1>>& vector_eigen_input)
+    {
+        assert(vector_eigen_input.size() > 0);
+
+        Eigen::Matrix<data_type, Eigen::Dynamic, 1> eigen_result;
+        eigen_result.resize((ssize_t)vector_eigen_input.size() * vector_eigen_input[0].rows());
+
+        for (size_t idx = 0; idx < vector_eigen_input.size(); idx++)
+        {
+            eigen_result.segment((ssize_t)idx * LENGTH, LENGTH) = vector_eigen_input[idx];
+        }
+
+        return eigen_result;
+    }
+
+    template<typename data_type, int LENGTH>
+    inline std::vector<Eigen::Matrix<data_type, LENGTH, 1>, Eigen::aligned_allocator<Eigen::Matrix<data_type, LENGTH, 1>>> EigenVectorXToVectorEigenVector(const Eigen::Matrix<data_type, Eigen::Dynamic, 1>& eigen_input)
+    {
+        assert(eigen_input.rows() % LENGTH == 0);
+        size_t num_vectors = eigen_input.rows() / LENGTH;
+
+        std::vector<Eigen::Matrix<data_type, LENGTH, 1>, Eigen::aligned_allocator<Eigen::Matrix<data_type, LENGTH, 1>>> vector_eigen_output(num_vectors);
+
+        for (size_t idx = 0; idx < num_vectors; idx++)
+        {
+            vector_eigen_output[idx] = eigen_input.segment<LENGTH>((ssize_t)idx * LENGTH);
+        }
+
+        return vector_eigen_output;
+    }
+}
+
+#endif // EIGEN_HELPERS_CONVERSIONS_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/filesystem.hpp

@@ -0,0 +1,35 @@
+#ifndef FILESYSTEM_HPP
+#define FILESYSTEM_HPP
+
+#include <iostream>
+#include <boost/filesystem.hpp>
+#include "arc_utilities/arc_exceptions.hpp"
+
+namespace arc_utilities
+{
+    void CreateDirectory(const boost::filesystem::path& p)
+    {
+        if (!boost::filesystem::is_directory(p))
+        {
+            std::cout << "\x1b[33;1m" << p << " does not exist! Creating ... ";
+
+            // NOTE: create_directories should be able to return true in this case
+            // however due to a bug related to a trailing '/' this is not currently
+            // the case in my version of boost
+            // https://svn.boost.org/trac/boost/ticket/7258
+            boost::filesystem::create_directories(p);
+            if (boost::filesystem::is_directory(p))
+    //            if (boost::filesystem::create_directories(p))
+            {
+                std::cout << "Succeeded!\x1b[37m\n";
+            }
+            else
+            {
+                std::cout << "\x1b[31;1mFailed!\x1b[37m\n";
+                throw_arc_exception(std::runtime_error, "Unable to create directory, likely a 'trailing slash' issue");
+            }
+        }
+    }
+}
+
+#endif // FILESYSTEM_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/first_order_deformation.h

@@ -0,0 +1,21 @@
+#ifndef FIRST_ORDER_DEFORMATION_H
+#define FIRST_ORDER_DEFORMATION_H
+
+#include <functional>
+
+namespace arc_utilities
+{
+    namespace FirstOrderDeformation
+    {
+        typedef std::pair<ssize_t, ssize_t> ConfigType;
+        typedef std::pair<ConfigType, double> ConfigAndDistType;
+        typedef std::function<bool(const ssize_t row, const ssize_t col)> ValidityCheckFnType;
+
+        bool CheckFirstOrderDeformation(
+                const ssize_t rows,
+                const ssize_t cols,
+                const ValidityCheckFnType& validity_check_fn);
+    }
+}
+
+#endif // FIRST_ORDER_DEFORMATION_H

flightlib/third_party/arc_utilities/include/arc_utilities/get_neighbours.hpp

@@ -0,0 +1,114 @@
+#ifndef GET_NEIGHBOURS_HPP
+#define GET_NEIGHBOURS_HPP
+
+#include <algorithm>
+#include <functional>
+#include <vector>
+
+namespace arc_utilities
+{
+    namespace GetNeighbours
+    {
+        template<typename ConfigType, typename StepType, typename Allocator = std::allocator<ConfigType>>
+        inline std::vector<ConfigType, Allocator> TwoDimensional8Connected(
+                const ConfigType& config,
+                const StepType& min_x,
+                const StepType& max_x,
+                const StepType& min_y,
+                const StepType& max_y,
+                const StepType& step_size,
+                const std::function<ConfigType(const ConfigType&)>& round_to_grid_fn,
+                const std::function<bool(const ConfigType&)>& validity_check_fn)
+        {
+            std::vector<ConfigType, Allocator> neighbours;
+            neighbours.reserve(8);
+
+            const StepType x_min = std::max(min_x, config[0] - step_size);
+            const StepType x_max = std::min(max_x, config[0] + step_size);
+
+            const StepType y_min = std::max(min_y, config[1] - step_size);
+            const StepType y_max = std::min(max_y, config[1] + step_size);
+
+            const ConfigType min_vector = round_to_grid_fn(ConfigType(x_min, y_min));
+            const ConfigType max_vector = round_to_grid_fn(ConfigType(x_max, y_max));
+
+            for (int x_offset = -1; x_offset <= 1; ++x_offset)
+            {
+                const double x = config[0] + step_size * x_offset;
+                for (int y_offset = -1; y_offset <= 1; ++y_offset)
+                {
+                    const double y = config[1] + step_size * y_offset;
+                    const ConfigType neighbour = round_to_grid_fn(ConfigType(x, y));
+
+                    if (min_vector[0] <= neighbour[0] && neighbour[0] <= max_vector[0] &&
+                        min_vector[1] <= neighbour[1] && neighbour[1] <= max_vector[1] &&
+                        (neighbour[0] != config[0] || neighbour[1] != config[1]) &&
+                        validity_check_fn(neighbour) == true)
+                    {
+                        neighbours.push_back(neighbour);
+                    }
+                }
+            }
+
+            neighbours.shrink_to_fit();
+            return neighbours;
+        }
+
+        template<typename ConfigType, typename StepType, typename Allocator = std::allocator<ConfigType>>
+        inline std::vector<ConfigType, Allocator> ThreeDimensional8Connected(
+                const ConfigType& config,
+                const StepType& min_x,
+                const StepType& max_x,
+                const StepType& min_y,
+                const StepType& max_y,
+                const StepType& min_z,
+                const StepType& max_z,
+                const StepType& step_size,
+                const std::function<ConfigType(const ConfigType&)>& round_to_grid_fn,
+                const std::function<bool(const ConfigType&)>& validity_check_fn)
+        {
+            std::vector<ConfigType, Allocator> neighbours;
+            neighbours.reserve(26);
+
+            const StepType x_min = std::max(min_x, config[0] - step_size);
+            const StepType x_max = std::min(max_x, config[0] + step_size);
+
+            const StepType y_min = std::max(min_y, config[1] - step_size);
+            const StepType y_max = std::min(max_y, config[1] + step_size);
+
+            const StepType z_min = std::max(min_z, config[2] - step_size);
+            const StepType z_max = std::min(max_z, config[2] + step_size);
+
+            const ConfigType min_vector = round_to_grid_fn(ConfigType(x_min, y_min, z_min));
+            const ConfigType max_vector = round_to_grid_fn(ConfigType(x_max, y_max, z_max));
+
+            for (int x_offset = -1; x_offset <= 1; ++x_offset)
+            {
+                const double x = config[0] + step_size * x_offset;
+                for (int y_offset = -1; y_offset <= 1; ++y_offset)
+                {
+                    const double y = config[1] + step_size * y_offset;
+                    for (int z_offset = -1; z_offset <= 1; ++z_offset)
+                    {
+                        const double z = config[2] + step_size * z_offset;
+                        const ConfigType neighbour = round_to_grid_fn(ConfigType(x, y, z));
+
+                        if (min_vector[0] <= neighbour[0] && neighbour[0] <= max_vector[0] &&
+                            min_vector[1] <= neighbour[1] && neighbour[1] <= max_vector[1] &&
+                            min_vector[2] <= neighbour[2] && neighbour[2] <= max_vector[2] &&
+                            (neighbour[0] != config[0] || neighbour[1] != config[1] || neighbour[2] != config[2]) &&
+                            validity_check_fn(neighbour) == true)
+                        {
+                            neighbours.push_back(neighbour);
+                        }
+                    }
+                }
+            }
+
+            neighbours.shrink_to_fit();
+            return neighbours;
+        }
+    }
+}
+
+#endif // GET_NEIGHBOURS_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/iiwa_14_fk_fast.hpp

@@ -0,0 +1,171 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <vector>
+#include <map>
+#include <Eigen/Geometry>
+#include <arc_utilities/arc_helpers.hpp>
+#include <arc_utilities/eigen_helpers.hpp>
+
+#ifndef IIWA_14_FK_FAST_HPP
+#define IIWA_14_FK_FAST_HPP
+
+namespace IIWA_14_FK_FAST
+{
+    const size_t IIWA_14_NUM_ACTIVE_JOINTS = 7;
+    const size_t IIWA_14_NUM_LINKS = 8;
+
+    const std::string IIWA_14_ACTIVE_JOINT_1_NAME = "iiwa_joint_1";
+    const std::string IIWA_14_ACTIVE_JOINT_2_NAME = "iiwa_joint_2";
+    const std::string IIWA_14_ACTIVE_JOINT_3_NAME = "iiwa_joint_3";
+    const std::string IIWA_14_ACTIVE_JOINT_4_NAME = "iiwa_joint_4";
+    const std::string IIWA_14_ACTIVE_JOINT_5_NAME = "iiwa_joint_5";
+    const std::string IIWA_14_ACTIVE_JOINT_6_NAME = "iiwa_joint_6";
+    const std::string IIWA_14_ACTIVE_JOINT_7_NAME = "iiwa_joint_7";
+
+    const std::string IIWA_14_LINK_1_NAME = "iiwa_link_0";
+    const std::string IIWA_14_LINK_2_NAME = "iiwa_link_1";
+    const std::string IIWA_14_LINK_3_NAME = "iiwa_link_2";
+    const std::string IIWA_14_LINK_4_NAME = "iiwa_link_3";
+    const std::string IIWA_14_LINK_5_NAME = "iiwa_link_4";
+    const std::string IIWA_14_LINK_6_NAME = "iiwa_link_5";
+    const std::string IIWA_14_LINK_7_NAME = "iiwa_link_6";
+    const std::string IIWA_14_LINK_8_NAME = "iiwa_link_7";
+
+    inline Eigen::Isometry3d Get_link_0_joint_1_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.1575);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, 0.0);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_1_joint_2_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.2025);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, M_PI);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_2_joint_3_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.2045, 0.0);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, M_PI);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_3_joint_4_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.2155);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, 0.0);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_4_joint_5_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.1845, 0.0);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(-M_PI_2, M_PI, 0.0);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_5_joint_6_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.2155);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, 0.0);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_6_joint_7_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.081, 0.0);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(-M_PI_2, M_PI, 0.0);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline EigenHelpers::VectorIsometry3d GetLinkTransforms(const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
+    {
+        assert(configuration.size() == IIWA_14_NUM_ACTIVE_JOINTS);
+        EigenHelpers::VectorIsometry3d link_transforms(IIWA_14_NUM_LINKS);
+        link_transforms[0] = base_transform;
+        link_transforms[1] = link_transforms[0] * Get_link_0_joint_1_LinkJointTransform(configuration[0]);
+        link_transforms[2] = link_transforms[1] * Get_link_1_joint_2_LinkJointTransform(configuration[1]);
+        link_transforms[3] = link_transforms[2] * Get_link_2_joint_3_LinkJointTransform(configuration[2]);
+        link_transforms[4] = link_transforms[3] * Get_link_3_joint_4_LinkJointTransform(configuration[3]);
+        link_transforms[5] = link_transforms[4] * Get_link_4_joint_5_LinkJointTransform(configuration[4]);
+        link_transforms[6] = link_transforms[5] * Get_link_5_joint_6_LinkJointTransform(configuration[5]);
+        link_transforms[7] = link_transforms[6] * Get_link_6_joint_7_LinkJointTransform(configuration[6]);
+        return link_transforms;
+    }
+
+    inline EigenHelpers::VectorIsometry3d GetLinkTransforms(const std::map<std::string, double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
+    {
+        std::vector<double> configuration_vector(IIWA_14_NUM_ACTIVE_JOINTS);
+        configuration_vector[0] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_1_NAME, 0.0);
+        configuration_vector[1] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_2_NAME, 0.0);
+        configuration_vector[2] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_3_NAME, 0.0);
+        configuration_vector[3] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_4_NAME, 0.0);
+        configuration_vector[4] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_5_NAME, 0.0);
+        configuration_vector[5] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_6_NAME, 0.0);
+        configuration_vector[6] = arc_helpers::RetrieveOrDefault(configuration, IIWA_14_ACTIVE_JOINT_7_NAME, 0.0);
+        return GetLinkTransforms(configuration_vector, base_transform);
+    }
+
+    inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
+    {
+        const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
+        EigenHelpers::MapStringIsometry3d link_transforms_map;
+        link_transforms_map[IIWA_14_LINK_1_NAME] = link_transforms[0];
+        link_transforms_map[IIWA_14_LINK_2_NAME] = link_transforms[1];
+        link_transforms_map[IIWA_14_LINK_3_NAME] = link_transforms[2];
+        link_transforms_map[IIWA_14_LINK_4_NAME] = link_transforms[3];
+        link_transforms_map[IIWA_14_LINK_5_NAME] = link_transforms[4];
+        link_transforms_map[IIWA_14_LINK_6_NAME] = link_transforms[5];
+        link_transforms_map[IIWA_14_LINK_7_NAME] = link_transforms[6];
+        link_transforms_map[IIWA_14_LINK_8_NAME] = link_transforms[7];
+        return link_transforms_map;
+    }
+
+    inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(const std::map<std::string, double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
+    {
+        const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
+        EigenHelpers::MapStringIsometry3d link_transforms_map;
+        link_transforms_map[IIWA_14_LINK_1_NAME] = link_transforms[0];
+        link_transforms_map[IIWA_14_LINK_2_NAME] = link_transforms[1];
+        link_transforms_map[IIWA_14_LINK_3_NAME] = link_transforms[2];
+        link_transforms_map[IIWA_14_LINK_4_NAME] = link_transforms[3];
+        link_transforms_map[IIWA_14_LINK_5_NAME] = link_transforms[4];
+        link_transforms_map[IIWA_14_LINK_6_NAME] = link_transforms[5];
+        link_transforms_map[IIWA_14_LINK_7_NAME] = link_transforms[6];
+        link_transforms_map[IIWA_14_LINK_8_NAME] = link_transforms[7];
+        return link_transforms_map;
+    }
+}
+
+#endif // IIWA_14_FK_FAST_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/iiwa_7_fk_fast.hpp

@@ -0,0 +1,171 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <vector>
+#include <map>
+#include <Eigen/Geometry>
+#include <arc_utilities/arc_helpers.hpp>
+#include <arc_utilities/eigen_helpers.hpp>
+
+#ifndef IIWA_7_FK_FAST_HPP
+#define IIWA_7_FK_FAST_HPP
+
+namespace IIWA_7_FK_FAST
+{
+    const size_t IIWA_7_NUM_ACTIVE_JOINTS = 7;
+    const size_t IIWA_7_NUM_LINKS = 8;
+
+    const std::string IIWA_7_ACTIVE_JOINT_1_NAME = "iiwa_joint_1";
+    const std::string IIWA_7_ACTIVE_JOINT_2_NAME = "iiwa_joint_2";
+    const std::string IIWA_7_ACTIVE_JOINT_3_NAME = "iiwa_joint_3";
+    const std::string IIWA_7_ACTIVE_JOINT_4_NAME = "iiwa_joint_4";
+    const std::string IIWA_7_ACTIVE_JOINT_5_NAME = "iiwa_joint_5";
+    const std::string IIWA_7_ACTIVE_JOINT_6_NAME = "iiwa_joint_6";
+    const std::string IIWA_7_ACTIVE_JOINT_7_NAME = "iiwa_joint_7";
+
+    const std::string IIWA_7_LINK_1_NAME = "iiwa_link_0";
+    const std::string IIWA_7_LINK_2_NAME = "iiwa_link_1";
+    const std::string IIWA_7_LINK_3_NAME = "iiwa_link_2";
+    const std::string IIWA_7_LINK_4_NAME = "iiwa_link_3";
+    const std::string IIWA_7_LINK_5_NAME = "iiwa_link_4";
+    const std::string IIWA_7_LINK_6_NAME = "iiwa_link_5";
+    const std::string IIWA_7_LINK_7_NAME = "iiwa_link_6";
+    const std::string IIWA_7_LINK_8_NAME = "iiwa_link_7";
+
+    inline Eigen::Isometry3d Get_link_0_joint_1_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.15);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(0.0, 0.0, 0.0);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_1_joint_2_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.19);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, M_PI);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_2_joint_3_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.21, 0.0);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, M_PI);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_3_joint_4_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.0, 0.19);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, 0.0);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_4_joint_5_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.21, 0.0);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(-M_PI_2, M_PI, 0.0);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_5_joint_6_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.06070, 0.19);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(M_PI_2, 0.0, 0.0);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline Eigen::Isometry3d Get_link_6_joint_7_LinkJointTransform(const double joint_val)
+    {
+        const Eigen::Translation3d pre_joint_translation(0.0, 0.081, 0.06070);
+        const Eigen::Quaterniond pre_joint_rotation = EigenHelpers::QuaternionFromUrdfRPY(-M_PI_2, M_PI, 0.0);
+        const Eigen::Isometry3d pre_joint_transform = pre_joint_translation * pre_joint_rotation;
+        const Eigen::Translation3d joint_translation(0.0, 0.0, 0.0);
+        const Eigen::Quaterniond joint_rotation(Eigen::AngleAxisd(joint_val, Eigen::Vector3d::UnitZ()));
+        const Eigen::Isometry3d joint_transform = joint_translation * joint_rotation;
+        return (pre_joint_transform * joint_transform);
+    }
+
+    inline EigenHelpers::VectorIsometry3d GetLinkTransforms(const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
+    {
+        assert(configuration.size() == IIWA_7_NUM_ACTIVE_JOINTS);
+        EigenHelpers::VectorIsometry3d link_transforms(IIWA_7_NUM_LINKS);
+        link_transforms[0] = base_transform;
+        link_transforms[1] = link_transforms[0] * Get_link_0_joint_1_LinkJointTransform(configuration[0]);
+        link_transforms[2] = link_transforms[1] * Get_link_1_joint_2_LinkJointTransform(configuration[1]);
+        link_transforms[3] = link_transforms[2] * Get_link_2_joint_3_LinkJointTransform(configuration[2]);
+        link_transforms[4] = link_transforms[3] * Get_link_3_joint_4_LinkJointTransform(configuration[3]);
+        link_transforms[5] = link_transforms[4] * Get_link_4_joint_5_LinkJointTransform(configuration[4]);
+        link_transforms[6] = link_transforms[5] * Get_link_5_joint_6_LinkJointTransform(configuration[5]);
+        link_transforms[7] = link_transforms[6] * Get_link_6_joint_7_LinkJointTransform(configuration[6]);
+        return link_transforms;
+    }
+
+    inline EigenHelpers::VectorIsometry3d GetLinkTransforms(const std::map<std::string, double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
+    {
+        std::vector<double> configuration_vector(IIWA_7_NUM_ACTIVE_JOINTS);
+        configuration_vector[0] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_1_NAME, 0.0);
+        configuration_vector[1] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_2_NAME, 0.0);
+        configuration_vector[2] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_3_NAME, 0.0);
+        configuration_vector[3] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_4_NAME, 0.0);
+        configuration_vector[4] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_5_NAME, 0.0);
+        configuration_vector[5] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_6_NAME, 0.0);
+        configuration_vector[6] = arc_helpers::RetrieveOrDefault(configuration, IIWA_7_ACTIVE_JOINT_7_NAME, 0.0);
+        return GetLinkTransforms(configuration_vector, base_transform);
+    }
+
+    inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(const std::vector<double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
+    {
+        const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
+        EigenHelpers::MapStringIsometry3d link_transforms_map;
+        link_transforms_map[IIWA_7_LINK_1_NAME] = link_transforms[0];
+        link_transforms_map[IIWA_7_LINK_2_NAME] = link_transforms[1];
+        link_transforms_map[IIWA_7_LINK_3_NAME] = link_transforms[2];
+        link_transforms_map[IIWA_7_LINK_4_NAME] = link_transforms[3];
+        link_transforms_map[IIWA_7_LINK_5_NAME] = link_transforms[4];
+        link_transforms_map[IIWA_7_LINK_6_NAME] = link_transforms[5];
+        link_transforms_map[IIWA_7_LINK_7_NAME] = link_transforms[6];
+        link_transforms_map[IIWA_7_LINK_8_NAME] = link_transforms[7];
+        return link_transforms_map;
+    }
+
+    inline EigenHelpers::MapStringIsometry3d GetLinkTransformsMap(const std::map<std::string, double>& configuration, const Eigen::Isometry3d& base_transform=Eigen::Isometry3d::Identity())
+    {
+        const EigenHelpers::VectorIsometry3d link_transforms = GetLinkTransforms(configuration, base_transform);
+        EigenHelpers::MapStringIsometry3d link_transforms_map;
+        link_transforms_map[IIWA_7_LINK_1_NAME] = link_transforms[0];
+        link_transforms_map[IIWA_7_LINK_2_NAME] = link_transforms[1];
+        link_transforms_map[IIWA_7_LINK_3_NAME] = link_transforms[2];
+        link_transforms_map[IIWA_7_LINK_4_NAME] = link_transforms[3];
+        link_transforms_map[IIWA_7_LINK_5_NAME] = link_transforms[4];
+        link_transforms_map[IIWA_7_LINK_6_NAME] = link_transforms[5];
+        link_transforms_map[IIWA_7_LINK_7_NAME] = link_transforms[6];
+        link_transforms_map[IIWA_7_LINK_8_NAME] = link_transforms[7];
+        return link_transforms_map;
+    }
+}
+
+#endif // IIWA_7_FK_FAST_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/log.hpp

@@ -0,0 +1,151 @@
+#include <fstream>
+#include <iostream>
+#include <iomanip>
+#include <string>
+#include <stdexcept>
+#include <boost/filesystem.hpp>
+
+#ifndef LOG_HPP
+#define LOG_HPP
+
+#define LOG(log, message)                   \
+    (log).logMessage(                       \
+        static_cast<std::ostringstream&>(   \
+            std::ostringstream().flush()    \
+            << std::setprecision(12)        \
+            << (message)                    \
+       ).str()                              \
+   )
+
+#define LOG_STREAM(log, message)            \
+    (log).logMessage(                       \
+        static_cast<std::ostringstream&>(   \
+            std::ostringstream().flush()    \
+            << std::setprecision(12)        \
+            << message                      \
+       ).str()                              \
+   )
+
+#define LOG_COND(log, cond, message)        \
+    if ((cond)) LOG(log, message)
+
+
+#define LOG_COND_STREAM(log, cond, message) \
+    if ((cond)) LOG_STREAM(log, message)
+
+// TODO: confirm that I havn't made any mistakes in this file
+namespace Log
+{
+    class Log
+    {
+        public:
+            Log(const std::string& filename, bool add_header = true)
+                : filename_(filename)
+            {
+                // If it hasn't been opened, assume that it is because the
+                // directory doesn't exist.
+                boost::filesystem::path p(filename_);
+                boost::filesystem::path dir = p.parent_path();
+                if (!boost::filesystem::is_directory(dir))
+                {
+                    std::cerr << "\x1b[33;1m" << dir << " does not exist! Creating ... ";
+
+                    // NOTE: create_directories should be able to return true in this case
+                    // however due to a bug related to a trailing '/' this is not currently
+                    // the case in my version of boost
+                    // https://svn.boost.org/trac/boost/ticket/7258
+                    boost::filesystem::create_directories(dir);
+                    if (boost::filesystem::is_directory(dir))
+        //            if (boost::filesystem::create_directories(p))
+                    {
+                        std::cerr << "Succeeded!\x1b[37m\n";
+                    }
+                    else
+                    {
+                        std::cerr << "\x1b[31;1mFailed!\x1b[37m\n";
+                    }
+                }
+
+                out_file_.open(filename, std::ios_base::out | std::ios_base::trunc);
+                // check if we've succesfully opened the file
+                if (!out_file_.is_open())
+                {
+                    std::cerr << "\x1b[31;1m Unable to create folder/file to log to: " << filename << "\x1b[37m \n";
+                    throw std::invalid_argument("filename must be write-openable");
+                }
+
+                if (add_header)
+                {
+                    time_t rawtime;
+                    tm * timeinfo;
+                    char buffer[80];
+
+                    time(&rawtime);
+                    timeinfo = localtime(&rawtime);
+
+                    strftime(buffer, 80, "%Y-%m-%d %H:%M:%S", timeinfo);
+
+                    out_file_ << buffer << std::endl;
+                }
+            }
+
+            /** Copy constructor */
+            Log(const Log& other)
+                : filename_(other.filename_)
+                , out_file_(filename_, std::ios_base::out | std::ios_base::app)
+            {
+            }
+
+            /** Move constructor */
+            Log(Log&& other)
+                : filename_(other.filename_)
+                , out_file_(filename_, std::ios_base::out | std::ios_base::app)
+            {
+                other.out_file_.close();
+            }
+
+            /** Destructor */
+            ~Log()
+            {
+                if (out_file_.is_open())
+                {
+                    out_file_.close();
+                }
+            }
+
+            /** Copy assignment operator */
+            Log& operator= (const Log& other)
+            {
+                Log tmp(other); // re-use copy-constructor
+                *this = std::move(tmp); // re-use move-assignment
+                return *this;
+            }
+
+            /** Move assignment operator */
+            Log& operator= (Log&& other)
+            {
+                std::swap(filename_, other.filename_);
+                other.out_file_.close();
+
+                if (out_file_.is_open())
+                {
+                    out_file_.close();
+                }
+
+                out_file_.open(filename_, std::ios_base::out | std::ios_base::app);
+
+                return *this;
+            }
+
+            void logMessage(const std::string& message)
+            {
+                out_file_ << message << std::endl;
+            }
+
+        private:
+            std::string filename_;
+            std::ofstream out_file_;
+    };
+}
+
+#endif // LOG_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/maybe.hpp

@@ -0,0 +1,81 @@
+#include <assert.h>
+
+#ifndef MAYBE_HPP
+#define MAYBE_HPP
+
+/* Simple implementation of a Maybe/Option type (or monad, if you're into that sort of thing).
+ * Allows the safe passing of a value (or lack of a value!) without the need for null values.
+ *
+ * For example, Maybe<double> stores a double value OR the lack of such value.
+ *
+ * To use this class, create a Maybe<Type> via Maybe<Type> example_maybe(Type value)
+ * To assign a new value, use example_maybe = Type value
+ * To check if a Maybe contains a value, use example_maybe.Valid()
+ * To get the stored value, use example_maybe.Get() or example_maybe.GetImmutable()
+ * Note that both getter functions assert that a valid value is containted!
+ *
+ */
+namespace Maybe
+{
+    template <typename T>
+    class Maybe
+    {
+    protected:
+
+        bool maybe_;
+        T value_;
+
+    public:
+
+        Maybe() : maybe_(false) {}
+
+        Maybe(const T& value) : maybe_(true), value_(value) {}
+
+        Maybe(T&& value) : maybe_(true), value_(value) {}
+
+        bool Valid() const
+        {
+            return maybe_;
+        }
+
+        T& Get()
+        {
+            assert(maybe_);
+            return value_;
+        }
+
+        const T& GetImmutable() const
+        {
+            assert(maybe_);
+            return value_;
+        }
+
+        void Set(const T& value)
+        {
+            maybe_ = true;
+            value_ = value;
+        }
+
+        void Set(T&& value)
+        {
+            maybe_ = true;
+            value_ = value;
+        }
+
+        Maybe& operator=(const T& value)
+        {
+            maybe_ = true;
+            value_ = value;
+            return *this;
+        }
+
+        Maybe& operator=(T&& value)
+        {
+            maybe_ = true;
+            value_ = value;
+            return *this;
+        }
+    };
+}
+
+#endif // MAYBE_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/pretty_print.hpp

@@ -0,0 +1,781 @@
+#include <iostream>
+#include <iomanip>
+#include <array>
+#include <vector>
+#include <deque>
+#include <forward_list>
+#include <list>
+#include <map>
+#include <set>
+#include <unordered_map>
+#include <unordered_set>
+#include <string>
+#include <Eigen/Geometry>
+#include <arc_utilities/arc_helpers.hpp>
+
+#ifndef PRETTY_PRINT_HPP
+#define PRETTY_PRINT_HPP
+
+// Handy functions for printing vectors and pairs
+namespace PrettyPrint
+{
+    // Base template function for printing types
+    template <typename T>
+    inline std::string PrettyPrint(const T& toprint, const bool add_delimiters=false, const std::string& separator=", ")
+    {
+        UNUSED(add_delimiters);
+        UNUSED(separator);
+        std::ostringstream strm;
+        strm << toprint;
+        return strm.str();
+    }
+
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////
+    /////                                       PROTOTYPES ONLY                                         /////
+    ///// Specializations for specific types - if you want a specialization for a new type, add it here /////
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+    template<>
+    inline std::string PrettyPrint(const bool& bool_to_print, const bool add_delimiters, const std::string& separator);
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::Vector2d& vector_to_print, const bool add_delimiters, const std::string& separator);
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::Vector3d& vector_to_print, const bool add_delimiters, const std::string& separator);
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::Vector4d& vector_to_print, const bool add_delimiters, const std::string& separator);
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::VectorXd& vector_to_print, const bool add_delimiters, const std::string& separator);
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::MatrixXd& matrix_to_print, const bool add_delimiters, const std::string& separator);
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::Quaterniond& quaternion_to_print, const bool add_delimiters, const std::string& separator);
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::Isometry3d& transform_to_print, const bool add_delimiters, const std::string& separator);
+
+    template <typename A, typename B>
+    inline std::string PrettyPrint(const std::pair<A, B>& pairtoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    template <typename T, size_t N>
+    inline std::string PrettyPrint(const std::array<T, N>& arraytoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    template <typename T, typename Allocator=std::allocator<T>>
+    inline std::string PrettyPrint(const std::vector<T, Allocator>& vectoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    template <typename T, typename Allocator=std::allocator<T>>
+    inline std::string PrettyPrint(const std::list<T, Allocator>& listtoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    template <typename T, typename Allocator=std::allocator<T>>
+    inline std::string PrettyPrint(const std::forward_list<T, Allocator>& listtoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    template <typename T, typename Allocator=std::allocator<T>>
+    inline std::string PrettyPrint(const std::deque<T, Allocator>& dequetoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    template <typename A, typename B, typename Compare=std::less<A>, typename Allocator=std::allocator<std::pair<const A, B>>>
+    inline std::string PrettyPrint(const std::map<A, B, Compare, Allocator>& maptoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    template <typename A, typename B, typename Compare=std::less<A>, typename Allocator=std::allocator<std::pair<const A, B>>>
+    inline std::string PrettyPrint(const std::multimap<A, B, Compare, Allocator>& maptoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    template <typename T, typename Compare=std::less<T>, typename Allocator=std::allocator<T>>
+    inline std::string PrettyPrint(const std::set<T, Compare, Allocator>& settoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    template <typename T, typename Compare=std::less<T>, typename Allocator=std::allocator<T>>
+    inline std::string PrettyPrint(const std::multiset<T, Compare, Allocator>& settoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    template <typename A, typename B, typename Hash=std::hash<A>, typename Predicate=std::equal_to<A>, typename Allocator=std::allocator<std::pair<const A, B>>>
+    inline std::string PrettyPrint(const std::unordered_map<A, B, Hash, Predicate, Allocator>& maptoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    template <typename A, typename B, typename Hash=std::hash<A>, typename Predicate=std::equal_to<A>, typename Allocator=std::allocator<std::pair<const A, B>>>
+    inline std::string PrettyPrint(const std::unordered_multimap<A, B, Hash, Predicate, Allocator>& maptoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    template <typename T, typename Hash=std::hash<T>, typename Predicate=std::equal_to<T>, typename Allocator=std::allocator<T>>
+    inline std::string PrettyPrint(const std::unordered_set<T, Hash, Predicate, Allocator>& settoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    template <typename T, typename Hash=std::hash<T>, typename Predicate=std::equal_to<T>, typename Allocator=std::allocator<T>>
+    inline std::string PrettyPrint(const std::unordered_multiset<T, Hash, Predicate, Allocator>& settoprint, const bool add_delimiters=false, const std::string& separator=", ");
+
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////
+    /////                                   IMPLEMENTATIONS ONLY                                        /////
+    ///// Specializations for specific types - if you want a specialization for a new type, add it here /////
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+    template<>
+    inline std::string PrettyPrint(const bool& bool_to_print, const bool add_delimiters, const std::string& separator)
+    {
+        UNUSED(add_delimiters);
+        UNUSED(separator);
+        if (bool_to_print)
+        {
+            return "true";
+        }
+        else
+        {
+            return "false";
+        }
+    }
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::Vector2d& vector_to_print, const bool add_delimiters, const std::string& separator)
+    {
+        UNUSED(separator);
+        std::ostringstream strm;
+        strm << std::setprecision(12);
+        if (add_delimiters)
+        {
+            strm << "Vector2d: <x: " << vector_to_print(0) << " y: " << vector_to_print(1) << ">";
+        }
+        else
+        {
+            strm << vector_to_print(0) << ", " << vector_to_print(1);
+        }
+        return strm.str();
+    }
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::Vector3d& vector_to_print, const bool add_delimiters, const std::string& separator)
+    {
+        UNUSED(separator);
+        std::ostringstream strm;
+        strm << std::setprecision(12);
+        if (add_delimiters)
+        {
+            strm << "Vector3d: <x: " << vector_to_print.x() << " y: " << vector_to_print.y() << " z: " << vector_to_print.z() << ">";
+        }
+        else
+        {
+            strm << vector_to_print.x() << ", " << vector_to_print.y() << ", " << vector_to_print.z();;
+        }
+        return strm.str();
+    }
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::Vector4d& vector_to_print, const bool add_delimiters, const std::string& separator)
+    {
+        UNUSED(separator);
+        std::ostringstream strm;
+        strm << std::setprecision(12);
+        if (add_delimiters)
+        {
+            strm << "Vector4d: <x: " << vector_to_print(0) << " y: " << vector_to_print(1) << " z: " << vector_to_print(2) << " w: " << vector_to_print(3) << ">";
+        }
+        else
+        {
+            strm << vector_to_print(0) << ", " << vector_to_print(1) << ", " << vector_to_print(2) << ", " << vector_to_print(3);
+        }
+        return strm.str();
+    }
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::VectorXd& vector_to_print, const bool add_delimiters, const std::string& separator)
+    {
+        UNUSED(separator);
+        Eigen::IOFormat io_format(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "", "");
+        if (add_delimiters)
+        {
+            io_format = Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "VectorXd: <", ">");;
+        }
+        std::ostringstream strm;
+        strm << std::setprecision(12);
+        strm << vector_to_print.format(io_format);
+        return strm.str();
+    }
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::MatrixXd& matrix_to_print, const bool add_delimiters, const std::string& separator)
+    {
+        UNUSED(separator);
+        Eigen::IOFormat io_format(Eigen::StreamPrecision, 0, ", ", "\n", "", "", "", "");
+        if (add_delimiters)
+        {
+            io_format = Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", "", "", "MatrixXd:\n[", "]");;
+        }
+        std::ostringstream strm;
+        strm << std::setprecision(12);
+        strm << matrix_to_print.format(io_format);
+        return strm.str();
+    }
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::Quaterniond& quaternion_to_print, const bool add_delimiters, const std::string& separator)
+    {
+        UNUSED(separator);
+        std::ostringstream strm;
+        strm << std::setprecision(12);
+        if (add_delimiters)
+        {
+            strm << "Quaterniond <x: " << quaternion_to_print.x() << " y: " << quaternion_to_print.y() << " z: " << quaternion_to_print.z() << " w: " << quaternion_to_print.w() << ">";
+        }
+        else
+        {
+            strm << quaternion_to_print.x() << ", " << quaternion_to_print.y() << ", " << quaternion_to_print.z() << ", " << quaternion_to_print.w();
+        }
+        return strm.str();
+    }
+
+    template<>
+    inline std::string PrettyPrint(const Eigen::Isometry3d& transform_to_print, const bool add_delimiters, const std::string& separator)
+    {
+        UNUSED(separator);
+        std::ostringstream strm;
+        strm << std::setprecision(12);
+        Eigen::Vector3d vector_to_print = transform_to_print.translation();
+        Eigen::Quaterniond quaternion_to_print(transform_to_print.rotation());
+        if (add_delimiters)
+        {
+            strm << "Isometry3d <x: " << vector_to_print.x() << " y: " << vector_to_print.y() << " z: " << vector_to_print.z() << ">, <x: " << quaternion_to_print.x() << " y: " << quaternion_to_print.y() << " z: "  << quaternion_to_print.z() << " w: " << quaternion_to_print.w() << ">";
+        }
+        else
+        {
+            strm << vector_to_print.x() << ", " << vector_to_print.y() << ", " << vector_to_print.z() << " : " << quaternion_to_print.x() << ", " << quaternion_to_print.y() << ", "  << quaternion_to_print.z() << ", " << quaternion_to_print.w();
+        }
+        return strm.str();
+    }
+
+    template <typename A, typename B>
+    inline std::string PrettyPrint(const std::pair<A, B>& pairtoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (add_delimiters)
+        {
+            strm << "<" << PrettyPrint(pairtoprint.first, add_delimiters, separator) << ": " << PrettyPrint(pairtoprint.second, add_delimiters, separator) << ">";
+        }
+        else
+        {
+            strm << PrettyPrint(pairtoprint.first, add_delimiters, separator) << ": " << PrettyPrint(pairtoprint.second, add_delimiters, separator);
+        }
+        return strm.str();
+    }
+
+    template <typename T, size_t N>
+    inline std::string PrettyPrint(const std::array<T, N>& arraytoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (arraytoprint.size() > 0)
+        {
+            if (add_delimiters)
+            {
+                strm << "[" << PrettyPrint(arraytoprint[0], add_delimiters, separator);
+                for (size_t idx = 1; idx < arraytoprint.size(); idx++)
+                {
+                    strm << separator << PrettyPrint(arraytoprint[idx], add_delimiters, separator);
+                }
+                strm << "]";
+            }
+            else
+            {
+                strm << PrettyPrint(arraytoprint[0], add_delimiters, separator);
+                for (size_t idx = 1; idx < arraytoprint.size(); idx++)
+                {
+                    strm << separator << PrettyPrint(arraytoprint[idx], add_delimiters, separator);
+                }
+            }
+        }
+        return strm.str();
+    }
+
+    template <typename T, typename Allocator>
+    inline std::string PrettyPrint(const std::vector<T, Allocator>& vectoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (vectoprint.size() > 0)
+        {
+            if (add_delimiters)
+            {
+                strm << "[" << PrettyPrint(vectoprint[0], add_delimiters, separator);
+                for (size_t idx = 1; idx < vectoprint.size(); idx++)
+                {
+                    strm << separator << PrettyPrint(vectoprint[idx], add_delimiters, separator);
+                }
+                strm << "]";
+            }
+            else
+            {
+                strm << PrettyPrint(vectoprint[0], add_delimiters, separator);
+                for (size_t idx = 1; idx < vectoprint.size(); idx++)
+                {
+                    strm << separator << PrettyPrint(vectoprint[idx], add_delimiters, separator);
+                }
+            }
+        }
+        return strm.str();
+    }
+
+    template <typename T, typename Allocator>
+    inline std::string PrettyPrint(const std::list<T, Allocator>& listtoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (listtoprint.size() > 0)
+        {
+            if (add_delimiters)
+            {
+                strm << "[";
+                typename std::list<T, Allocator>::const_iterator itr;
+                for (itr = listtoprint.begin(); itr != listtoprint.end(); ++itr)
+                {
+                    if (itr != listtoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+                strm << "]";
+            }
+            else
+            {
+                typename std::list<T, Allocator>::const_iterator itr;
+                for (itr = listtoprint.begin(); itr != listtoprint.end(); ++itr)
+                {
+                    if (itr != listtoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+            }
+        }
+        return strm.str();
+    }
+
+    template <typename T, typename Allocator>
+    inline std::string PrettyPrint(const std::forward_list<T, Allocator>& listtoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (listtoprint.size() > 0)
+        {
+            if (add_delimiters)
+            {
+                strm << "[";
+                typename std::forward_list<T, Allocator>::const_iterator itr;
+                for (itr = listtoprint.begin(); itr != listtoprint.end(); ++itr)
+                {
+                    if (itr != listtoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+                strm << "]";
+            }
+            else
+            {
+                typename std::forward_list<T, Allocator>::const_iterator itr;
+                for (itr = listtoprint.begin(); itr != listtoprint.end(); ++itr)
+                {
+                    if (itr != listtoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+            }
+        }
+        return strm.str();
+    }
+
+    template <typename T, typename Allocator>
+    inline std::string PrettyPrint(const std::deque<T, Allocator>& dequetoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (dequetoprint.size() > 0)
+        {
+            if (add_delimiters)
+            {
+                strm << "[";
+                typename std::deque<T, Allocator>::const_iterator itr;
+                for (itr = dequetoprint.begin(); itr != dequetoprint.end(); ++itr)
+                {
+                    if (itr != dequetoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+                strm << "]";
+            }
+            else
+            {
+                typename std::deque<T, Allocator>::const_iterator itr;
+                for (itr = dequetoprint.begin(); itr != dequetoprint.end(); ++itr)
+                {
+                    if (itr != dequetoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+            }
+        }
+        return strm.str();
+    }
+
+    template <typename A, typename B, typename Compare, typename Allocator>
+    inline std::string PrettyPrint(const std::map<A, B, Compare, Allocator>& maptoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (maptoprint.size() > 0)
+        {
+            if (add_delimiters)
+            {
+                strm << "{";
+                typename std::map<A, B, Compare, Allocator>::const_iterator itr;
+                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
+                {
+                    std::pair<A, B> cur_pair(itr->first, itr->second);
+                    if (itr != maptoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                }
+                strm << "}";
+            }
+            else
+            {
+                typename std::map<A, B, Compare, Allocator>::const_iterator itr;
+                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
+                {
+                    std::pair<A, B> cur_pair(itr->first, itr->second);
+                    if (itr != maptoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                }
+            }
+        }
+        return strm.str();
+    }
+
+    template <typename A, typename B, typename Compare, typename Allocator>
+    inline std::string PrettyPrint(const std::multimap<A, B, Compare, Allocator>& maptoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (maptoprint.size() > 0)
+        {
+            if (add_delimiters)
+            {
+                strm << "{";
+                typename std::multimap<A, B, Compare, Allocator>::const_iterator itr;
+                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
+                {
+                    std::pair<A, B> cur_pair(itr->first, itr->second);
+                    if (itr != maptoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                }
+                strm << "}";
+            }
+            else
+            {
+                typename std::multimap<A, B, Compare, Allocator>::const_iterator itr;
+                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
+                {
+                    std::pair<A, B> cur_pair(itr->first, itr->second);
+                    if (itr != maptoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                }
+            }
+        }
+        return strm.str();
+    }
+
+    template <typename T, typename Compare, typename Allocator>
+    inline std::string PrettyPrint(const std::set<T, Compare, Allocator>& settoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (settoprint.size() > 0)
+        {
+            if (add_delimiters)
+            {
+                strm << "(";
+                typename std::set<T, Compare, Allocator>::const_iterator itr;
+                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
+                {
+                    if (itr != settoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+                strm << ")";
+            }
+            else
+            {
+                typename std::set<T, Compare, Allocator>::const_iterator itr;
+                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
+                {
+                    if (itr != settoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+            }
+        }
+        return strm.str();
+    }
+
+    template <typename T, typename Compare, typename Allocator>
+    inline std::string PrettyPrint(const std::multiset<T, Compare, Allocator>& settoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (settoprint.size() > 0)
+        {
+            if (add_delimiters)
+            {
+                strm << "(";
+                typename std::multiset<T, Compare, Allocator>::const_iterator itr;
+                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
+                {
+                    if (itr != settoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+                strm << ")";
+            }
+            else
+            {
+                typename std::multiset<T, Compare, Allocator>::const_iterator itr;
+                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
+                {
+                    if (itr != settoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+            }
+        }
+        return strm.str();
+    }
+
+    template <typename A, typename B, typename Hash, typename Predicate, typename Allocator>
+    inline std::string PrettyPrint(const std::unordered_map<A, B, Hash, Predicate, Allocator>& maptoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (maptoprint.size() > 0)
+        {
+            if (add_delimiters)
+            {
+                strm << "{";
+                typename std::unordered_map<A, B, Hash, Predicate, Allocator>::const_iterator itr;
+                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
+                {
+                    std::pair<A, B> cur_pair(itr->first, itr->second);
+                    if (itr != maptoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                }
+                strm << "}";
+            }
+            else
+            {
+                typename std::unordered_map<A, B, Hash, Predicate, Allocator>::const_iterator itr;
+                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
+                {
+                    std::pair<A, B> cur_pair(itr->first, itr->second);
+                    if (itr != maptoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                }
+            }
+        }
+        return strm.str();
+    }
+
+    template <typename A, typename B, typename Hash, typename Predicate, typename Allocator>
+    inline std::string PrettyPrint(const std::unordered_multimap<A, B, Hash, Predicate, Allocator>& maptoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (maptoprint.size() > 0)
+        {
+            if (add_delimiters)
+            {
+                strm << "{";
+                typename std::unordered_multimap<A, B, Hash, Predicate, Allocator>::const_iterator itr;
+                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
+                {
+                    std::pair<A, B> cur_pair(itr->first, itr->second);
+                    if (itr != maptoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                }
+                strm << "}";
+            }
+            else
+            {
+                typename std::unordered_multimap<A, B, Hash, Predicate, Allocator>::const_iterator itr;
+                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
+                {
+                    std::pair<A, B> cur_pair(itr->first, itr->second);
+                    if (itr != maptoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
+                    }
+                }
+            }
+        }
+        return strm.str();
+    }
+
+    template <typename T, typename Hash, typename Predicate, typename Allocator>
+    inline std::string PrettyPrint(const std::unordered_set<T, Hash, Predicate, Allocator>& settoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (settoprint.size() > 0)
+        {
+            if (add_delimiters)
+            {
+                strm << "(";
+                typename std::unordered_set<T, Hash, Predicate, Allocator>::const_iterator itr;
+                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
+                {
+                    if (itr != settoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+                strm << ")";
+            }
+            else
+            {
+                typename std::unordered_set<T, Hash, Predicate, Allocator>::const_iterator itr;
+                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
+                {
+                    if (itr != settoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+            }
+        }
+        return strm.str();
+    }
+
+    template <typename T, typename Hash, typename Predicate, typename Allocator>
+    inline std::string PrettyPrint(const std::unordered_multiset<T, Hash, Predicate, Allocator>& settoprint, const bool add_delimiters, const std::string& separator)
+    {
+        std::ostringstream strm;
+        if (settoprint.size() > 0)
+        {
+            if (add_delimiters)
+            {
+                strm << "(";
+                typename std::unordered_multiset<T, Hash, Predicate, Allocator>::const_iterator itr;
+                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
+                {
+                    if (itr != settoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+                strm << ")";
+            }
+            else
+            {
+                typename std::unordered_multiset<T, Hash, Predicate, Allocator>::const_iterator itr;
+                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
+                {
+                    if (itr != settoprint.begin())
+                    {
+                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                    else
+                    {
+                        strm << PrettyPrint(*itr, add_delimiters, separator);
+                    }
+                }
+            }
+        }
+        return strm.str();
+    }
+}
+
+#endif // PRETTY_PRINT_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/ros_helpers.hpp

@@ -0,0 +1,120 @@
+#include <ros/ros.h>
+#include <ros/callback_queue.h>
+
+#include "arc_utilities/maybe.hpp"
+
+#ifndef ROS_HELPERS_HPP
+#define ROS_HELPERS_HPP
+
+#define PARAM_NAME_WIDTH (50)
+
+namespace ROSHelpers
+{
+    inline void Spin(const double loop_period)
+    {
+        while (ros::ok())
+        {
+            ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(loop_period));
+        }
+    }
+
+    template <typename T>
+    inline T GetParam(const ros::NodeHandle& nh, const std::string& param_name, const T& default_val)
+    {
+        T param_val;
+        if (nh.getParam(param_name, param_val))
+        {
+            ROS_INFO_STREAM_NAMED("params", "Retrieving " << std::left << std::setw(PARAM_NAME_WIDTH) << param_name << " as " << param_val);
+        }
+        else
+        {
+            param_val = default_val;
+            ROS_WARN_STREAM_NAMED("params", "Defaulting " << std::left << std::setw(PARAM_NAME_WIDTH) << param_name << " to " << param_val);
+        }
+        return param_val;
+    }
+
+    template <typename T>
+    inline T GetParam(const ros::NodeHandle& nh, const std::string& param_name, T&& default_val)
+    {
+        T param_val;
+        if (nh.getParam(param_name, param_val))
+        {
+            ROS_INFO_STREAM_NAMED("params", "Retrieving " << std::left << std::setw(PARAM_NAME_WIDTH) << param_name << " as " << param_val);
+        }
+        else
+        {
+            param_val = default_val;
+            ROS_WARN_STREAM_NAMED("params", "Defaulting " << std::left << std::setw(PARAM_NAME_WIDTH) << param_name << " to " << param_val);
+        }
+        return param_val;
+    }
+
+    template <typename T>
+    inline T GetParamDebugLog(const ros::NodeHandle& nh, const std::string& param_name, const T& default_val)
+    {
+        T param_val;
+        if (nh.getParam(param_name, param_val))
+        {
+            ROS_DEBUG_STREAM_NAMED("params", "Retrieving " << std::left << std::setw(PARAM_NAME_WIDTH) << param_name << " as " << param_val);
+        }
+        else
+        {
+            param_val = default_val;
+            ROS_DEBUG_STREAM_NAMED("params", "Defaulting " << std::left << std::setw(PARAM_NAME_WIDTH) << param_name << " to " << param_val);
+        }
+        return param_val;
+    }
+
+    template <typename T>
+    inline T GetParamDebugLog(const ros::NodeHandle& nh, const std::string& param_name, T&& default_val)
+    {
+        T param_val;
+        if (nh.getParam(param_name, param_val))
+        {
+            ROS_DEBUG_STREAM_NAMED("params", "Retrieving " << std::left << std::setw(PARAM_NAME_WIDTH) << param_name << " as " << param_val);
+        }
+        else
+        {
+            param_val = default_val;
+            ROS_DEBUG_STREAM_NAMED("params", "Defaulting " << std::left << std::setw(PARAM_NAME_WIDTH) << param_name << " to " << param_val);
+        }
+        return param_val;
+    }
+
+    template <typename T>
+    inline Maybe::Maybe<T> GetParamRequired(const ros::NodeHandle& nh, const std::string& param_name, const std::string& calling_fn_name)
+    {
+        ROS_DEBUG_STREAM_NAMED("params", "No default value for " << param_name << ": Value must be on paramter sever");
+        T param_val;
+        if (nh.getParam(param_name, param_val))
+        {
+            ROS_INFO_STREAM_NAMED("params", "Retrieving " << std::left << std::setw(PARAM_NAME_WIDTH) << param_name << " as " << param_val);
+            return Maybe::Maybe<T>(param_val);
+        }
+        else
+        {
+            ROS_FATAL_STREAM_NAMED("params", "Cannot find " << nh.getNamespace() << "/" << param_name << " on parameter server for " << calling_fn_name << ": Value must be on paramter sever");
+            return Maybe::Maybe<T>();
+        }
+    }
+
+    template <typename T>
+    inline Maybe::Maybe<T> GetParamRequiredDebugLog(const ros::NodeHandle& nh, const std::string& param_name, const std::string& calling_fn_name)
+    {
+        ROS_DEBUG_STREAM_NAMED("params", "No default value for " << param_name << ": Value must be on paramter sever");
+        T param_val;
+        if (nh.getParam(param_name, param_val))
+        {
+            ROS_DEBUG_STREAM_NAMED("params", "Retrieving " << std::left << std::setw(PARAM_NAME_WIDTH) << param_name << " as " << param_val);
+            return Maybe::Maybe<T>(param_val);
+        }
+        else
+        {
+            ROS_FATAL_STREAM_NAMED("params", "Cannot find " << nh.getNamespace() << "/" << param_name << " on parameter server for " << calling_fn_name << ": Value must be on paramter sever");
+            return Maybe::Maybe<T>();
+        }
+    }
+}
+
+#endif // ROS_HELPERS_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/serialization.hpp

@@ -0,0 +1,290 @@
+#ifndef SERIALIZATION_HPP
+#define SERIALIZATION_HPP
+
+#include <cstring>
+#include <vector>
+#include <map>
+#include <functional>
+#include <assert.h>
+
+namespace arc_utilities
+{
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////
+    /////                                       PROTOTYPES ONLY                                         /////
+    ///// Specializations for specific types - if you want a specialization for a new type, add it here /////
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+    template<typename T>
+    inline uint64_t SerializeFixedSizePOD(
+            const T& item_to_serialize,
+            std::vector<uint8_t>& buffer);
+
+    template<typename T>
+    inline std::pair<T, uint64_t> DeserializeFixedSizePOD(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current);
+
+    template<typename T, typename Allocator = std::allocator<T>>
+    inline uint64_t SerializeVector(
+            const std::vector<T, Allocator>& vec_to_serialize, std::vector<uint8_t>& buffer,
+            const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& item_serializer);
+
+    template<typename T, typename Allocator = std::allocator<T>>
+    inline std::pair<std::vector<T, Allocator>, uint64_t> DeserializeVector(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current,
+            const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& item_deserializer);
+
+    template<typename Key, typename T, typename Compare = std::less<Key>, typename Allocator = std::allocator<std::pair<const Key, T>>>
+    inline uint64_t SerializeMap(
+            const std::map<Key, T, Compare, Allocator>& map_to_serialize,
+            std::vector<uint8_t>& buffer,
+            const std::function<uint64_t(const Key&, std::vector<uint8_t>&)>& key_serializer,
+            const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& value_serializer);
+
+    template<typename Key, typename T, typename Compare = std::less<Key>, typename Allocator = std::allocator<std::pair<const Key, T>>>
+    inline std::pair<std::map<Key, T, Compare, Allocator>, uint64_t> DeserializeMap(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current,
+            const std::function<std::pair<Key, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& key_deserializer,
+            const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer);
+
+    template<typename First, typename Second>
+    inline uint64_t SerializePair(
+            const std::pair<First, Second>& pair_to_serialize,
+            std::vector<uint8_t>& buffer,
+            const std::function<uint64_t(const First&, std::vector<uint8_t>&)>& first_serializer,
+            const std::function<uint64_t(const Second&, std::vector<uint8_t>&)>& second_serializer);
+
+    template<typename First, typename Second>
+    inline const std::pair<std::pair<First, Second>, uint64_t> DeserializePair(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current,
+            const std::function<std::pair<First, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& first_deserializer,
+            const std::function<std::pair<Second, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& second_deserializer);
+
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////
+    /////                                   IMPLEMENTATIONS ONLY                                        /////
+    ///// Specializations for specific types - if you want a specialization for a new type, add it here /////
+    /////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+    template<typename T>
+    inline uint64_t SerializeFixedSizePOD(
+            const T& item_to_serialize,
+            std::vector<uint8_t>& buffer)
+    {
+        const uint64_t start_buffer_size = buffer.size();
+        // Fixed-size serialization via memcpy
+        std::vector<uint8_t> temp_buffer(sizeof(item_to_serialize), 0x00);
+        std::memcpy(&temp_buffer[0], &item_to_serialize, sizeof(item_to_serialize));
+        // Move to buffer
+        buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
+        // Figure out how many bytes were written
+        const uint64_t end_buffer_size = buffer.size();
+        const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+        return bytes_written;
+    }
+
+    template<typename T>
+    inline std::pair<T, uint64_t> DeserializeFixedSizePOD(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current)
+    {
+        T temp_item;
+        assert(current <= buffer.size());
+        assert((current + sizeof(temp_item)) <= buffer.size());
+        std::memcpy(&temp_item, &buffer[current], sizeof(temp_item));
+        return std::make_pair(temp_item, sizeof(temp_item));
+    }
+
+    template<typename CharType>
+    inline uint64_t SerializeString(
+            const std::basic_string<CharType>& str_to_serialize,
+            std::vector<uint8_t>& buffer)
+    {
+        const uint64_t start_buffer_size = buffer.size();
+        // First, write a uint64_t size header
+        const uint64_t size = (uint64_t)str_to_serialize.size();
+        SerializeFixedSizePOD<uint64_t>(size, buffer);
+        // Serialize the contained items
+        for (size_t idx = 0; idx < size; idx++)
+        {
+            const CharType& current = str_to_serialize[idx];
+            SerializeFixedSizePOD<CharType>(current, buffer);
+        }
+        // Figure out how many bytes were written
+        const uint64_t end_buffer_size = buffer.size();
+        const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+        return bytes_written;
+    }
+
+    template<typename CharType>
+    inline std::pair<std::basic_string<CharType>, uint64_t> DeserializeString(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current)
+    {
+        // First, try to load the header
+        assert(current < buffer.size());
+        uint64_t current_position = current;
+        // Load the header
+        const std::pair<uint64_t, uint64_t> deserialized_size = DeserializeFixedSizePOD<uint64_t>(buffer, current_position);
+        const uint64_t size = deserialized_size.first;
+        current_position += deserialized_size.second;
+        // Deserialize the items
+        std::basic_string<CharType> deserialized;
+        deserialized.reserve(size);
+        for (uint64_t idx = 0; idx < size; idx++)
+        {
+            const std::pair<CharType, uint64_t> deserialized_char = DeserializeFixedSizePOD<CharType>(buffer, current_position);
+            deserialized.push_back(deserialized_char.first);
+            current_position += deserialized_char.second;
+        }
+        deserialized.shrink_to_fit();
+        // Figure out how many bytes were read
+        const uint64_t bytes_read = current_position - current;
+        return std::make_pair(deserialized, bytes_read);
+    }
+
+    template<typename T, typename Allocator>
+    inline uint64_t SerializeVector(
+            const std::vector<T, Allocator>& vec_to_serialize,
+            std::vector<uint8_t>& buffer,
+            const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& item_serializer)
+    {
+        const uint64_t start_buffer_size = buffer.size();
+        // First, write a uint64_t size header
+        const uint64_t size = (uint64_t)vec_to_serialize.size();
+        SerializeFixedSizePOD<uint64_t>(size, buffer);
+        // Serialize the contained items
+        for (size_t idx = 0; idx < size; idx++)
+        {
+            const T& current = vec_to_serialize[idx];
+            item_serializer(current, buffer);
+        }
+        // Figure out how many bytes were written
+        const uint64_t end_buffer_size = buffer.size();
+        const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+        return bytes_written;
+    }
+
+    template<typename T, typename Allocator>
+    inline std::pair<std::vector<T, Allocator>, uint64_t> DeserializeVector(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current,
+            const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& item_deserializer)
+    {
+        // First, try to load the header
+        assert(current < buffer.size());
+        uint64_t current_position = current;
+        // Load the header
+        const std::pair<uint64_t, uint64_t> deserialized_size = DeserializeFixedSizePOD<uint64_t>(buffer, current_position);
+        const uint64_t size = deserialized_size.first;
+        current_position += deserialized_size.second;
+        // Deserialize the items
+        std::vector<T, Allocator> deserialized;
+        deserialized.reserve(size);
+        for (uint64_t idx = 0; idx < size; idx++)
+        {
+            const std::pair<T, uint64_t> deserialized_item = item_deserializer(buffer, current_position);
+            deserialized.push_back(deserialized_item.first);
+            current_position += deserialized_item.second;
+        }
+        deserialized.shrink_to_fit();
+        // Figure out how many bytes were read
+        const uint64_t bytes_read = current_position - current;
+        return std::make_pair(deserialized, bytes_read);
+    }
+
+    template<typename Key, typename T, typename Compare, typename Allocator>
+    inline uint64_t SerializeMap(
+            const std::map<Key, T, Compare, Allocator>& map_to_serialize,
+            std::vector<uint8_t>& buffer,
+            const std::function<uint64_t(const Key&, std::vector<uint8_t>&)>& key_serializer,
+            const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& value_serializer)
+    {
+        const uint64_t start_buffer_size = buffer.size();
+        // First, write a uint64_t size header
+        const uint64_t size = (uint64_t)map_to_serialize.size();
+        SerializeFixedSizePOD<uint64_t>(size, buffer);
+        // Serialize the contained items
+        typename std::map<Key, T, Compare, Allocator>::const_iterator itr;
+        for (itr = map_to_serialize.begin(); itr != map_to_serialize.end(); ++itr)
+        {
+            SerializePair<Key, T>(*itr, buffer, key_serializer, value_serializer);
+        }
+        // Figure out how many bytes were written
+        const uint64_t end_buffer_size = buffer.size();
+        const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+        return bytes_written;
+    }
+
+    template<typename Key, typename T, typename Compare, typename Allocator>
+    inline std::pair<std::map<Key, T, Compare, Allocator>, uint64_t> DeserializeMap(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current,
+            const std::function<std::pair<Key, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& key_deserializer,
+            const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
+    {
+        // First, try to load the header
+        assert(current < buffer.size());
+        uint64_t current_position = current;
+        // Load the header
+        const std::pair<uint64_t, uint64_t> deserialized_size = DeserializeFixedSizePOD<uint64_t>(buffer, current_position);
+        const uint64_t size = deserialized_size.first;
+        current_position += deserialized_size.second;
+        // Deserialize the items
+        std::map<Key, T, Compare, Allocator> deserialized;
+        for (uint64_t idx = 0; idx < size; idx++)
+        {
+            std::pair<std::pair<Key, T>, uint64_t> deserialized_pair = DeserializePair(buffer, current_position, key_deserializer, value_deserializer);
+            deserialized.insert(deserialized_pair.first);
+            current_position += deserialized_pair.second;
+        }
+        // Figure out how many bytes were read
+        const uint64_t bytes_read = current_position - current;
+        return std::make_pair(deserialized, bytes_read);
+    }
+
+    template<typename First, typename Second>
+    inline uint64_t SerializePair(
+            const std::pair<First, Second>& pair_to_serialize,
+            std::vector<uint8_t>& buffer,
+            const std::function<uint64_t(const First&, std::vector<uint8_t>&)>& first_serializer,
+            const std::function<uint64_t(const Second&, std::vector<uint8_t>&)>& second_serializer)
+    {
+        const uint64_t start_buffer_size = buffer.size();
+        uint64_t running_total = 0u;
+        // Write each element of the pair into the buffer
+        running_total += first_serializer(pair_to_serialize.first, buffer);
+        running_total += second_serializer(pair_to_serialize.second, buffer);
+        // Figure out how many bytes were written
+        const uint64_t end_buffer_size = buffer.size();
+        const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+        assert(bytes_written == running_total);
+        return bytes_written;
+    }
+
+    template<typename First, typename Second>
+    inline const std::pair<std::pair<First, Second>, uint64_t> DeserializePair(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current,
+            const std::function<std::pair<First, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& first_deserializer,
+            const std::function<std::pair<Second, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& second_deserializer)
+    {
+        assert(current < buffer.size());
+        // Deserialize each item in the pair individually
+        uint64_t current_position = current;
+        const std::pair<First, uint64_t> deserialized_first = first_deserializer(buffer, current_position);
+        current_position += deserialized_first.second;
+        const std::pair<Second, uint64_t> deserialized_second = second_deserializer(buffer, current_position);
+        current_position += deserialized_second.second;
+        // Build the resulting pair
+        // TODO: Why can't I used make_pair here?
+        const std::pair<First, Second> deserialized(deserialized_first.first, deserialized_second.first);
+        // Figure out how many bytes were read
+        const uint64_t bytes_read = current_position - current;
+        return std::make_pair(deserialized, bytes_read);
+    }
+}
+
+#endif // SERIALIZATION_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/serialization_eigen.hpp

@@ -0,0 +1,179 @@
+#ifndef SERIALIZATION_EIGEN_HPP
+#define SERIALIZATION_EIGEN_HPP
+
+#include "arc_utilities/serialization.hpp"
+#include "arc_utilities/eigen_helpers.hpp"
+
+namespace arc_utilities
+{
+    ////////////////////////////////////////////////////////////////////////////
+    // Serialization/Deserialization functions
+    ////////////////////////////////////////////////////////////////////////////
+
+    // Prototypes for serialization/deserialization functions
+    template<typename Container>
+    inline uint64_t SerializedSizeEigenType(const Container& value);
+
+    // For fixed-size containers only (others have a uint64_t size header first)
+    template<typename Container>
+    inline uint64_t SerializedSizeEigenType(void);
+
+    template<typename Container>
+    inline uint64_t SerializeEigenType(const Container& value, std::vector<uint8_t>& buffer);
+
+    template<typename Container>
+    inline std::pair<Container, uint64_t> DeserializeEigenType(const std::vector<uint8_t>& buffer, const uint64_t current);
+
+    // Concrete implementations
+    template<>
+    inline uint64_t SerializedSizeEigenType(const Eigen::VectorXd& value)
+    {
+        (void)(value);
+        return (uint64_t)((1 * sizeof(uint64_t)) + ((size_t)value.size() * sizeof(double))); // Space for a uint64_t size header and the data
+    }
+
+    template<>
+    inline uint64_t SerializeEigenType(const Eigen::VectorXd& value, std::vector<uint8_t>& buffer)
+    {
+        // Takes a state to serialize and a buffer to serialize into
+        // Return number of bytes written to buffer
+        const uint64_t serialized_size = SerializedSizeEigenType(value);
+        std::vector<uint8_t> temp_buffer(serialized_size, 0x00);
+        // Make the header
+        const uint64_t size_header = (uint64_t)value.size();
+        memcpy(&temp_buffer.front(), & size_header, sizeof(size_header));
+        // Copy the data
+        memcpy(&(temp_buffer[sizeof(size_header)]), value.data(), (serialized_size - sizeof(size_header)));
+        buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
+        return serialized_size;
+    }
+
+    template<>
+    inline std::pair<Eigen::VectorXd, uint64_t> DeserializeEigenType<Eigen::VectorXd>(const std::vector<uint8_t>& buffer, const uint64_t current)
+    {
+        assert(current < buffer.size());
+        assert((current + sizeof(uint64_t)) <= buffer.size());
+        // Takes a buffer to read from and the starting index in the buffer
+        // Return the loaded state and how many bytes we read from the buffer
+        // Load the header
+        uint64_t size_header = 0u;
+        memcpy(&size_header, &buffer[current], sizeof(uint64_t));
+        // Check buffer size
+        Eigen::VectorXd temp_value = Eigen::VectorXd::Zero((ssize_t)size_header);
+        const uint64_t serialized_size = SerializedSizeEigenType(temp_value);
+        assert((current + serialized_size) <= buffer.size());
+        // Load from the buffer
+        memcpy(temp_value.data(), &buffer[current + sizeof(size_header)], (serialized_size - sizeof(size_header)));
+        return std::make_pair(temp_value, serialized_size);
+    }
+
+    template<>
+    inline uint64_t SerializedSizeEigenType(const Eigen::Vector3d& value)
+    {
+        (void)(value);
+        return (uint64_t)(3 * sizeof(double));
+    }
+
+    template<>
+    inline uint64_t SerializedSizeEigenType<Eigen::Vector3d>(void)
+    {
+        return (uint64_t)(3 * sizeof(double));
+    }
+
+    template<>
+    inline uint64_t SerializeEigenType(const Eigen::Vector3d& value, std::vector<uint8_t>& buffer)
+    {
+        // Takes a state to serialize and a buffer to serialize into
+        // Return number of bytes written to buffer
+        std::vector<uint8_t> temp_buffer(SerializedSizeEigenType<Eigen::Vector3d>(), 0x00);
+        memcpy(&temp_buffer.front(), value.data(), SerializedSizeEigenType<Eigen::Vector3d>());
+        buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
+        return SerializedSizeEigenType<Eigen::Vector3d>();
+    }
+
+    template<>
+    inline std::pair<Eigen::Vector3d, uint64_t> DeserializeEigenType<Eigen::Vector3d>(const std::vector<uint8_t>& buffer, const uint64_t current)
+    {
+        assert(current < buffer.size());
+        assert((current + SerializedSizeEigenType<Eigen::Vector3d>()) <= buffer.size());
+        // Takes a buffer to read from and the starting index in the buffer
+        // Return the loaded state and how many bytes we read from the buffer
+        Eigen::Vector3d temp_value;
+        memcpy(temp_value.data(), &buffer[current], SerializedSizeEigenType<Eigen::Vector3d>());
+        return std::make_pair(temp_value, SerializedSizeEigenType<Eigen::Vector3d>());
+    }
+
+    template<>
+    inline uint64_t SerializedSizeEigenType(const Eigen::Matrix<double, 6, 1>& value)
+    {
+        (void)(value);
+        return (uint64_t)(6 * sizeof(double));
+    }
+
+    template<>
+    inline uint64_t SerializedSizeEigenType<Eigen::Matrix<double, 6, 1>>(void)
+    {
+        return (uint64_t)(6 * sizeof(double));
+    }
+
+    template<>
+    inline uint64_t SerializeEigenType(const Eigen::Matrix<double, 6, 1>& value, std::vector<uint8_t>& buffer)
+    {
+        // Takes a state to serialize and a buffer to serialize into
+        // Return number of bytes written to buffer
+        std::vector<uint8_t> temp_buffer(SerializedSizeEigenType<Eigen::Matrix<double, 6, 1>>(), 0x00);
+        memcpy(&temp_buffer.front(), value.data(), SerializedSizeEigenType<Eigen::Matrix<double, 6, 1>>());
+        buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
+        return SerializedSizeEigenType<Eigen::Matrix<double, 6, 1>>();
+    }
+
+    template<>
+    inline std::pair<Eigen::Matrix<double, 6, 1>, uint64_t> DeserializeEigenType<Eigen::Matrix<double, 6, 1>>(const std::vector<uint8_t>& buffer, const uint64_t current)
+    {
+        assert(current < buffer.size());
+        assert((current + SerializedSizeEigenType<Eigen::Matrix<double, 6, 1>>()) <= buffer.size());
+        // Takes a buffer to read from and the starting index in the buffer
+        // Return the loaded state and how many bytes we read from the buffer
+        Eigen::Matrix<double, 6, 1> temp_value;
+        memcpy(temp_value.data(), &buffer[current], SerializedSizeEigenType<Eigen::Matrix<double, 6, 1>>());
+        return std::make_pair(temp_value, SerializedSizeEigenType<Eigen::Matrix<double, 6, 1>>());
+    }
+
+    template<>
+    inline uint64_t SerializedSizeEigenType(const Eigen::Isometry3d& value)
+    {
+        (void)(value);
+        return (uint64_t)(16 * sizeof(double));
+    }
+
+    template<>
+    inline uint64_t SerializedSizeEigenType<Eigen::Isometry3d>(void)
+    {
+        return (uint64_t)(16 * sizeof(double));
+    }
+
+    template<>
+    inline uint64_t SerializeEigenType(const Eigen::Isometry3d& value, std::vector<uint8_t>& buffer)
+    {
+        // Takes a state to serialize and a buffer to serialize into
+        // Return number of bytes written to buffer
+        std::vector<uint8_t> temp_buffer(SerializedSizeEigenType<Eigen::Isometry3d>(), 0x00);
+        memcpy(&temp_buffer.front(), value.matrix().data(), SerializedSizeEigenType<Eigen::Isometry3d>());
+        buffer.insert(buffer.end(), temp_buffer.begin(), temp_buffer.end());
+        return SerializedSizeEigenType<Eigen::Isometry3d>();
+    }
+
+    template<>
+    inline std::pair<Eigen::Isometry3d, uint64_t> DeserializeEigenType<Eigen::Isometry3d>(const std::vector<uint8_t>& buffer, const uint64_t current)
+    {
+        assert(current < buffer.size());
+        assert((current + SerializedSizeEigenType<Eigen::Isometry3d>()) <= buffer.size());
+        // Takes a buffer to read from and the starting index in the buffer
+        // Return the loaded state and how many bytes we read from the buffer
+        Eigen::Isometry3d temp_value;
+        memcpy(temp_value.matrix().data(), &buffer[current], SerializedSizeEigenType<Eigen::Isometry3d>());
+        return std::make_pair(temp_value, SerializedSizeEigenType<Eigen::Isometry3d>());
+    }
+}
+
+#endif // SERIALIZATION_EIGEN_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/serialization_ros.hpp

@@ -0,0 +1,241 @@
+#ifndef SERIALIZATION_ROS_HPP
+#define SERIALIZATION_ROS_HPP
+
+#include "arc_utilities/serialization.hpp"
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Point.h>
+#include <geometry_msgs/Vector3.h>
+
+namespace arc_utilities
+{
+    inline uint64_t SerializeHeader(
+            const std_msgs::Header& header,
+            std::vector<uint8_t>& buffer)
+    {
+        uint64_t bytes_written = 0;
+        bytes_written += SerializeFixedSizePOD(header.seq, buffer);
+        bytes_written += SerializeFixedSizePOD(header.stamp.sec, buffer);
+        bytes_written += SerializeFixedSizePOD(header.stamp.nsec, buffer);
+        bytes_written += SerializeString(header.frame_id, buffer);
+        return bytes_written;
+    }
+
+    inline std::pair<std_msgs::Header, uint64_t> DeserializeHeader(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current)
+    {
+        std_msgs::Header header;
+        uint64_t bytes_read = 0;
+        const auto deserialized_seq = DeserializeFixedSizePOD<decltype(header.seq)>(buffer, current + bytes_read);
+        header.seq = deserialized_seq.first;
+        bytes_read += deserialized_seq.second;
+        const auto deserialized_sec = DeserializeFixedSizePOD<decltype(header.stamp.sec)>(buffer, current + bytes_read);
+        header.stamp.sec = deserialized_sec.first;
+        bytes_read += deserialized_sec.second;
+        const auto deserialized_nsec = DeserializeFixedSizePOD<decltype(header.stamp.sec)>(buffer, current + bytes_read);
+        header.stamp.nsec = deserialized_nsec.first;
+        bytes_read += deserialized_nsec.second;
+        const auto deserialized_frame_id = DeserializeString<char>(buffer, current + bytes_read);
+        header.frame_id = deserialized_frame_id.first;
+        bytes_read += deserialized_frame_id.second;
+        return {header, bytes_read};
+    }
+
+    inline uint64_t SerializePoint(
+            const geometry_msgs::Point& point,
+            std::vector<uint8_t>& buffer)
+    {
+        uint64_t bytes_written = 0;
+        bytes_written += SerializeFixedSizePOD(point.x, buffer);
+        bytes_written += SerializeFixedSizePOD(point.y, buffer);
+        bytes_written += SerializeFixedSizePOD(point.z, buffer);
+        return bytes_written;
+    }
+
+    inline std::pair<geometry_msgs::Point, uint64_t> DeserializePoint(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current)
+    {
+        geometry_msgs::Point point;
+        uint64_t bytes_read = 0;
+        const auto deserialized_x = DeserializeFixedSizePOD<decltype(point.x)>(buffer, current + bytes_read);
+        point.x = deserialized_x.first;
+        bytes_read += deserialized_x.second;
+        const auto deserialized_y = DeserializeFixedSizePOD<decltype(point.y)>(buffer, current + bytes_read);
+        point.y = deserialized_y.first;
+        bytes_read += deserialized_y.second;
+        const auto deserialized_z = DeserializeFixedSizePOD<decltype(point.z)>(buffer, current + bytes_read);
+        point.z = deserialized_z.first;
+        bytes_read += deserialized_z.second;
+        return {point, bytes_read};
+    }
+
+    inline uint64_t SerializeVector3(
+            const geometry_msgs::Vector3& vector,
+            std::vector<uint8_t>& buffer)
+    {
+        uint64_t bytes_written = 0;
+        bytes_written += SerializeFixedSizePOD(vector.x, buffer);
+        bytes_written += SerializeFixedSizePOD(vector.y, buffer);
+        bytes_written += SerializeFixedSizePOD(vector.z, buffer);
+        return bytes_written;
+    }
+
+    inline std::pair<geometry_msgs::Vector3, uint64_t> DeserializeVector3(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current)
+    {
+        geometry_msgs::Vector3 vector;
+        uint64_t bytes_read = 0;
+        const auto deserialized_x = DeserializeFixedSizePOD<decltype(vector.x)>(buffer, current + bytes_read);
+        vector.x = deserialized_x.first;
+        bytes_read += deserialized_x.second;
+        const auto deserialized_y = DeserializeFixedSizePOD<decltype(vector.y)>(buffer, current + bytes_read);
+        vector.y = deserialized_y.first;
+        bytes_read += deserialized_y.second;
+        const auto deserialized_z = DeserializeFixedSizePOD<decltype(vector.z)>(buffer, current + bytes_read);
+        vector.z = deserialized_z.first;
+        bytes_read += deserialized_z.second;
+        return {vector, bytes_read};
+    }
+
+    inline uint64_t SerializeQuaternion(
+            const geometry_msgs::Quaternion& quat,
+            std::vector<uint8_t>& buffer)
+    {
+        uint64_t bytes_written = 0;
+        bytes_written += SerializeFixedSizePOD(quat.x, buffer);
+        bytes_written += SerializeFixedSizePOD(quat.y, buffer);
+        bytes_written += SerializeFixedSizePOD(quat.z, buffer);
+        bytes_written += SerializeFixedSizePOD(quat.w, buffer);
+        return bytes_written;
+    }
+
+    inline std::pair<geometry_msgs::Quaternion, uint64_t> DeserializeQuaternion(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current)
+    {
+        geometry_msgs::Quaternion quat;
+        uint64_t bytes_read = 0;
+        const auto deserialized_x = DeserializeFixedSizePOD<decltype(quat.x)>(buffer, current + bytes_read);
+        quat.x = deserialized_x.first;
+        bytes_read += deserialized_x.second;
+        const auto deserialized_y = DeserializeFixedSizePOD<decltype(quat.y)>(buffer, current + bytes_read);
+        quat.y = deserialized_y.first;
+        bytes_read += deserialized_y.second;
+        const auto deserialized_z = DeserializeFixedSizePOD<decltype(quat.z)>(buffer, current + bytes_read);
+        quat.z = deserialized_z.first;
+        bytes_read += deserialized_z.second;
+        const auto deserialized_w = DeserializeFixedSizePOD<decltype(quat.z)>(buffer, current + bytes_read);
+        quat.w = deserialized_w.first;
+        bytes_read += deserialized_w.second;
+        return {quat, bytes_read};
+    }
+
+    inline uint64_t SerializePose(
+            const geometry_msgs::Pose& pose,
+            std::vector<uint8_t>& buffer)
+    {
+        uint64_t bytes_written = 0;
+        bytes_written += SerializePoint(pose.position, buffer);
+        bytes_written += SerializeQuaternion(pose.orientation, buffer);
+        return bytes_written;
+    }
+
+    inline std::pair<geometry_msgs::Pose, uint64_t> DeserializePose(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current)
+    {
+        geometry_msgs::Pose pose;
+        uint64_t bytes_read = 0;
+        const auto deserialized_position = DeserializePoint(buffer, current + bytes_read);
+        pose.position = deserialized_position.first;
+        bytes_read += deserialized_position.second;
+        const auto deserialized_quat = DeserializeQuaternion(buffer, current + bytes_read);
+        pose.orientation = deserialized_quat.first;
+        bytes_read += deserialized_quat.second;
+        return {pose, bytes_read};
+    }
+
+    inline uint64_t SerializePoseStamped(
+            const geometry_msgs::PoseStamped& pose,
+            std::vector<uint8_t>& buffer)
+    {
+        uint64_t bytes_written = 0;
+        bytes_written += SerializeHeader(pose.header, buffer);
+        bytes_written += SerializePose(pose.pose, buffer);
+        return bytes_written;
+    }
+
+    inline std::pair<geometry_msgs::PoseStamped, uint64_t> DeserializePoseStamped(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current)
+    {
+        geometry_msgs::PoseStamped pose;
+        uint64_t bytes_read = 0;
+        const auto deserialized_header = DeserializeHeader(buffer, current + bytes_read);
+        pose.header = deserialized_header.first;
+        bytes_read += deserialized_header.second;
+        const auto deserialized_pose = DeserializePose(buffer, current + bytes_read);
+        pose.pose = deserialized_pose.first;
+        bytes_read += deserialized_pose.second;
+        return {pose, bytes_read};
+    }
+
+    inline uint64_t SerializeTransform(
+            const geometry_msgs::Transform& transform,
+            std::vector<uint8_t>& buffer)
+    {
+        uint64_t bytes_written = 0;
+        bytes_written += SerializeVector3(transform.translation, buffer);
+        bytes_written += SerializeQuaternion(transform.rotation, buffer);
+        return bytes_written;
+    }
+
+    inline std::pair<geometry_msgs::Transform, uint64_t> DeserializeTransform(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current)
+    {
+        geometry_msgs::Transform transform;
+        uint64_t bytes_read = 0;
+        const auto deserialized_translation = DeserializeVector3(buffer, current + bytes_read);
+        transform.translation = deserialized_translation.first;
+        bytes_read += deserialized_translation.second;
+        const auto deserialized_quat = DeserializeQuaternion(buffer, current + bytes_read);
+        transform.rotation = deserialized_quat.first;
+        bytes_read += deserialized_quat.second;
+        return {transform, bytes_read};
+    }
+
+    inline uint64_t SerializeTransformStamped(
+            const geometry_msgs::TransformStamped& transform,
+            std::vector<uint8_t>& buffer)
+    {
+        uint64_t bytes_written = 0;
+        bytes_written += SerializeHeader(transform.header, buffer);
+        bytes_written += SerializeTransform(transform.transform, buffer);
+        bytes_written += SerializeString<char>(transform.child_frame_id, buffer);
+        return bytes_written;
+    }
+
+    inline std::pair<geometry_msgs::TransformStamped, uint64_t> DeserializeTransformStamped(
+            const std::vector<uint8_t>& buffer,
+            const uint64_t current)
+    {
+        geometry_msgs::TransformStamped transform;
+        uint64_t bytes_read = 0;
+        const auto deserialized_header = DeserializeHeader(buffer, current + bytes_read);
+        transform.header = deserialized_header.first;
+        bytes_read += deserialized_header.second;
+        const auto deserialized_transform = DeserializeTransform(buffer, current + bytes_read);
+        transform.transform = deserialized_transform.first;
+        bytes_read += deserialized_transform.second;
+        const auto deserialized_child_frame_id = DeserializeString<char>(buffer, current + bytes_read);
+        transform.child_frame_id = deserialized_child_frame_id.first;
+        bytes_read += deserialized_child_frame_id.second;
+        return {transform, bytes_read};
+    }
+}
+
+#endif // SERIALIZATION_ROS_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/shortcut_smoothing.hpp

@@ -0,0 +1,210 @@
+#include <functional>
+#include <arc_utilities/arc_helpers.hpp>
+#include <arc_utilities/eigen_helpers.hpp>
+
+#ifndef SHORTCUT_SMOOTHING_HPP
+#define SHORTCUT_SMOOTHING_HPP
+
+namespace shortcut_smoothing
+{    
+    inline EigenHelpers::VectorVector3d InterpolateWithCollisionCheck(
+            const EigenHelpers::VectorVector3d& input_vector,
+            const size_t first_ind,
+            const size_t second_ind,
+            const double step_size,
+            const std::function<bool(const Eigen::Vector3d&)>& collision_fn)
+    {
+        const size_t starting_ind = std::min(first_ind, second_ind);
+        const size_t ending_ind = std::max(first_ind, second_ind);
+
+        const Eigen::Vector3d& starting_point = input_vector[starting_ind];
+        const Eigen::Vector3d& ending_point = input_vector[ending_ind];
+        const Eigen::Vector3d delta = ending_point - starting_point;
+        const Eigen::Vector3d delta_unit_vec = delta.normalized();
+
+        const double total_dist = delta.norm();
+
+        if (total_dist <= step_size)
+        {
+            return input_vector;
+        }
+
+        // Collision check the path between the first and second index
+        // We assume that the endpoints are not in collision, so we don't check dist == 0 or dist == total_dist
+        bool collision = false;
+        for (double dist = step_size; !collision && dist < total_dist; dist += step_size)
+        {
+            const Eigen::Vector3d point_to_check = starting_point + dist * delta_unit_vec;
+            collision = collision_fn(point_to_check);
+        }
+
+
+        if (!collision)
+        {
+            const size_t num_original_elements = ending_ind - starting_ind - 1;
+            const size_t num_new_elements = (size_t)std::ceil(total_dist / step_size);
+
+            EigenHelpers::VectorVector3d output_vector(input_vector.size() - num_original_elements + num_new_elements - 1);
+
+            // Copy in the first unchanged elements of the vector
+            std::copy(input_vector.begin(), input_vector.begin() + starting_ind + 1, output_vector.begin());
+
+            // Assign the replaced elements
+            for (size_t new_element_ind = 1; new_element_ind < num_new_elements; ++new_element_ind)
+            {
+                const double dist = (double)new_element_ind * step_size;
+                output_vector[starting_ind + new_element_ind] = starting_point + dist * delta_unit_vec;
+            }
+
+            // Copy in the last unchanged elements of the vector
+            std::copy(input_vector.begin() + ending_ind, input_vector.end(), output_vector.begin() + starting_ind + num_new_elements);
+
+            return output_vector;
+
+        }
+        else
+        {
+            return input_vector;
+        }
+    }
+
+
+    /**
+     * @brief ShortcutSmoothPath
+     * @param path
+     * @param max_iterations
+     * @param max_failed_iterations
+     * @param max_shortcut_fraction
+     * @param edge_validity_check_fn - Must match the following prototype: std::function<bool(const Configuration&, const Configuration&)>&>
+     * @param prng
+     * @return
+     */
+    template<typename PRNG, typename Configuration, typename ConfigAlloc = std::allocator<Configuration>, class EdgeValidityCheckFn>
+    inline std::vector<Configuration, ConfigAlloc> ShortcutSmoothPath(
+            const std::vector<Configuration, ConfigAlloc>& path,
+            const uint32_t max_iterations, const uint32_t max_failed_iterations,
+            const double max_shortcut_fraction,
+            const EdgeValidityCheckFn& edge_validity_check_fn,
+            PRNG& prng)
+    {
+        std::vector<Configuration, ConfigAlloc> current_path = path;
+        uint32_t num_iterations = 0;
+        uint32_t failed_iterations = 0;
+        while (num_iterations < max_iterations && failed_iterations < max_failed_iterations && current_path.size() > 2)
+        {
+            // Attempt a shortcut
+            const int64_t base_index = std::uniform_int_distribution<size_t>(0, current_path.size() - 1)(prng);
+            // Pick an offset fraction
+            const double offset_fraction = std::uniform_real_distribution<double>(-max_shortcut_fraction, max_shortcut_fraction)(prng);
+            // Compute the offset index
+            const int64_t offset_index = base_index + (int64_t)((double)current_path.size() * offset_fraction); // Could be out of bounds
+            const int64_t safe_offset_index = arc_helpers::ClampValue(offset_index, (int64_t)0, (int64_t)(current_path.size() - 1));
+            // Get start & end indices
+            const size_t start_index = (size_t)std::min(base_index, safe_offset_index);
+            const size_t end_index = (size_t)std::max(base_index, safe_offset_index);
+            if (end_index <= start_index + 1)
+            {
+                num_iterations++;
+                continue;
+            }
+            // Check if the edge is valid
+            const Configuration& start_config = current_path[start_index];
+            const Configuration& end_config = current_path[end_index];
+            const bool edge_valid = edge_validity_check_fn(start_config, end_config);
+            if (edge_valid)
+            {
+                // Make the shortened path
+                std::vector<Configuration, ConfigAlloc> shortened_path;
+                shortened_path.insert(shortened_path.end(), current_path.begin(), current_path.begin() + start_index + 1);
+                shortened_path.insert(shortened_path.end(), current_path.begin() + end_index, current_path.end());
+                current_path = shortened_path;
+                num_iterations++;
+            }
+            else
+            {
+                num_iterations++;
+                failed_iterations++;
+            }
+        }
+        return current_path;
+    }
+
+    /**
+     * @brief ResamplePathPartial Returns the resampled portion of the path between [start_ind, end_ind); *not* the whole path
+     * @param path
+     * @param start_ind
+     * @param end_ind
+     * @param resampled_state_distance
+     * @param state_distance_fn - must match the following prototype: std::function<double(const Configuration&, const Configuration&, const)>
+     * @param state_interpolation_fn - must match the following prototype: std::function<Configuration(const Configuration&, const Configuration&, const double)>
+     * @return
+     */
+    template<typename Configuration, typename ConfigAlloc = std::allocator<Configuration>, class DistanceFn, class InterpolationFn>
+    inline std::vector<Configuration, ConfigAlloc> ResamplePathPartial(
+            const std::vector<Configuration, ConfigAlloc>& path,
+            const size_t start_ind,
+            const size_t end_ind,
+            const double resampled_state_distance,
+            const DistanceFn& state_distance_fn,
+            const InterpolationFn& state_interpolation_fn)
+    {
+        assert(end_ind > start_ind);
+        assert(end_ind <= path.size());
+
+        // If we only have one element, to resample between, return it
+        if (end_ind - start_ind == 1)
+        {
+            return std::vector<Configuration, ConfigAlloc>(1, path[start_ind]);
+        }
+
+        // Add the first state
+        std::vector<Configuration, ConfigAlloc> resampled_path;
+        resampled_path.push_back(path[start_ind]);
+
+        // Loop through the path, adding interpolated states as needed
+        for (size_t idx = start_ind; idx < end_ind - 1; idx++)
+        {
+            // Get the states from the original path
+            const Configuration& current_state = path[idx];
+            const Configuration& next_state = path[idx + 1];
+
+            // We want to add all the intermediate states to our returned path
+            const double distance = state_distance_fn(current_state, next_state);
+            const double raw_num_intervals = distance / resampled_state_distance;
+            const uint32_t num_segments = (uint32_t)std::ceil(raw_num_intervals);
+
+            if (num_segments == 0u)
+            {
+                // Do nothing because this means distance was exactly 0, so we are going to discard the extra point
+            }
+            // If there's only one segment, we just add the end state of the window
+            else if (num_segments == 1u)
+            {
+                resampled_path.push_back(next_state);
+            }
+            // If there is more than one segment, interpolate between previous_state and current_state (including the next_state)
+            else
+            {
+                for (uint32_t segment = 1u; segment <= num_segments; segment++)
+                {
+                    const double interpolation_ratio = (double)segment / (double)num_segments;
+                    const Configuration interpolated_state = state_interpolation_fn(current_state, next_state, interpolation_ratio);
+                    resampled_path.push_back(interpolated_state);
+                }
+            }
+        }
+        return resampled_path;
+    }
+
+    template<typename Configuration, typename ConfigAlloc = std::allocator<Configuration>, class DistanceFn, class InterpolationFn>
+    inline std::vector<Configuration, ConfigAlloc> ResamplePath(
+            const std::vector<Configuration, ConfigAlloc>& path,
+            const double resampled_state_distance,
+            const DistanceFn& state_distance_fn,
+            const InterpolationFn& state_interpolation_fn)
+    {
+        return ResamplePathPartial(path, 0, path.size(), resampled_state_distance, state_distance_fn, state_interpolation_fn);
+    }
+}
+
+#endif // SHORTCUT_SMOOTHING_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/simple_astar_planner.hpp

@@ -0,0 +1,205 @@
+#ifndef SIMPLE_ASTAR_PLANNER_HPP
+#define SIMPLE_ASTAR_PLANNER_HPP
+
+#include <vector>
+#include <map>
+#include <queue>
+#include <unordered_set>
+#include <unordered_map>
+#include <functional>
+#include <algorithm>
+#include <chrono>
+
+#include <arc_utilities/eigen_helpers.hpp>
+
+//#include <ros/ros.h>
+//#include <visualization_msgs/Marker.h>
+//#include <arc_utilities/arc_helpers.hpp>
+//#include <arc_utilities/ros_helpers.hpp>
+//#include <arc_utilities/eigen_helpers_conversions.hpp>
+
+namespace simple_astar_planner
+{
+    template<typename ConfigType, typename Allocator = std::allocator<ConfigType>>
+    class SimpleAStarPlanner
+    {
+        protected:
+            SimpleAStarPlanner() {}
+
+            typedef std::pair<ConfigType, double> ConfigAndDistType;
+            struct AStarComparator
+            {
+                public:
+                    AStarComparator(const std::function<double(const ConfigType&)>& heuristic_fn)
+                        : heuristic_fn_(heuristic_fn)
+                    {}
+
+                    // Defines a "less" operation"; by using "greater" then the smallest element will appear at the top of the priority queue
+                    bool operator()(const ConfigAndDistType& c1, const ConfigAndDistType& c2) const
+                    {
+                        // If both expected distances are the same, then we want to explore the one that has the smaller heuristic distance
+                        if (EigenHelpers::IsApprox(c1.second, c2.second, 1e-10))
+                        {
+                            const double hdist_c1 = heuristic_fn_(c1.first);
+                            const double hdist_c2 = heuristic_fn_(c2.first);
+                            return (hdist_c1 > hdist_c2);
+                        }
+                        // If expected distances are different, we want to explore the one with the smaller expected distance
+                        else
+                        {
+                            return (c1.second > c2.second);
+                        }
+                    }
+
+                    const std::function<double(const ConfigType&)>& heuristic_fn_;
+            };
+
+            template<typename ConfigHasher = std::hash<ConfigType>>
+            static std::vector<ConfigType, Allocator> ExtractPathBasic(const std::unordered_map<ConfigType, ConfigType, ConfigHasher>& backpointers, ConfigType last_state)
+            {
+                std::vector<ConfigType, Allocator> path;
+                for (auto backpointer_ittr = backpointers.find(last_state); backpointer_ittr != backpointers.end(); backpointer_ittr = backpointers.find(last_state))
+                {
+                    path.push_back(last_state);
+                    last_state = backpointer_ittr->second;
+                }
+                path.push_back(last_state);
+                std::reverse(path.begin(), path.end());
+                return path;
+            }
+
+        public:
+            template<typename ConfigHasher = std::hash<ConfigType>>
+            static std::pair<std::vector<ConfigType, Allocator>, std::map<std::string, double>> Plan(
+                    const ConfigType& start,
+                    const std::function<std::vector<ConfigType, Allocator>(const ConfigType&)>& neighbour_fn,
+                    const std::function<double(const ConfigType&, const ConfigType&)>& distance_fn,
+                    const std::function<double(const ConfigType&)>& heuristic_fn,
+                    const std::function<bool(const ConfigType&)>& goal_reached_fn)
+            {
+                const auto start_time = std::chrono::steady_clock::now();
+
+//                ros::NodeHandle nh;
+//                visualization_msgs::Marker marker;
+//                marker.header.frame_id = "mocap_world";
+//                marker.type = visualization_msgs::Marker::CUBE_LIST;
+//                marker.action = visualization_msgs::Marker::ADD;
+//                marker.ns = "explored_states";
+//                marker.id = 1;
+//                marker.scale.x = ROSHelpers::GetParam(nh, "world_x_step", 1.0);
+//                marker.scale.y = ROSHelpers::GetParam(nh, "world_y_step", 1.0);
+//                marker.scale.z = ROSHelpers::GetParam(nh, "world_z_step", 1.0);
+//                marker.points.reserve(4096);
+//                marker.color = arc_helpers::RGBAColorBuilder<std_msgs::ColorRGBA>::MakeFromFloatColors(1.0, 1.0, 0.0, 1.0);
+
+//                visualization_msgs::Marker neighbours_marker;
+//                neighbours_marker.header.frame_id = "mocap_world";
+//                neighbours_marker.type = visualization_msgs::Marker::CUBE_LIST;
+//                neighbours_marker.action = visualization_msgs::Marker::ADD;
+//                neighbours_marker.ns = "neighbours";
+//                neighbours_marker.id = 1;
+//                neighbours_marker.scale.x = ROSHelpers::GetParam(nh, "world_x_step", 1.0);
+//                neighbours_marker.scale.y = ROSHelpers::GetParam(nh, "world_y_step", 1.0);
+//                neighbours_marker.scale.z = ROSHelpers::GetParam(nh, "world_z_step", 1.0);
+//                neighbours_marker.points.reserve(26);
+//                neighbours_marker.color = arc_helpers::RGBAColorBuilder<std_msgs::ColorRGBA>::MakeFromFloatColors(0.0, 0.0, 1.0, 0.3);
+
+//                ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 100, false);
+
+
+
+                std::pair<std::vector<ConfigType, Allocator>, std::map<std::string, double>> results;
+
+                const AStarComparator astar_compare(heuristic_fn);
+                std::priority_queue<ConfigAndDistType, std::vector<ConfigAndDistType>, AStarComparator> frontier(astar_compare);
+                std::unordered_set<ConfigType, ConfigHasher> explored;
+                std::unordered_map<ConfigType, double, ConfigHasher> cost_to_come;
+                std::unordered_map<ConfigType, ConfigType, ConfigHasher> backpointers;
+
+                frontier.push(ConfigAndDistType(start, heuristic_fn(start)));
+                cost_to_come[start] = 0.0;
+
+                bool goal_reached = false;
+                while (!goal_reached && frontier.size() > 0)
+                {
+                    const ConfigAndDistType current = frontier.top();
+                    frontier.pop();
+                    const ConfigType& current_node = current.first;
+
+                    if (goal_reached_fn(current_node) == true)
+                    {
+                        results.first = ExtractPathBasic(backpointers, current_node);
+                        goal_reached = true;
+                    }
+                    // Double check if we've already explored this node:
+                    //    a single node can be inserted into the frontier multiple times at the same or different priorities
+                    //    so we want to avoid the expense of re-exploring it, and just discard this one once we pop it
+                    else if (explored.find(current_node) == explored.end())
+                    {
+//                        geometry_msgs::Point p;
+//                        p.x = current_node[0];
+//                        p.y = current_node[1];
+//                        p.z = current_node[2];
+//                        marker.points.push_back(p);
+//                        marker.header.stamp = ros::Time::now();
+//                        marker_pub.publish(marker);
+
+                        explored.insert(current_node);
+                        const double current_cost_to_come = cost_to_come.at(current_node);
+
+                        // Expand the node to find all neighbours, adding them to the frontier if we have not already explored them
+                        const auto neighbours = neighbour_fn(current_node);
+
+//                        neighbours_marker.points = EigenHelpersConversions::VectorEigenVector3dToVectorGeometryPoint(neighbours);
+//                        neighbours_marker.header.stamp = ros::Time::now();
+//                        marker_pub.publish(neighbours_marker);
+//                        std::cout << std::flush;
+
+                        for (const auto neighbour : neighbours)
+                        {
+                            // Check if we've already explored this neighbour
+                            if (explored.find(neighbour) != explored.end())
+                            {
+                                continue;
+                            }
+
+                            // Do some sanity checks so that we can make assumptions later
+                            const auto neighbour_cost_to_come_ittr = cost_to_come.find(neighbour);
+                            const auto neighbour_backpointer_ittr = backpointers.find(neighbour);
+                            if (neighbour_cost_to_come_ittr == cost_to_come.end())
+                            {
+                                assert(neighbour_backpointer_ittr == backpointers.end());
+                            }
+                            if (neighbour_backpointer_ittr == backpointers.end())
+                            {
+                                assert(neighbour_cost_to_come_ittr == cost_to_come.end());
+                            }
+
+                            // If we haven't already explored this neighbour, see if we've found a cheaper path
+                            const double neighbour_new_cost_to_come = current_cost_to_come + distance_fn(current_node, neighbour);
+                            if (neighbour_cost_to_come_ittr != cost_to_come.end() && neighbour_cost_to_come_ittr->second <= neighbour_new_cost_to_come)
+                            {
+                                continue;
+                            }
+
+                            frontier.push(ConfigAndDistType(neighbour, neighbour_new_cost_to_come + heuristic_fn(neighbour)));
+                            cost_to_come[neighbour] = neighbour_new_cost_to_come;
+                            backpointers[neighbour] = current_node;
+                        }
+                    }
+                    else
+                    {
+//                        std::cout << "Already explored this node, skipping\n";
+                    }
+                }
+
+                const auto end_time = std::chrono::steady_clock::now();
+                results.second["planning time"] = std::chrono::duration<double>(end_time - start_time).count();
+                results.second["nodes explored"] = explored.size();
+
+                return results;
+            }
+    };
+}
+
+#endif // SIMPLE_ASTAR_PLANNER_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/simple_dtw.hpp

@@ -0,0 +1,117 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <vector>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <stdexcept>
+#include <functional>
+#include <mutex>
+#include <Eigen/Geometry>
+
+#ifndef SIMPLE_DTW_HPP
+#define SIMPLE_DTW_HPP
+
+namespace simple_dtw
+{
+    template<typename FirstDatatype, typename SecondDatatype, typename DistanceFn,
+             typename FirstAllocator = std::allocator<FirstDatatype>,
+             typename SecondAllocator = std::allocator<SecondDatatype>>
+    class SimpleDTW
+    {
+    private:
+
+        void InitializeMatrix(const size_t first_sequence_size, const size_t second_sequence_size)
+        {
+            const ssize_t rows = (ssize_t)first_sequence_size + 1;
+            const ssize_t cols = (ssize_t)second_sequence_size + 1;
+            if (dtw_matrix_.rows() < rows || dtw_matrix_.cols() < cols)
+            {
+                dtw_matrix_ = Eigen::MatrixXd::Zero(rows, cols);
+                if (rows > 1 && cols > 1)
+                {
+                    for (ssize_t row = 1; row < rows; row++)
+                    {
+                        dtw_matrix_(row, 0) = std::numeric_limits<double>::infinity();
+                    }
+                    for (ssize_t col = 1; col < cols; col++)
+                    {
+                        dtw_matrix_(0, col) = std::numeric_limits<double>::infinity();
+                    }
+                }
+            }
+
+        }
+
+        Eigen::MatrixXd dtw_matrix_;
+
+    public:
+
+        SimpleDTW()
+        {
+            InitializeMatrix(0, 0);
+        }
+
+        SimpleDTW(const size_t first_sequence_size, const size_t second_sequence_size)
+        {
+            InitializeMatrix(first_sequence_size, second_sequence_size);
+        }
+
+        double EvaluateWarpingCost(
+                const std::vector<FirstDatatype, FirstAllocator>& first_sequence,
+                const std::vector<SecondDatatype, SecondAllocator>& second_sequence,
+                const DistanceFn& distance_fn)
+        {
+            InitializeMatrix(first_sequence.size(), second_sequence.size());
+            //Compute DTW cost for the two sequences
+            for (ssize_t i = 1; i <= (ssize_t)first_sequence.size(); i++)
+            {
+                const FirstDatatype& first_item = first_sequence[(size_t)i - 1];
+                for (ssize_t j = 1; j <= (ssize_t)second_sequence.size(); j++)
+                {
+                    const SecondDatatype& second_item = second_sequence[(size_t)j - 1];
+                    const double index_cost = distance_fn(first_item, second_item);
+                    double prev_cost = 0.0;
+                    // Get the three neighboring values from the matrix to use for the update
+                    double im1j = dtw_matrix_(i - 1, j);
+                    double im1jm1 = dtw_matrix_(i - 1, j - 1);
+                    double ijm1 = dtw_matrix_(i, j - 1);
+                    // Start the update step
+                    if (im1j < im1jm1 && im1j < ijm1)
+                    {
+                        prev_cost = im1j;
+                    }
+                    else if (ijm1 < im1j && ijm1 < im1jm1)
+                    {
+                        prev_cost = ijm1;
+                    }
+                    else
+                    {
+                        prev_cost = im1jm1;
+                    }
+                    // Update the value in the matrix
+                    const double new_cost = index_cost + prev_cost;
+                    dtw_matrix_(i, j) = new_cost;
+                }
+            }
+            //Return total path cost
+            const double warping_cost = dtw_matrix_((ssize_t)first_sequence.size(), (ssize_t)second_sequence.size());
+            return warping_cost;
+        }
+    };
+
+    // DistanceFn must match the prototype std::function<double(const FirstDataype&, const SecondDatatype&)>
+    template<typename FirstDatatype, typename SecondDatatype, typename DistanceFn,
+             typename FirstAllocator = std::allocator<FirstDatatype>,
+             typename SecondAllocator = std::allocator<SecondDatatype>>
+    inline double ComputeDTWDistance(
+            const std::vector<FirstDatatype, FirstAllocator>& first_sequence,
+            const std::vector<SecondDatatype, SecondAllocator>& second_sequence,
+            const DistanceFn& distance_fn)
+    {
+        SimpleDTW<FirstDatatype, SecondDatatype, DistanceFn, FirstAllocator, SecondAllocator> dtw_evaluator;
+        return dtw_evaluator.EvaluateWarpingCost(first_sequence, second_sequence, distance_fn);
+    }
+}
+
+#endif // SIMPLE_DTW_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/simple_hausdorff_distance.hpp

@@ -0,0 +1,95 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <vector>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <stdexcept>
+#include <functional>
+#include <mutex>
+#include <Eigen/Geometry>
+
+#ifdef ENABLE_PARALLEL_HAUSDORFF_DISTANCE
+    #include <omp.h>
+#endif
+
+#ifndef SIMPLE_HAUSDORFF_DISTANCE_HPP
+#define SIMPLE_HAUSDORFF_DISTANCE_HPP
+
+namespace simple_hausdorff_distance
+{
+    class SimpleHausdorffDistance
+    {
+    private:
+
+        SimpleHausdorffDistance() {}
+
+        static inline size_t GetNumOMPThreads(void)
+        {
+#ifdef ENABLE_PARALLEL_HAUSDORFF_DISTANCE
+        #if defined(_OPENMP)
+            size_t num_threads = 0;
+            #pragma omp parallel
+            {
+                num_threads = (size_t)omp_get_num_threads();
+            }
+            return num_threads;
+        #else
+            return 1;
+        #endif
+#else
+            return 1;
+#endif
+        }
+
+    public:
+
+        template<typename FirstDatatype, typename SecondDatatype, typename FirstAllocator=std::allocator<FirstDatatype>, typename SecondAllocator=std::allocator<SecondDatatype>>
+        static double ComputeDistance(const std::vector<FirstDatatype, FirstAllocator>& first_distribution, const std::vector<SecondDatatype, SecondAllocator>& second_distribution, const std::function<double(const FirstDatatype&, const SecondDatatype&)>& distance_fn)
+        {
+            // Compute the Hausdorff distance - the "maximum minimum" distance
+            std::vector<double> per_thread_storage(GetNumOMPThreads(), 0.0);
+#ifdef ENABLE_PARALLEL_HAUSDORFF_DISTANCE
+            #pragma omp parallel for
+#endif
+            for (size_t idx = 0; idx < first_distribution.size(); idx++)
+            {
+                const FirstDatatype& first = first_distribution[idx];
+                double minimum_distance = INFINITY;
+                for (size_t jdx = 0; jdx < second_distribution.size(); jdx++)
+                {
+                    const SecondDatatype& second = second_distribution[jdx];
+                    const double current_distance = distance_fn(first, second);
+                    if (current_distance < minimum_distance)
+                    {
+                        minimum_distance = current_distance;
+                    }
+                }
+#ifdef ENABLE_PARALLEL_HAUSDORFF_DISTANCE
+                #if defined(_OPENMP)
+                const size_t current_thread_id = (size_t)omp_get_thread_num();
+                #else
+                const size_t current_thread_id = 0;
+                #endif
+#else
+                const size_t current_thread_id = 0;
+#endif
+                if (minimum_distance > per_thread_storage[current_thread_id])
+                {
+                    per_thread_storage[current_thread_id] = minimum_distance;
+                }
+            }
+            double maximum_minimum_distance = 0.0;
+            for (size_t idx = 0; idx < per_thread_storage.size(); idx++)
+            {
+                const double temp_minimum_distance = per_thread_storage[idx];
+                if (temp_minimum_distance > maximum_minimum_distance)
+                {
+                    maximum_minimum_distance = temp_minimum_distance;
+                }
+            }
+            return maximum_minimum_distance;
+        }
+    };
+}
+#endif // SIMPLE_HAUSDORFF_DISTANCE_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/simple_hierarchical_clustering.hpp

@@ -0,0 +1,378 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <vector>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <stdexcept>
+#include <functional>
+#include <arc_utilities/arc_helpers.hpp>
+#include <Eigen/Geometry>
+
+#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
+    #include <omp.h>
+#endif
+
+#ifndef SIMPLE_HIERARCHICAL_CLUSTERING_HPP
+#define SIMPLE_HIERARCHICAL_CLUSTERING_HPP
+
+namespace simple_hierarchical_clustering
+{
+    class SimpleHierarchicalClustering
+    {
+    private:
+
+        SimpleHierarchicalClustering() {}
+
+        static inline size_t GetNumOMPThreads()
+        {
+#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
+            size_t num_threads = 0;
+            #pragma omp parallel
+            {
+                num_threads = (size_t)omp_get_num_threads();
+            }
+            return num_threads;
+#else
+            return 1;
+#endif
+        }
+
+        static std::pair<std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>>, double> GetClosestPair(const std::vector<uint8_t>& datapoint_mask, const Eigen::MatrixXd& distance_matrix, const std::vector<std::vector<int64_t>>& clusters)
+        {
+            // Compute distances between unclustered points <-> unclustered points, unclustered_points <-> clusters, and clusters <-> clusters
+            // Compute the minimum unclustered point <-> unclustered point / unclustered_point <-> cluster distance
+#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
+            const size_t num_threads = GetNumOMPThreads();
+            std::vector<double> per_thread_min_distances(num_threads, INFINITY);
+            std::vector<std::pair<int64_t, std::pair<bool, int64_t>>> per_thread_min_element_pairs(num_threads, std::make_pair(-1, std::make_pair(false, -1)));
+            #pragma omp parallel for
+#else
+            double min_distance = INFINITY;
+            std::pair<int64_t, std::pair<bool, int64_t>> min_element_pair(-1, std::pair<bool, int64_t>(false, -1));
+#endif
+            for (size_t idx = 0; idx < datapoint_mask.size(); idx++)
+            {
+                // Make sure we aren't in a cluster already
+                if (datapoint_mask[idx] == 0)
+                {
+                    // Compute the minimum unclustered point <-> unclustered point distance
+                    double min_point_point_distance = INFINITY;
+                    int64_t min_point_index = -1;
+                    for (size_t jdx = 0; jdx < datapoint_mask.size(); jdx++)
+                    {
+                        // Make sure the other point isn't us, and isn't already in a cluster
+                        if ((idx != jdx) && (datapoint_mask[jdx] == 0))
+                        {
+                            const double& current_distance = distance_matrix((ssize_t)idx, (ssize_t)jdx);
+                            // Update the closest point
+                            if (current_distance < min_point_point_distance)
+                            {
+                                min_point_point_distance = current_distance;
+                                min_point_index = (int64_t)jdx;
+                            }
+                        }
+                    }
+                    // Compute the minimum unclustered point <-> cluster distance
+                    double min_point_cluster_distance = INFINITY;
+                    int64_t min_cluster_index = -1;
+                    for (size_t cdx = 0; cdx < clusters.size(); cdx++)
+                    {
+                        // We only work with clusters that aren't empty
+                        if (clusters[cdx].size() > 0)
+                        {
+                            // Compute the distance to the current cluster
+                            double current_distance = 0.0;
+                            for (size_t cpdx = 0; cpdx < clusters[cdx].size(); cpdx++)
+                            {
+                                const int64_t& current_cluster_point_index = clusters[cdx][cpdx];
+                                const double& new_distance = distance_matrix((ssize_t)idx, (ssize_t)current_cluster_point_index);
+                                if (new_distance > current_distance)
+                                {
+                                    current_distance = new_distance;
+                                }
+                            }
+                            // Update the closest cluster
+                            if (current_distance < min_point_cluster_distance)
+                            {
+                                min_point_cluster_distance = current_distance;
+                                min_cluster_index = (int64_t)cdx;
+                            }
+                        }
+                    }
+#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
+                    const size_t thread_num = (size_t)omp_get_thread_num();
+                    double& per_thread_min_distance = per_thread_min_distances[thread_num];
+                    std::pair<int64_t, std::pair<bool, int64_t>>& per_thread_min_element_pair = per_thread_min_element_pairs[thread_num];
+                    // Update the closest index
+                    if (min_point_point_distance < per_thread_min_distance)
+                    {
+                        per_thread_min_distance = min_point_point_distance;
+                        per_thread_min_element_pair.first = idx;
+                        per_thread_min_element_pair.second.first = false;
+                        per_thread_min_element_pair.second.second = min_point_index;
+                    }
+                    if (min_point_cluster_distance < per_thread_min_distance)
+                    {
+                        per_thread_min_distance = min_point_cluster_distance;
+                        per_thread_min_element_pair.first = idx;
+                        per_thread_min_element_pair.second.first = true;
+                        per_thread_min_element_pair.second.second = min_cluster_index;
+                    }
+#else
+                    // Update the closest index
+                    if (min_point_point_distance < min_distance)
+                    {
+                        min_distance = min_point_point_distance;
+                        min_element_pair.first = (int64_t)idx;
+                        min_element_pair.second.first = false;
+                        min_element_pair.second.second = min_point_index;
+                    }
+                    if (min_point_cluster_distance < min_distance)
+                    {
+                        min_distance = min_point_cluster_distance;
+                        min_element_pair.first = (int64_t)idx;
+                        min_element_pair.second.first = true;
+                        min_element_pair.second.second = min_cluster_index;
+                    }
+#endif
+                }
+            }
+#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
+            double min_distance = INFINITY;
+            std::pair<int64_t, std::pair<bool, int64_t>> min_element_pair(-1, std::pair<bool, int64_t>(false, -1));
+            for (size_t idx = 0; idx < num_threads; idx++)
+            {
+                const double& current_min_distance = per_thread_min_distances[idx];
+                const std::pair<int64_t, std::pair<bool, int64_t>>& current_min_element_pair = per_thread_min_element_pairs[idx];
+                if (current_min_distance < min_distance)
+                {
+                    min_distance = current_min_distance;
+                    min_element_pair = current_min_element_pair;
+                }
+            }
+#endif
+            // Compute the minimum cluster <-> cluster distance
+#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
+            std::vector<double> per_thread_min_cluster_cluster_distances(num_threads, INFINITY);
+            std::vector<std::pair<int64_t, int64_t>> per_thread_min_cluster_pairs(num_threads, std::make_pair(-1, -1));
+            #pragma omp parallel for
+#else
+            double min_cluster_cluster_distance = INFINITY;
+            std::pair<int64_t, int64_t> min_cluster_pair(-1, -1);
+#endif
+            for (size_t fcdx = 0; fcdx < clusters.size(); fcdx++)
+            {
+                const std::vector<int64_t>& first_cluster = clusters[fcdx];
+                // Don't evaluate empty clusters
+                if (first_cluster.size() > 0)
+                {
+                    for (size_t scdx = 0; scdx < clusters.size(); scdx++)
+                    {
+                        // Don't compare against ourself
+                        if (fcdx != scdx)
+                        {
+                            const std::vector<int64_t>& second_cluster = clusters[scdx];
+                            // Don't evaluate empty clusters
+                            if (second_cluster.size() > 0)
+                            {
+                                // Compute the cluster <-> cluster distance
+                                double max_point_point_distance = 0.0;
+                                // Find the maximum-pointwise distance between clusters
+                                for (size_t fcpx = 0; fcpx < first_cluster.size(); fcpx++)
+                                {
+                                    const int64_t& fcp_index = first_cluster[fcpx];
+                                    for (size_t scpx = 0; scpx < second_cluster.size(); scpx++)
+                                    {
+                                        const int64_t& scp_index = second_cluster[scpx];
+                                        const double& new_distance = distance_matrix(fcp_index, scp_index);
+                                        if (new_distance > max_point_point_distance)
+                                        {
+                                            max_point_point_distance = new_distance;
+                                        }
+                                    }
+                                }
+                                const double cluster_cluster_distance = max_point_point_distance;
+#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
+                                const size_t thread_num = (size_t)omp_get_thread_num();
+                                double& per_thread_min_cluster_cluster_distance = per_thread_min_cluster_cluster_distances[thread_num];
+                                std::pair<int64_t, int64_t>& per_thread_min_cluster_pair = per_thread_min_cluster_pairs[thread_num];
+                                if (cluster_cluster_distance < per_thread_min_cluster_cluster_distance)
+                                {
+                                    per_thread_min_cluster_cluster_distance = cluster_cluster_distance;
+                                    per_thread_min_cluster_pair.first = fcdx;
+                                    per_thread_min_cluster_pair.second = scdx;
+                                }
+#else
+                                if (cluster_cluster_distance < min_cluster_cluster_distance)
+                                {
+                                    min_cluster_cluster_distance = cluster_cluster_distance;
+                                    min_cluster_pair.first = (int64_t)fcdx;
+                                    min_cluster_pair.second = (int64_t)scdx;
+                                }
+#endif
+                            }
+                        }
+                    }
+                }
+            }
+#ifdef ENABLE_PARALLEL_COMPLETE_LINK_CLUSTERING
+            double min_cluster_cluster_distance = INFINITY;
+            std::pair<int64_t, int64_t> min_cluster_pair(-1, -1);
+            for (size_t idx = 0; idx < num_threads; idx++)
+            {
+                const double& current_min_cluster_cluster_distance = per_thread_min_cluster_cluster_distances[idx];
+                const std::pair<int64_t, int64_t>& current_min_cluster_pair = per_thread_min_cluster_pairs[idx];
+                if (current_min_cluster_cluster_distance < min_cluster_cluster_distance)
+                {
+                    min_cluster_cluster_distance = current_min_cluster_cluster_distance;
+                    min_cluster_pair = current_min_cluster_pair;
+                }
+            }
+#endif
+            // Return the minimum-distance pair
+            if (min_distance < min_cluster_cluster_distance)
+            {
+                // Set the indices
+                const std::pair<bool, int64_t> first_index(false, min_element_pair.first);
+                const std::pair<bool, int64_t> second_index = min_element_pair.second;
+                const std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>> indices(first_index, second_index);
+                const std::pair<std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>>, double> minimum_pair(indices, min_distance);
+                return minimum_pair;
+            }
+            // A cluster <-> cluster pair is closest
+            else
+            {
+                // Set the indices
+                const std::pair<bool, int64_t> first_index(true, min_cluster_pair.first);
+                const std::pair<bool, int64_t> second_index(true, min_cluster_pair.second);
+                const std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>> indices(first_index, second_index);
+                const std::pair<std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>>, double> minimum_pair(indices, min_cluster_cluster_distance);
+                return minimum_pair;
+            }
+        }
+
+    public:
+
+        template<typename Datatype, typename Allocator=std::allocator<Datatype>>
+        static std::pair<std::vector<std::vector<Datatype, Allocator>>, double> Cluster(const std::vector<Datatype, Allocator>& data, const std::function<double(const Datatype&, const Datatype&)>& distance_fn, const double max_cluster_distance)
+        {
+            const Eigen::MatrixXd distance_matrix = arc_helpers::BuildDistanceMatrix(data, distance_fn);
+            return Cluster(data, distance_matrix, max_cluster_distance);
+        }
+
+        template<typename Datatype, typename Allocator=std::allocator<Datatype>>
+        static std::pair<std::vector<std::vector<Datatype, Allocator>>, double> Cluster(const std::vector<Datatype, Allocator>& data, const Eigen::MatrixXd& distance_matrix, const double max_cluster_distance)
+        {
+            assert((size_t)distance_matrix.rows() == data.size());
+            assert((size_t)distance_matrix.cols() == data.size());
+            std::vector<uint8_t> datapoint_mask(data.size(), 0u);
+            std::vector<std::vector<int64_t>> cluster_indices;
+            double closest_distance = 0.0;
+            bool complete = false;
+            while (!complete)
+            {
+                // Get closest pair of elements (an element can be a cluster or single data value!)
+                const std::pair<std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>>, double> closest_element_pair = GetClosestPair(datapoint_mask, distance_matrix, cluster_indices);
+                const std::pair<std::pair<bool, int64_t>, std::pair<bool, int64_t>>& closest_elements = closest_element_pair.first;
+                closest_distance = closest_element_pair.second;
+                //std::cout << "Element pair: " << PrettyPrint::PrettyPrint(closest_element_pair, true) << std::endl;
+                if (closest_distance <= max_cluster_distance)
+                {
+                    const std::pair<bool, int64_t>& first_element = closest_elements.first;
+                    const std::pair<bool, int64_t>& second_element = closest_elements.second;
+                    // If both elements are points, create a new cluster
+                    if ((first_element.first == false) && (second_element.first == false))
+                    {
+                        //std::cout << "New point-point cluster" << std::endl;
+                        const int64_t first_element_index = first_element.second;
+                        assert(first_element_index >= 0);
+                        const int64_t second_element_index = second_element.second;
+                        assert(second_element_index >= 0);
+                        // Add a cluster
+                        cluster_indices.push_back(std::vector<int64_t>{first_element_index, second_element_index});
+                        // Mask out the indices
+                        datapoint_mask[(size_t)first_element_index] = 1u;
+                        datapoint_mask[(size_t)second_element_index] = 1u;
+                    }
+                    // If both elements are clusters, merge the clusters
+                    else if ((first_element.first == true) && (second_element.first == true))
+                    {
+                        //std::cout << "Combining clusters" << std::endl;
+                        // Get the cluster indices
+                        const int64_t first_cluster_index = first_element.second;
+                        assert(first_cluster_index >= 0);
+                        const int64_t second_cluster_index = second_element.second;
+                        assert(second_cluster_index >= 0);
+                        // Merge the second cluster into the first
+                        std::vector<int64_t>& first_cluster = cluster_indices[(size_t)first_cluster_index];
+                        std::vector<int64_t>& second_cluster = cluster_indices[(size_t)second_cluster_index];
+                        first_cluster.insert(first_cluster.end(), second_cluster.begin(), second_cluster.end());
+                        // Empty the second cluster (we don't remove, because this triggers move)
+                        second_cluster.clear();
+                    }
+                    // If one of the elements is a cluster and the other is a point, add the point to the existing cluster
+                    else
+                    {
+                        //std::cout << "Adding to an existing cluster" << std::endl;
+                        int64_t cluster_index = -1;
+                        int64_t element_index = -1;
+                        if (first_element.first)
+                        {
+                            cluster_index = first_element.second;
+                            element_index = second_element.second;
+                        }
+                        else if (second_element.first)
+                        {
+                            cluster_index = second_element.second;
+                            element_index = first_element.second;
+                        }
+                        else
+                        {
+                            assert(false);
+                        }
+                        assert(cluster_index >= 0);
+                        assert(element_index >= 0);
+                        // Add the element to the cluster
+                        std::vector<int64_t>& cluster = cluster_indices[(size_t)cluster_index];
+                        cluster.push_back(element_index);
+                        // Mask out the element index
+                        datapoint_mask[(size_t)element_index] = 1u;
+                    }
+                }
+                else
+                {
+                    complete = true;
+                }
+            }
+            // Extract the actual cluster data
+            std::vector<std::vector<Datatype, Allocator>> clusters;
+            for (size_t idx = 0; idx < cluster_indices.size(); idx++)
+            {
+                const std::vector<int64_t>& current_cluster = cluster_indices[idx];
+                // Ignore empty clusters
+                if (current_cluster.size() > 0)
+                {
+                    std::vector<Datatype, Allocator> new_cluster;
+                    for (size_t cdx = 0; cdx < current_cluster.size(); cdx++)
+                    {
+                        const int64_t index = current_cluster[cdx];
+                        new_cluster.push_back(data[(size_t)index]);
+                    }
+                    clusters.push_back(new_cluster);
+                }
+            }
+            // Add any points that we haven't clustered into their own clusters
+            for (size_t idx = 0; idx < datapoint_mask.size(); idx++)
+            {
+                // If an element hasn't been clustered at all
+                if (datapoint_mask[idx] == 0)
+                {
+                    clusters.push_back(std::vector<Datatype, Allocator>{data[idx]});
+                }
+            }
+            return std::pair<std::vector<std::vector<Datatype, Allocator>>, double>(clusters, closest_distance);
+        }
+    };
+}
+#endif // SIMPLE_HIERARCHICAL_CLUSTERING_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/simple_kmeans_clustering.hpp

@@ -0,0 +1,398 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <vector>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <stdexcept>
+#include <functional>
+#include <chrono>
+#include <random>
+#include <Eigen/Geometry>
+
+#ifndef SIMPLE_KMEANS_CLUSTERING_HPP
+#define SIMPLE_KMEANS_CLUSTERING_HPP
+
+namespace simple_kmeans_clustering
+{
+    class SimpleKMeansClustering
+    {
+    private:
+
+        SimpleKMeansClustering() {}
+
+        template<typename Datatype, typename Allocator = std::allocator<Datatype>>
+        static uint32_t GetClosestCluster(
+                const Datatype& datapoint,
+                const std::function<double(const Datatype&, const Datatype&)>& distance_fn,
+                const std::vector<Datatype, Allocator>& cluster_centers)
+        {
+            int64_t best_label = -1;
+            double best_distance = INFINITY;
+            for (size_t cluster = 0; cluster < cluster_centers.size(); cluster++)
+            {
+                const Datatype& cluster_center = cluster_centers[cluster];
+                const double distance = distance_fn(cluster_center, datapoint);
+                if (distance < best_distance)
+                {
+                    best_distance = distance;
+                    best_label = (int64_t)cluster;
+                }
+            }
+            assert(best_label >= 0);
+            return (uint32_t)best_label;
+        }
+
+        template<typename Datatype, typename Allocator = std::allocator<Datatype>>
+        static std::vector<uint32_t> PerformSingleClusteringIteration(
+                const std::vector<Datatype, Allocator>& data,
+                const std::function<double(const Datatype&, const Datatype&)>& distance_fn,
+                const std::vector<Datatype, Allocator>& cluster_centers)
+        {
+            std::vector<uint32_t> cluster_labels(data.size());
+            for (size_t idx = 0; idx < data.size(); idx++)
+            {
+                const Datatype& datapoint = data[idx];
+                const uint32_t label = GetClosestCluster(datapoint, distance_fn, cluster_centers);
+                cluster_labels[idx] = label;
+            }
+            return cluster_labels;
+        }
+
+        template<typename Datatype, typename Allocator = std::allocator<Datatype>>
+        static std::vector<Datatype, Allocator> ComputeClusterCenters(
+                const std::vector<Datatype, Allocator>& data,
+                const std::vector<uint32_t>& cluster_labels,
+                const std::function<Datatype(const std::vector<Datatype, Allocator>&)>& average_fn,
+                const uint32_t num_clusters)
+        {
+            assert(data.size() == cluster_labels.size());
+
+            // Separate the datapoints into their clusters
+            std::vector<std::vector<Datatype, Allocator>> clustered_data(num_clusters);
+            for (size_t idx = 0; idx < data.size(); idx++)
+            {
+                const Datatype& datapoint = data[idx];
+                const uint32_t label = cluster_labels[idx];
+                clustered_data[label].push_back(datapoint);
+            }
+
+            // Compute the center of each cluster
+            std::vector<Datatype, Allocator> cluster_centers(num_clusters);
+            for (uint32_t cluster = 0; cluster < num_clusters; cluster++)
+            {
+                const std::vector<Datatype, Allocator>& cluster_data = clustered_data[cluster];
+                cluster_centers[cluster] = average_fn(cluster_data);
+            }
+            return cluster_centers;
+        }
+
+        template<typename Datatype, typename Allocator = std::allocator<Datatype>>
+        static std::vector<Datatype, Allocator> ComputeClusterCentersWeighted(
+                const std::vector<Datatype, Allocator>& data,
+                const std::vector<double>& weights,
+                const std::vector<uint32_t>& cluster_labels,
+                const std::function<Datatype(const std::vector<Datatype, Allocator>&, const std::vector<double>&)>& average_fn,
+                const uint32_t num_clusters)
+        {
+            assert(data.size() == cluster_labels.size());
+
+            // Separate the datapoints into their clusters
+            std::vector<std::vector<Datatype, Allocator>> clustered_data(num_clusters);
+            std::vector<std::vector<double>> clustered_weights(num_clusters);
+            for (size_t idx = 0; idx < data.size(); idx++)
+            {
+                const Datatype& datapoint = data[idx];
+                const double weight = weights[idx];
+                const uint32_t label = cluster_labels[idx];
+
+                clustered_data[label].push_back(datapoint);
+                clustered_weights[label].push_back(weight);
+            }
+
+            // Compute the center of each cluster
+            std::vector<Datatype, Allocator> cluster_centers(num_clusters);
+            for (uint32_t cluster = 0; cluster < num_clusters; cluster++)
+            {
+                const std::vector<Datatype, Allocator>& cluster_data = clustered_data[cluster];
+                const std::vector<double>& cluster_weights = clustered_weights[cluster];
+                cluster_centers[cluster] = average_fn(cluster_data, cluster_weights);
+            }
+            return cluster_centers;
+        }
+
+        static bool CheckForConvergence(
+                const std::vector<uint32_t>& old_labels,
+                const std::vector<uint32_t>& new_labels)
+        {
+            assert(old_labels.size() == new_labels.size());
+            for (size_t idx = 0; idx < old_labels.size(); idx++)
+            {
+                const uint32_t old_label = old_labels[idx];
+                const uint32_t new_label = new_labels[idx];
+                if (old_label != new_label)
+                {
+                    return false;
+                }
+            }
+            return true;
+        }
+
+    public:
+
+        template<typename Datatype, typename Allocator = std::allocator<Datatype>>
+        static std::pair<std::vector<uint32_t>, std::vector<Datatype, Allocator>> Cluster(
+                const std::vector<Datatype, Allocator>& data,
+                const std::function<double(const Datatype&, const Datatype&)>& distance_fn,
+                const std::function<Datatype(const std::vector<Datatype, Allocator>&)>& average_fn,
+                std::vector<Datatype, Allocator> cluster_centers)
+        {
+            const uint32_t num_clusters = cluster_centers.size();
+            assert(num_clusters < std::numeric_limits<uint32_t>::max());
+
+            // Run the first iteration of clustering
+            std::vector<uint32_t> cluster_labels = PerformSingleClusteringIteration(data, distance_fn, cluster_centers);
+
+            // Itterate until converged
+            bool converged = false;
+            uint32_t iteration = 1u;
+            while (!converged)
+            {
+                // Update cluster centers
+                cluster_centers = ComputeClusterCenters(data, cluster_labels, average_fn, num_clusters);
+                // Cluster with the new centers
+                std::vector<uint32_t> new_cluster_labels = PerformSingleClusteringIteration(data, distance_fn, cluster_centers);
+                // Check for convergence
+                converged = CheckForConvergence(cluster_labels, new_cluster_labels);
+                cluster_labels = new_cluster_labels;
+                iteration++;
+            }
+            std::cerr << "[K-means clustering] Clustering converged after " << iteration << " iterations" << std::endl;
+            return std::make_pair(cluster_labels, cluster_centers);
+        }
+
+        template<typename Datatype, typename Allocator = std::allocator<Datatype>>
+        static std::vector<uint32_t> Cluster(
+                const std::vector<Datatype, Allocator>& data,
+                const std::function<double(const Datatype&, const Datatype&)>& distance_fn,
+                const std::function<Datatype(const std::vector<Datatype, Allocator>&)>& average_fn,
+                const uint32_t num_clusters,
+                const bool do_preliminary_clustering = false)
+        {
+            assert(data.size() > 0);
+            assert(num_clusters > 0);
+
+            if (num_clusters == 1)
+            {
+                std::cerr << "[K-means clustering] Provided num_clusters = 1, returning default labels for cluster 0" << std::endl;
+                return std::vector<uint32_t>(data.size(), 0u);
+            }
+
+            // Prepare an RNG for cluster initialization
+            auto seed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
+            std::mt19937_64 prng(seed);
+            std::uniform_int_distribution<size_t> initialization_distribution(0u, data.size() - 1);
+            // Initialize cluster centers
+            std::vector<Datatype, Allocator> cluster_centers;
+
+            // Make sure we have enough datapoints to do meaningful preliminary clustering
+            bool enable_preliminary_clustering = do_preliminary_clustering;
+            if (enable_preliminary_clustering)
+            {
+                const size_t subset_size = (size_t)ceil((double)data.size() * 0.1);
+                if (subset_size >= (num_clusters * 5))
+                {
+                    enable_preliminary_clustering = true;
+                    std::cerr << "[K-means clustering] Preliminary clustering enabled, using subset of " << subset_size << " datapoints from " << data.size() << " total" << std::endl;
+                }
+                else
+                {
+                    enable_preliminary_clustering = false;
+                    std::cerr << "[K-means clustering] Preliminary clustering disabled as input data is too small w.r.t. number of clusters" << std::endl;
+                }
+            }
+
+            if (enable_preliminary_clustering)
+            {
+                // Select a random 10% of the input data
+                const size_t subset_size = (size_t)ceil((double)data.size() * 0.1);
+                // This makes sure we don't get duplicates
+                std::map<size_t, uint8_t> index_map;
+                while (index_map.size() < subset_size)
+                {
+                    const size_t random_index = initialization_distribution(prng);
+                    index_map[random_index] = 1u;
+                }
+                std::vector<Datatype, Allocator> random_subset;
+                random_subset.reserve(subset_size);
+                for (auto itr = index_map.begin(); itr != index_map.end(); ++itr)
+                {
+                    if (itr->second == 1u)
+                    {
+                        const size_t random_index = itr->first;
+                        const Datatype& random_element = data[random_index];
+                        random_subset.push_back(random_element);
+                    }
+                }
+                assert(random_subset.size() == subset_size);
+                // Run clustering on the subset
+                std::vector<uint32_t> random_subset_labels = Cluster(random_subset, distance_fn, average_fn, num_clusters, false);
+                // Now we use the centers of the clusters to form the cluster centers
+                cluster_centers = ComputeClusterCenters(random_subset, random_subset_labels, average_fn, num_clusters);
+            }
+            else
+            {
+                // This makes sure we don't get duplicates
+                std::map<size_t, uint8_t> index_map;
+                while (index_map.size() < num_clusters)
+                {
+                    const size_t random_index = initialization_distribution(prng);
+                    index_map[random_index] = 1u;
+                }
+                cluster_centers.reserve(num_clusters);
+                for (auto itr = index_map.begin(); itr != index_map.end(); ++itr)
+                {
+                    if (itr->second == 1u)
+                    {
+                        const size_t random_index = itr->first;
+                        const Datatype& random_element = data[random_index];
+                        cluster_centers.push_back(random_element);
+                    }
+                }
+            }
+
+            assert(cluster_centers.size() == num_clusters);
+            return Cluster(data, distance_fn, average_fn, cluster_centers).first;
+        }
+
+
+
+        template<typename Datatype, typename Allocator = std::allocator<Datatype>>
+        static std::pair<std::vector<uint32_t>, std::vector<Datatype, Allocator>> ClusterWeighted(
+                const std::vector<Datatype, Allocator>& data,
+                const std::vector<double>& weights,
+                const std::function<double(const Datatype&, const Datatype&)>& distance_fn,
+                const std::function<Datatype(const std::vector<Datatype, Allocator>&, const std::vector<double>&)>& average_fn,
+                std::vector<Datatype, Allocator> cluster_centers)
+        {
+            const uint32_t num_clusters = cluster_centers.size();
+            assert(num_clusters < std::numeric_limits<uint32_t>::max());
+
+            // Run the first iteration of clustering
+            std::vector<uint32_t> cluster_labels = PerformSingleClusteringIteration(data, distance_fn, cluster_centers);
+
+            // Itterate until converged
+            bool converged = false;
+            uint32_t iteration = 1u;
+            while (!converged)
+            {
+                // Update cluster centers
+                cluster_centers = ComputeClusterCentersWeighted(data, weights, cluster_labels, average_fn, num_clusters);
+                // Cluster with the new centers
+                std::vector<uint32_t> new_cluster_labels = PerformSingleClusteringIteration(data, distance_fn, cluster_centers);
+                // Check for convergence
+                converged = CheckForConvergence(cluster_labels, new_cluster_labels);
+                cluster_labels = new_cluster_labels;
+                iteration++;
+            }
+            std::cerr << "[K-means clustering] Clustering converged after " << iteration << " iterations" << std::endl;
+            return std::make_pair(cluster_labels, cluster_centers);
+        }
+
+        template<typename Datatype, typename Allocator = std::allocator<Datatype>>
+        static std::vector<uint32_t> ClusterWeighted(
+                const std::vector<Datatype, Allocator>& data,
+                const std::vector<double>& weights,
+                const std::function<double(const Datatype&, const Datatype&)>& distance_fn,
+                const std::function<Datatype(const std::vector<Datatype, Allocator>&, const std::vector<double>&)>& average_fn,
+                const uint32_t num_clusters,
+                const bool do_preliminary_clustering = false)
+        {
+            assert(data.size() > 0);
+            assert(num_clusters > 0);
+
+            if (num_clusters == 1)
+            {
+                std::cerr << "[K-means clustering] Provided num_clusters = 1, returning default labels for cluster 0" << std::endl;
+                return std::vector<uint32_t>(data.size(), 0u);
+            }
+
+            // Prepare an RNG for cluster initialization
+            auto seed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
+            std::mt19937_64 prng(seed);
+            std::uniform_int_distribution<size_t> initialization_distribution(0u, data.size() - 1);
+            // Initialize cluster centers
+            std::vector<Datatype, Allocator> cluster_centers;
+
+            // Make sure we have enough datapoints to do meaningful preliminary clustering
+            bool enable_preliminary_clustering = do_preliminary_clustering;
+            if (enable_preliminary_clustering)
+            {
+                const size_t subset_size = (size_t)ceil((double)data.size() * 0.1);
+                if (subset_size >= (num_clusters * 5))
+                {
+                    enable_preliminary_clustering = true;
+                    std::cerr << "[K-means clustering] Preliminary clustering enabled, using subset of " << subset_size << " datapoints from " << data.size() << " total" << std::endl;
+                }
+                else
+                {
+                    enable_preliminary_clustering = false;
+                    std::cerr << "[K-means clustering] Preliminary clustering disabled as input data is too small w.r.t. number of clusters" << std::endl;
+                }
+            }
+
+            if (enable_preliminary_clustering)
+            {
+                // Select a random 10% of the input data
+                const size_t subset_size = (size_t)ceil((double)data.size() * 0.1);
+                // This makes sure we don't get duplicates
+                std::map<size_t, uint8_t> index_map;
+                while (index_map.size() < subset_size)
+                {
+                    const size_t random_index = initialization_distribution(prng);
+                    index_map[random_index] = 1u;
+                }
+                std::vector<Datatype, Allocator> random_subset;
+                random_subset.reserve(subset_size);
+                for (auto itr = index_map.begin(); itr != index_map.end(); ++itr)
+                {
+                    if (itr->second == 1u)
+                    {
+                        const size_t random_index = itr->first;
+                        const Datatype& random_element = data[random_index];
+                        random_subset.push_back(random_element);
+                    }
+                }
+                assert(random_subset.size() == subset_size);
+                // Run clustering on the subset
+                std::vector<uint32_t> random_subset_labels = ClusterWeighted(random_subset, weights, distance_fn, average_fn, num_clusters, false);
+                // Now we use the centers of the clusters to form the cluster centers
+                cluster_centers = ComputeClusterCentersWeighted(random_subset, weights, random_subset_labels, average_fn, num_clusters);
+            }
+            else
+            {
+                // This makes sure we don't get duplicates
+                std::map<size_t, uint8_t> index_map;
+                while (index_map.size() < num_clusters)
+                {
+                    const size_t random_index = initialization_distribution(prng);
+                    index_map[random_index] = 1u;
+                }
+                cluster_centers.reserve(num_clusters);
+                for (auto itr = index_map.begin(); itr != index_map.end(); ++itr)
+                {
+                    if (itr->second == 1u)
+                    {
+                        const size_t random_index = itr->first;
+                        const Datatype& random_element = data[random_index];
+                        cluster_centers.push_back(random_element);
+                    }
+                }
+            }
+
+            assert(cluster_centers.size() == num_clusters);
+            return ClusterWeighted(data, weights, distance_fn, average_fn, cluster_centers).first;
+        }
+    };
+}
+#endif // SIMPLE_KMEANS_CLUSTERING_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/simple_prm_planner.hpp

@@ -0,0 +1,450 @@
+#include <stdlib.h>
+#include <functional>
+#include <omp.h>
+#ifdef ENABLE_PARALLEL_ROADMAP
+    #ifndef ENABLE_PARALLEL_K_NEAREST_NEIGHBORS
+        #define ENABLE_PARALLEL_K_NEAREST_NEIGHBORS
+        #include <arc_utilities/arc_helpers.hpp>
+        #undef ENABLE_PARALLEL_K_NEAREST_NEIGHBORS
+    #else
+        #include <arc_utilities/arc_helpers.hpp>
+    #endif
+#else
+    #include <arc_utilities/arc_helpers.hpp>
+#endif
+#include <arc_utilities/dijkstras.hpp>
+
+
+#ifndef SIMPLE_PRM_PLANNER_HPP
+#define SIMPLE_PRM_PLANNER_HPP
+
+namespace simple_prm_planner
+{
+    class SimpleGeometricPrmPlanner
+    {
+    protected:
+
+        SimpleGeometricPrmPlanner() {}
+
+        enum NNDistanceDirection {ROADMAP_TO_NEW_STATE, NEW_STATE_TO_ROADMAP};
+
+        template<typename T, typename Allocator = std::allocator<T>>
+        static int64_t AddNodeToRoadmap(
+                const T& state,
+                const NNDistanceDirection nn_distance_direction,
+                arc_dijkstras::Graph<T, Allocator>& roadmap,
+                const std::function<double(const T&, const T&)>& distance_fn,
+                const std::function<bool(const T&, const T&)>& edge_validity_check_fn,
+                const size_t K,
+                const bool distance_is_symmetric = true)
+        {
+            // Make the node->graph or graph->node distance function as needed, then call KNN
+            std::function<double(const arc_dijkstras::GraphNode<T, Allocator>&, const T&)> graph_distance_fn = nullptr;
+            if (nn_distance_direction == ROADMAP_TO_NEW_STATE)
+            {
+                graph_distance_fn = [&] (const arc_dijkstras::GraphNode<T, Allocator>& node, const T& state)
+                {
+                    return distance_fn(node.GetValueImmutable(), state);
+                };
+            }
+            else
+            {
+                graph_distance_fn = [&] (const arc_dijkstras::GraphNode<T, Allocator>& node, const T& state)
+                {
+                    return distance_fn(state, node.GetValueImmutable());
+                };
+            }
+            const std::vector<std::pair<int64_t, double>> nearest_neighbors =
+                    arc_helpers::GetKNearestNeighbors(roadmap.GetNodesImmutable(), state, graph_distance_fn, K);
+
+            // Check to see this node is already in the PRM, if it is, don't re-add it, returning the existing state index
+            for (const auto& nn_result : nearest_neighbors)
+            {
+                // If the distance is zero, check that the states are also equal
+                // This allows for a psuedo-metric and not just a true metric to be used as the distance function
+                if (nn_result.second == 0)
+                {
+                    const int64_t neighbour_node_idx = nn_result.first;
+                    const T& neighbour_state = roadmap.GetNodeImmutable(neighbour_node_idx).GetValueImmutable();
+                    if (neighbour_state == state)
+                    {
+                        return neighbour_node_idx;
+                    }
+                }
+            }
+
+            // Add the new node AFTER KNN is performed
+            const int64_t new_node_index = roadmap.AddNode(state);
+            // Parallelize the collision-checking and distance computation
+            std::vector<std::pair<double, double>> nearest_neighbors_distances(nearest_neighbors.size());
+#ifdef ENABLE_PARALLEL_ROADMAP
+            #pragma omp parallel for
+#endif
+            for (size_t idx = 0; idx < nearest_neighbors.size(); idx++)
+            {
+                const std::pair<int64_t, double>& nearest_neighbor = nearest_neighbors[idx];
+                const int64_t nearest_neighbor_index = nearest_neighbor.first;
+                const double nearest_neighbor_distance = nearest_neighbor.second;
+                const T& nearest_neighbor_state = roadmap.GetNodeImmutable(nearest_neighbor_index).GetValueImmutable();
+                if (edge_validity_check_fn(nearest_neighbor_state, state))
+                {
+                    if (distance_is_symmetric)
+                    {
+                        nearest_neighbors_distances[idx] = std::make_pair(nearest_neighbor_distance, nearest_neighbor_distance);
+                    }
+                    else
+                    {
+                        const double reverse_graph_distance = distance_fn(state, roadmap.GetNodeImmutable(nearest_neighbor_index).GetValueImmutable());
+                        if (nn_distance_direction == ROADMAP_TO_NEW_STATE)
+                        {
+                            nearest_neighbors_distances[idx] = std::make_pair(nearest_neighbor_distance, reverse_graph_distance);
+                        }
+                        else if (nn_distance_direction == ROADMAP_TO_NEW_STATE)
+                        {
+                            nearest_neighbors_distances[idx] = std::make_pair(reverse_graph_distance, nearest_neighbor_distance);
+                        }
+                        else
+                        {
+                            assert(false && "This code should not be reachable");
+                        }
+                    }
+                }
+                else
+                {
+                    nearest_neighbors_distances[idx] = std::make_pair(-1.0, -1.0);
+                }
+            }
+            // THIS MUST BE SERIAL - add edges to roadmap
+            for (size_t idx = 0; idx < nearest_neighbors.size(); idx++)
+            {
+                const std::pair<int64_t, double>& nearest_neighbor = nearest_neighbors[idx];
+                const int64_t nearest_neighbor_index = nearest_neighbor.first;
+                const std::pair<double, double>& nearest_neighbor_distances = nearest_neighbors_distances[idx];
+                if (nearest_neighbor_distances.first >= 0.0 && nearest_neighbor_distances.second >= 0.0)
+                {
+                    // Add the edges individually to allow for different distances in each direction
+                    roadmap.AddEdgeBetweenNodes(nearest_neighbor_index, new_node_index, nearest_neighbor_distances.first);
+                    roadmap.AddEdgeBetweenNodes(new_node_index, nearest_neighbor_index, nearest_neighbor_distances.second);
+                }
+            }
+            return new_node_index;
+        }
+
+        template<typename T, typename Allocator = std::allocator<T>>
+        static std::vector<T, Allocator> ExtractSolutionPath(
+                const arc_dijkstras::Graph<T, Allocator>& roadmap,
+                const std::vector<int64_t>& solution_path_indices)
+        {
+            std::vector<T, Allocator> solution_path;
+            solution_path.reserve(solution_path_indices.size());
+            for (size_t idx = 0; idx < solution_path_indices.size(); idx++)
+            {
+                const int64_t path_index = solution_path_indices[idx];
+                solution_path.push_back(roadmap.GetNodeImmutable(path_index).GetValueImmutable());
+            }
+            solution_path.shrink_to_fit();
+            return solution_path;
+        }
+
+    public:
+
+        template<typename T, typename Allocator = std::allocator<T>>
+        static void ExtendRoadMap(
+                arc_dijkstras::Graph<T, Allocator>& roadmap,
+                const std::function<T(void)>& sampling_fn,
+                const std::function<double(const T&, const T&)>& distance_fn,
+                const std::function<bool(const T&)>& state_validity_check_fn,
+                const std::function<bool(const T&, const T&)>& edge_validity_check_fn,
+                const std::function<bool(void)>& termination_check_fn,
+                const size_t K,
+                const bool distance_is_symmetric = true)
+        {
+            while (!termination_check_fn())
+            {
+                const T random_state = sampling_fn();
+                if (state_validity_check_fn(random_state))
+                {
+                    AddNodeToRoadmap(random_state, ROADMAP_TO_NEW_STATE, roadmap, distance_fn, edge_validity_check_fn, K, distance_is_symmetric);
+                }
+            }
+        }
+
+        template<typename T, typename Allocator = std::allocator<T>>
+        static arc_dijkstras::Graph<T, Allocator> BuildRoadMap(
+                const std::function<T(void)>& sampling_fn,
+                const std::function<double(const T&, const T&)>& distance_fn,
+                const std::function<bool(const T&)>& state_validity_check_fn,
+                const std::function<bool(const T&, const T&)>& edge_validity_check_fn,
+                const std::function<bool(void)>& termination_check_fn,
+                const size_t K,
+                const bool distance_is_symmetric = true)
+        {
+            arc_dijkstras::Graph<T, Allocator> roadmap;
+            ExtendRoadMap(roadmap, sampling_fn, distance_fn, state_validity_check_fn, edge_validity_check_fn, termination_check_fn, K, distance_is_symmetric);
+            return roadmap;
+        }
+
+        template<typename T, typename Allocator = std::allocator<T>>
+        static void UpdateRoadMapEdges(
+                arc_dijkstras::Graph<T, Allocator>& roadmap,
+                const std::function<bool(const T&, const T&)>& edge_validity_check_fn,
+                const std::function<double(const T&, const T&)>& distance_fn)
+        {
+            assert(roadmap.CheckGraphLinkage());
+#ifdef ENABLE_PARALLEL_ROADMAP
+            #pragma omp parallel for
+#endif
+            for (size_t current_node_index = 0; current_node_index < roadmap.GetNodesImmutable().size(); current_node_index++)
+            {
+                arc_dijkstras::GraphNode<T, Allocator>& current_node = roadmap.GetNodeMutable(current_node_index);
+                std::vector<arc_dijkstras::GraphEdge>& current_node_out_edges = current_node.GetOutEdgesMutable();
+                for (size_t out_edge_idx = 0; out_edge_idx < current_node_out_edges.size(); out_edge_idx++)
+                {
+                    arc_dijkstras::GraphEdge& current_out_edge = current_node_out_edges[out_edge_idx];
+                    const int64_t other_node_idx = current_out_edge.GetToIndex();
+                    arc_dijkstras::GraphNode<T, Allocator>& other_node = roadmap.GetNodeMutable(other_node_idx);
+                    std::vector<arc_dijkstras::GraphEdge>& other_node_in_edges = other_node.GetInEdgesMutable();
+                    // If the edge is not valid, set the weight to infinity, otherwise use the distance function
+                    double updated_weight = std::numeric_limits<double>::infinity();
+                    if (edge_validity_check_fn(current_node.GetValueImmutable(), other_node.GetValueImmutable()))
+                    {
+                        updated_weight = distance_fn(current_node.GetValueImmutable(), other_node.GetValueImmutable());
+                    }
+                    // Update our out edge
+                    current_out_edge.SetWeight(updated_weight);
+                    // Update the other node's in edges
+                    for (size_t in_edge_idx = 0; in_edge_idx < other_node_in_edges.size(); in_edge_idx++)
+                    {
+                        arc_dijkstras::GraphEdge& other_in_edge = other_node_in_edges[in_edge_idx];
+                        if (other_in_edge.GetFromIndex() == current_node_index)
+                        {
+                            other_in_edge.SetWeight(updated_weight);
+                        }
+                    }
+                }
+            }
+        }
+
+        template<typename T, typename Allocator = std::allocator<T>>
+        static std::pair<std::vector<T, Allocator>, double> QueryPathAndAddNodesMultiStartSingleGoal(
+                const std::vector<T, Allocator>& starts,
+                const T& goal,
+                arc_dijkstras::Graph<T, Allocator>& roadmap,
+                const std::function<bool(const T&, const T&)>& edge_validity_check_fn,
+                const std::function<double(const T&, const T&)>& distance_fn,
+                const size_t K,
+                const bool distance_is_symmetric = true)
+        {
+            // Add the multiple start nodes to the roadmap
+            std::vector<int64_t> start_node_indices(starts.size());
+            for (size_t start_idx = 0; start_idx < starts.size(); start_idx++)
+            {
+                const T& start = starts[start_idx];
+                start_node_indices[start_idx] = AddNodeToRoadmap(start, NEW_STATE_TO_ROADMAP, roadmap, distance_fn, edge_validity_check_fn, K, distance_is_symmetric);
+            }
+            // Add the goal node to the roadmap
+            const int64_t goal_node_index = AddNodeToRoadmap(goal, ROADMAP_TO_NEW_STATE, roadmap, distance_fn, edge_validity_check_fn, K, distance_is_symmetric);
+            // Call Dijkstra's
+            const auto dijkstras_solution = arc_dijkstras::SimpleDijkstrasAlgorithm<T, Allocator>::PerformDijkstrasAlgorithm(roadmap, goal_node_index);
+            // Identify the lowest-distance starting state
+            const std::pair<std::vector<int64_t>, std::vector<double>>& solution_map_distances = dijkstras_solution.second;
+            double best_start_node_distance = std::numeric_limits<double>::infinity();
+            int64_t best_start_node_index = -1;
+            for (size_t start_idx = 0; start_idx < starts.size(); start_idx++)
+            {
+                const int64_t start_node_index = start_node_indices[start_idx];
+                const double start_node_distance = solution_map_distances.second[start_node_index];
+                if (start_node_distance < best_start_node_distance)
+                {
+                    best_start_node_distance = start_node_distance;
+                    best_start_node_index = start_node_index;
+                }
+            }
+            const int64_t start_node_index = best_start_node_index;
+            const double start_node_distance = best_start_node_distance;
+            // Extract solution path
+            if (std::isinf(start_node_distance))
+            {
+                return std::make_pair(std::vector<T, Allocator>(), std::numeric_limits<double>::infinity());
+            }
+            else
+            {
+                std::vector<int64_t> solution_path_indices;
+                solution_path_indices.push_back(start_node_index);
+                int64_t previous_index = solution_map_distances.first[start_node_index];
+                while (previous_index >= 0)
+                {
+                    const int64_t current_index = previous_index;
+                    solution_path_indices.push_back(current_index);
+                    if (current_index == goal_node_index)
+                    {
+                        break;
+                    }
+                    else
+                    {
+                        previous_index = solution_map_distances.first[current_index];
+                    }
+                }
+                const std::vector<T, Allocator> solution_path = ExtractSolutionPath(roadmap, solution_path_indices);
+                return std::make_pair(solution_path, start_node_distance);
+            }
+        }
+
+        template<typename T, typename Allocator = std::allocator<T>>
+        static std::pair<std::vector<T, Allocator>, double> QueryPathAndAddNodesSingleStartSingleGoal(
+                const T& start,
+                const T& goal,
+                arc_dijkstras::Graph<T, Allocator>& roadmap,
+                const std::function<bool(const T&, const T&)>& edge_validity_check_fn,
+                const std::function<double(const T&, const T&)>& distance_fn,
+                const size_t K,
+                const bool distance_is_symmetric = true,
+                const bool limit_astar_pqueue_duplicates = true)
+        {
+            // Add the start node to the roadmap
+            const int64_t start_node_index = AddNodeToRoadmap(start, NEW_STATE_TO_ROADMAP, roadmap, distance_fn, edge_validity_check_fn, K, distance_is_symmetric);
+            // Add the goal node to the roadmap
+            const int64_t goal_node_index = AddNodeToRoadmap(goal, ROADMAP_TO_NEW_STATE, roadmap, distance_fn, edge_validity_check_fn, K, distance_is_symmetric);
+            // Call graph A*
+            const std::pair<std::vector<int64_t>, double> astar_result = arc_dijkstras::SimpleGraphAstar<T, Allocator>::PerformAstar(
+                        roadmap, start_node_index, goal_node_index, distance_fn, limit_astar_pqueue_duplicates);
+            // Convert the solution path from A* provided as indices into real states
+            const std::vector<int64_t>& solution_path_indices = astar_result.first;
+            std::vector<T, Allocator> solution_path;
+            solution_path.reserve(astar_result.first.size());
+            for (size_t idx = 0; idx < solution_path_indices.size(); idx++)
+            {
+                const int64_t path_index = solution_path_indices[idx];
+                solution_path.push_back(roadmap.GetNodeImmutable(path_index).GetValueImmutable());
+            }
+            solution_path.shrink_to_fit();
+            return std::make_pair(solution_path, astar_result.second);
+        }
+
+        template<typename T, typename Allocator = std::allocator<T>>
+        static std::pair<std::vector<T, Allocator>, double> LazyQueryPathAndAddNodesSingleStartSingleGoal(
+                const T& start,
+                const T& goal,
+                arc_dijkstras::Graph<T, Allocator>& roadmap,
+                const std::function<bool(const T&, const T&)>& edge_validity_check_fn,
+                const std::function<double(const T&, const T&)>& distance_fn,
+                const size_t K, const bool distance_is_symmetric=true,
+                const bool limit_astar_pqueue_duplicates = true)
+        {
+            // Add the start node to the roadmap
+            const int64_t start_node_index = AddNodeToRoadmap(start, NEW_STATE_TO_ROADMAP, roadmap, distance_fn, edge_validity_check_fn, K, distance_is_symmetric);
+            // Add the goal node to the roadmap
+            const int64_t goal_node_index = AddNodeToRoadmap(goal, ROADMAP_TO_NEW_STATE, roadmap, distance_fn, edge_validity_check_fn, K, distance_is_symmetric);
+            // Call graph A*
+            const std::pair<std::vector<int64_t>, double> astar_result = arc_dijkstras::SimpleGraphAstar<T, Allocator>::PerformLazyAstar(
+                        roadmap, start_node_index, goal_node_index, edge_validity_check_fn, distance_fn, distance_fn, limit_astar_pqueue_duplicates);
+            // Convert the solution path from A* provided as indices into real states
+            const std::vector<T, Allocator> solution_path = ExtractSolutionPath(roadmap, astar_result.first);
+            return std::make_pair(solution_path, astar_result.second);
+        }
+
+        template<typename T, typename Allocator = std::allocator<T>, typename Generator = std::mt19937_64>
+        static std::pair<std::vector<T, Allocator>, double> QueryPathAndAddNodesSingleStartSingleGoalRandomWalk(
+                const T& start,
+                const T& goal,
+                Generator& generator,
+                arc_dijkstras::Graph<T, Allocator>& roadmap,
+                const std::function<bool(const T&, const T&)>& edge_validity_check_fn,
+                const std::function<double(const T&, const T&)>& distance_fn,
+                const size_t K,
+                const bool distance_is_symmetric = true)
+        {
+            // Add the start node to the roadmap
+            const int64_t start_node_index = AddNodeToRoadmap(start, NEW_STATE_TO_ROADMAP, roadmap, distance_fn, edge_validity_check_fn, K, distance_is_symmetric);
+            // Add the goal node to the roadmap
+            const int64_t goal_node_index = AddNodeToRoadmap(goal, ROADMAP_TO_NEW_STATE, roadmap, distance_fn, edge_validity_check_fn, K, distance_is_symmetric);
+            // Call the random walk algorithm
+            const auto random_walk_result = arc_dijkstras::GraphRandomWalk<T, Allocator>::PerformRandomWalk(roadmap, start_node_index, goal_node_index, generator);
+            // Convert the result into a path and return it
+            const auto solution_path = ExtractSolutionPath(roadmap, random_walk_result);
+            const auto distance = EigenHelpers::CalculateTotalDistance(solution_path, distance_fn);
+            return std::make_pair(solution_path, distance);
+        }
+
+        // TODO - figure out a better way to balance parallelism between KNN queries inside path calls and multiple calls to Dijkstras
+        template<typename T, typename Allocator = std::allocator<T>>
+        static std::pair<std::vector<T, Allocator>, double> QueryPathMultiStartMultiGoal(
+                const std::vector<T, Allocator>& starts,
+                const std::vector<T, Allocator>& goals,
+                const arc_dijkstras::Graph<T, Allocator>& roadmap,
+                const std::function<bool(const T&, const T&)>& edge_validity_check_fn,
+                const std::function<double(const T&, const T&)>& distance_fn,
+                const size_t K,
+                const bool distance_is_symmetric = true)
+        {
+            std::vector<std::pair<std::vector<T, Allocator>, double>> possible_solutions(goals.size());
+            for (size_t goal_idx = 0; goal_idx < goals.size(); goal_idx++)
+            {
+                possible_solutions[goal_idx] = QueryPathMultiStartSingleGoal(starts, goals[goal_idx], roadmap, edge_validity_check_fn, distance_fn, K, distance_is_symmetric);
+            }
+            const double best_solution_distance = std::numeric_limits<double>::infinity();
+            const int64_t best_solution_index = -1;
+            for (size_t goal_idx = 0; goal_idx < goals.size(); goal_idx++)
+            {
+                const double solution_distance = possible_solutions[goal_idx].second;
+                if (solution_distance < best_solution_distance)
+                {
+                    best_solution_distance = solution_distance;
+                    best_solution_index = goal_idx;
+                }
+            }
+            if ((best_solution_index >= 0) && (best_solution_distance < std::numeric_limits<double>::infinity()))
+            {
+                return possible_solutions[best_solution_index];
+            }
+            else
+            {
+                return std::make_pair(std::vector<T, Allocator>(), std::numeric_limits<double>::infinity());
+            }
+        }
+
+        template<typename T, typename Allocator = std::allocator<T>>
+        static std::pair<std::vector<T, Allocator>, double> QueryPathMultiStartSingleGoal(
+                const std::vector<T, Allocator>& starts,
+                const T& goal,
+                const arc_dijkstras::Graph<T, Allocator>& roadmap,
+                const std::function<bool(const T&, const T&)>& edge_validity_check_fn,
+                const std::function<double(const T&, const T&)>& distance_fn,
+                const size_t K,
+                const bool distance_is_symmetric = true)
+        {
+            arc_dijkstras::Graph<T, Allocator> working_copy = roadmap;
+            return QueryPathAndAddNodesMultiStartSingleGoal(starts, goal, working_copy, edge_validity_check_fn, distance_fn, K, distance_is_symmetric);
+        }
+
+        template<typename T, typename Allocator = std::allocator<T>>
+        static std::pair<std::vector<T, Allocator>, double> QueryPathSingleStartSingleGoal(
+                const T& start,
+                const T& goal,
+                const arc_dijkstras::Graph<T, Allocator>& roadmap,
+                const std::function<bool(const T&, const T&)>& edge_validity_check_fn,
+                const std::function<double(const T&, const T&)>& distance_fn,
+                const size_t K,
+                const bool distance_is_symmetric = true)
+        {
+            arc_dijkstras::Graph<T, Allocator> working_copy = roadmap;
+            return QueryPathAndAddNodesSingleStartSingleGoal(start, goal, working_copy, edge_validity_check_fn, distance_fn, K, distance_is_symmetric);
+        }
+
+        template<typename T, typename Allocator=std::allocator<T>>
+        static std::pair<std::vector<T, Allocator>, double> LazyQueryPathSingleStartSingleGoal(
+                const T& start, const T& goal, const arc_dijkstras::Graph<T, Allocator>& roadmap,
+                const std::function<bool(const T&, const T&)>& edge_validity_check_fn,
+                const std::function<double(const T&, const T&)>& distance_fn,
+                const size_t K,
+                const bool distance_is_symmetric = true)
+        {
+            arc_dijkstras::Graph<T, Allocator> working_copy = roadmap;
+            return LazyQueryPathAndAddNodesSingleStartSingleGoal(start, goal, working_copy, edge_validity_check_fn, distance_fn, K, distance_is_symmetric);
+        }
+
+        // TODO update to provide lazy and non-lazy variants of single start/single goal
+    };
+}
+
+#endif // SIMPLE_PRM_PLANNER

flightlib/third_party/arc_utilities/include/arc_utilities/simple_rrt_planner.hpp

@@ -0,0 +1,887 @@
+#include <stdlib.h>
+#include <vector>
+#include <string>
+#include <functional>
+#include <chrono>
+#include <random>
+#include <memory>
+#include <Eigen/Dense>
+#include <arc_utilities/serialization.hpp>
+
+#ifndef SIMPLE_RRT_PLANNER_HPP
+#define SIMPLE_RRT_PLANNER_HPP
+
+namespace simple_rrt_planner
+{
+    template<typename T, typename Allocator = std::allocator<T>>
+    class SimpleRRTPlannerState
+    {
+    protected:
+
+        T value_;
+        std::vector<int64_t> child_indices_;
+        int64_t parent_index_;
+        bool initialized_;
+
+    public:
+
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        static uint64_t Serialize(const SimpleRRTPlannerState<T, Allocator>& state, std::vector<uint8_t>& buffer, const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& value_serializer)
+        {
+            return state.SerializeSelf(buffer, value_serializer);
+        }
+
+        static std::pair<SimpleRRTPlannerState<T, Allocator>, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
+        {
+            SimpleRRTPlannerState<T, Allocator> temp_state;
+            const uint64_t bytes_read = temp_state.DeserializeSelf(buffer, current, value_deserializer);
+            return std::make_pair(temp_state, bytes_read);
+        }
+
+        SimpleRRTPlannerState()
+            : parent_index_(-1)
+            , initialized_(false)
+        {
+            child_indices_.clear();
+        }
+
+        SimpleRRTPlannerState(const T& value, const int64_t parent_index, const std::vector<int64_t>& child_indices)
+            : value_(value)
+            , child_indices_(child_indices)
+            , parent_index_(parent_index)
+            , initialized_(true)
+        {}
+
+        SimpleRRTPlannerState(const T& value, const int64_t parent_index)
+            : value_(value)
+            , parent_index_(parent_index)
+            , initialized_(true)
+        {
+            child_indices_.clear();
+        }
+
+        SimpleRRTPlannerState(const T& value)
+            : value_(value)
+            , parent_index_(-1)
+            , initialized_(true)
+        {
+            child_indices_.clear();
+        }
+
+        uint64_t SerializeSelf(std::vector<uint8_t>& buffer, const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& value_serializer) const
+        {
+            const uint64_t start_buffer_size = buffer.size();
+            // Serialize the initialized
+            arc_utilities::SerializeFixedSizePOD<uint8_t>((uint8_t)initialized_, buffer);
+            // Serialize the parent index
+            arc_utilities::SerializeFixedSizePOD<int64_t>(parent_index_, buffer);
+            // Serialize the child indices
+            arc_utilities::SerializeVector<int64_t>(child_indices_, buffer, arc_utilities::SerializeFixedSizePOD<int64_t>);
+            // Serialize the value
+            value_serializer(value_, buffer);
+            // Figure out how many bytes were written
+            const uint64_t end_buffer_size = buffer.size();
+            const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+            return bytes_written;
+        }
+
+        uint64_t DeserializeSelf(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
+        {
+            uint64_t current_position = current;
+            // Deserialize the initialized
+            const std::pair<uint8_t, uint64_t> initialized_deserialized = arc_utilities::DeserializeFixedSizePOD<uint8_t>(buffer, current_position);
+            initialized_ = (bool)initialized_deserialized.first;
+            current_position += initialized_deserialized.second;
+            // Deserialize the parent index
+            const std::pair<int64_t, uint64_t> parent_index_deserialized = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
+            parent_index_ = parent_index_deserialized.first;
+            current_position += parent_index_deserialized.second;
+            // Deserialize the child indices
+            const std::pair<std::vector<int64_t>, uint64_t> child_indices_deserialized = arc_utilities::DeserializeVector<int64_t>(buffer, current_position, arc_utilities::DeserializeFixedSizePOD<int64_t>);
+            child_indices_ = child_indices_deserialized.first;
+            current_position += child_indices_deserialized.second;
+            // Deserialize the value
+            const std::pair<T, uint64_t> value_deserialized = value_deserializer(buffer, current_position);
+            value_ = value_deserialized.first;
+            current_position += value_deserialized.second;
+            // Figure out how many bytes were read
+            const uint64_t bytes_read = current_position - current;
+            return bytes_read;
+        }
+
+        bool IsInitialized() const
+        {
+            return initialized_;
+        }
+
+        const T& GetValueImmutable() const
+        {
+            return value_;
+        }
+
+        T& GetValueMutable()
+        {
+            return value_;
+        }
+
+        int64_t GetParentIndex() const
+        {
+            return parent_index_;
+        }
+
+        void SetParentIndex(const int64_t parent_index)
+        {
+            parent_index_ = parent_index;
+        }
+
+        const std::vector<int64_t>& GetChildIndices() const
+        {
+            return child_indices_;
+        }
+
+        void ClearChildIndicies()
+        {
+            child_indices_.clear();
+        }
+
+        void AddChildIndex(const int64_t child_index)
+        {
+            for (size_t idx = 0; idx < child_indices_.size(); idx++)
+            {
+                if (child_indices_[idx] == child_index)
+                {
+                    return;
+                }
+            }
+            child_indices_.push_back(child_index);
+        }
+
+        void RemoveChildIndex(const int64_t child_index)
+        {
+            std::vector<int64_t> new_child_indices;
+            for (size_t idx = 0; idx < child_indices_.size(); idx++)
+            {
+                if (child_indices_[idx] != child_index)
+                {
+                    new_child_indices.push_back(child_indices_[idx]);
+                }
+            }
+            child_indices_ = new_child_indices;
+        }
+    };
+
+    template<typename T, typename Allocator = std::allocator<T>>
+    class SimpleRRTPlannerPointerState
+    {
+    protected:
+
+        T value_;
+        std::shared_ptr<const SimpleRRTPlannerPointerState<T, Allocator>> parent_;
+        bool initialized_;
+
+    public:
+
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        SimpleRRTPlannerPointerState()
+            : parent_(nullptr)
+            , initialized_(false)
+        {}
+
+        SimpleRRTPlannerPointerState(const T& value, const std::shared_ptr<const SimpleRRTPlannerPointerState<T, Allocator>>& parent)
+            : value_(value)
+            , parent_(parent)
+            , initialized_(true)
+        {}
+
+        SimpleRRTPlannerPointerState(const T& value)
+            : value_(value)
+            , parent_(std::shared_ptr<const SimpleRRTPlannerPointerState<T, Allocator>>())
+            , initialized_(true)
+        {}
+
+        bool IsInitialized() const
+        {
+            return initialized_;
+        }
+
+        const T& GetValueImmutable() const
+        {
+            return value_;
+        }
+
+        T& GetValueMutable()
+        {
+            return value_;
+        }
+
+        const std::shared_ptr<const SimpleRRTPlannerPointerState<T, Allocator>>& GetParent() const
+        {
+            return parent_;
+        }
+
+        void SetParent(const std::shared_ptr<const SimpleRRTPlannerPointerState<T, Allocator>>& parent)
+        {
+            parent_(parent);
+        }
+    };
+
+    class SimpleHybridRRTPlanner
+    {
+    private:
+
+        SimpleHybridRRTPlanner() {}
+
+    public:
+
+        /* Template-based single-tree RRT planner
+         *
+         * Template type T is your state type (i.e. a configuration)
+         *
+         * Arguments:
+         * start - starting configuration
+         * goal - target configuration
+         * nearest_neighbor_fn - given all nodes explored so far, and a new state, return the index of the "closest" node
+         * goal_reached_fn - return if a given state meets the goal conditions (for example, within a radius of the goal state)
+         * state_sampling_fn - returns a new state (randomly- or deterministically-sampled)
+         * forward_propagation_fn - given the nearest neighbor and a new target state, returns the states that would grow the tree towards the target
+         * goal_bias - in (0, 1), selects the probability that the new sampled state is the goal state
+         * time_limit - limit, in seconds, for the runtime of the planner
+         * rng - a random number generator matching the interface of the generators provided by std::random
+         *
+         * Returns:
+         * std::pair<path, statistics>
+         * path - vector of states corresponding to the planned path
+         * statistics - map of string keys/double values of planner statistics (i.e. run time, #states explored, #states in solution
+         */
+        template<typename RNG, typename T, typename Allocator=std::allocator<T>>
+        static std::pair<std::vector<T, Allocator>, std::map<std::string, double>> Plan(
+                const T& start,
+                const T& goal,
+                const std::function<int64_t(const std::vector<SimpleRRTPlannerState<T, Allocator>>&,const T&)>& nearest_neighbor_fn,
+                const std::function<bool(const T&)>& goal_reached_fn,
+                const std::function<T(void)>& state_sampling_fn,
+                const std::function<std::vector<std::pair<T, int64_t>>(const T&, const T&)>& forward_propagation_fn,
+                const double goal_bias,
+                const std::chrono::duration<double>& time_limit,
+                RNG& rng)
+        {
+            std::uniform_real_distribution<double> goal_bias_distribution(0.0, 1.0);
+            const std::function<T(void)> sampling_function = [&](void) { return ((goal_bias_distribution(rng) > goal_bias) ? state_sampling_fn() : goal); };
+            return Plan(start, nearest_neighbor_fn, goal_reached_fn, sampling_function, forward_propagation_fn, time_limit);
+        }
+
+        /* Template-based single-tree RRT planner
+         *
+         * Template type T is your state type (i.e. a configuration)
+         *
+         * Arguments:
+         * start - starting configuration
+         * nearest_neighbor_fn - given all nodes explored so far, and a new state, return the index of the "closest" node
+         * goal_reached_fn - return if a given state meets the goal conditions (for example, within a radius of a goal state)
+         * state_sampling_fn - returns a new state (randomly- or deterministically-sampled)
+         * goal_sampling_fn - returns a goal state (randomly- or deterministically-sampled)
+         * forward_propagation_fn - given the nearest neighbor and a new target state, returns the states that would grow the tree towards the target
+         * goal_bias - in (0, 1), selects the probability that the new sampled state is a goal state
+         * time_limit - limit, in seconds, for the runtime of the planner
+         * rng - a random number generator matching the interface of the generators provided by std::random
+         *
+         * Returns:
+         * std::pair<path, statistics>
+         * path - vector of states corresponding to the planned path
+         * statistics - map of string keys/double values of planner statistics (i.e. run time, #states explored, #states in solution
+         */
+        template<typename RNG, typename T, typename Allocator=std::allocator<T>>
+        static std::pair<std::vector<T, Allocator>, std::map<std::string, double>> Plan(
+                const T& start,
+                const std::function<int64_t(const std::vector<SimpleRRTPlannerState<T, Allocator>>&, const T&)>& nearest_neighbor_fn,
+                const std::function<bool(const T&)>& goal_reached_fn,
+                const std::function<T(void)>& state_sampling_fn,
+                const std::function<T(void)>& goal_sampling_fn,
+                const std::function<std::vector<std::pair<T, int64_t>>(const T&, const T&)>& forward_propagation_fn,
+                const double goal_bias,
+                const std::chrono::duration<double>& time_limit,
+                RNG& rng)
+        {
+            std::uniform_real_distribution<double> goal_bias_distribution(0.0, 1.0);
+            const std::function<T(void)> sampling_function = [&](void) { return ((goal_bias_distribution(rng) > goal_bias) ? state_sampling_fn() : goal_sampling_fn()); };
+            return Plan(start, nearest_neighbor_fn, goal_reached_fn, sampling_function, forward_propagation_fn, time_limit);
+        }
+
+        /* Template-based single-tree RRT planner
+         *
+         * Template type T is your state type (i.e. a configuration)
+         *
+         * Arguments:
+         * start - starting configuration
+         * nearest_neighbor_fn - given all nodes explored so far, and a new state, return the index of the "closest" node
+         * goal_reached_fn - return if a given state meets the goal conditions (for example, within a radius of a goal state)
+         * state_sampling_fn - returns a new state (randomly- or deterministically-sampled)
+         * forward_propagation_fn - given the nearest neighbor and a new target state, returns the states that would grow the tree towards the target
+         * time_limit - limit, in seconds, for the runtime of the planner
+         *
+         * Returns:
+         * std::pair<path, statistics>
+         * path - vector of states corresponding to the planned path
+         * statistics - map of string keys/double values of planner statistics (i.e. run time, #states explored, #states in solution
+         */
+        template<typename T, typename Allocator=std::allocator<T>>
+        static std::pair<std::vector<T, Allocator>, std::map<std::string, double>> Plan(
+                const T& start,
+                const std::function<int64_t(const std::vector<SimpleRRTPlannerState<T, Allocator>>&, const T&)>& nearest_neighbor_fn,
+                const std::function<bool(const T&)>& goal_reached_fn,
+                const std::function<T(void)>& sampling_fn,
+                const std::function<std::vector<std::pair<T, int64_t>>(const T&, const T&)>& forward_propagation_fn,
+                const std::chrono::duration<double>& time_limit)
+        {
+            std::chrono::time_point<std::chrono::steady_clock> start_time = std::chrono::steady_clock::now();
+            const std::function<bool(void)> termination_check_fn = [&](void) { return (((std::chrono::time_point<std::chrono::steady_clock>)std::chrono::steady_clock::now() - start_time) > time_limit); };
+            return Plan(start, nearest_neighbor_fn, goal_reached_fn, sampling_fn, forward_propagation_fn, termination_check_fn);
+        }
+
+        /* Template-based single-tree RRT planner
+         *
+         * Template type T is your state type (i.e. a configuration)
+         *
+         * Arguments:
+         * start - starting configuration
+         * nearest_neighbor_fn - given all nodes explored so far, and a new state, return the index of the "closest" node
+         * goal_reached_fn - return if a given state meets the goal conditions (for example, within a radius of a goal state)
+         * state_sampling_fn - returns a new state (randomly- or deterministically-sampled)
+         * forward_propagation_fn - given the nearest neighbor and a new target state, returns the states that would grow the tree towards the target
+         * termination_check_fn - returns if the planner should terminate (for example, if it has exceeded time/space limits)
+         *
+         * Returns:
+         * std::pair<path, statistics>
+         * path - vector of states corresponding to the planned path
+         * statistics - map of string keys/double values of planner statistics (i.e. run time, #states explored, #states in solution
+         */
+        template<typename T, typename Allocator=std::allocator<T>>
+        static std::pair<std::vector<T, Allocator>, std::map<std::string, double>> Plan(
+                const T& start,
+                const std::function<int64_t(const std::vector<SimpleRRTPlannerState<T, Allocator>>&, const T&)>& nearest_neighbor_fn,
+                const std::function<bool(const T&)>& goal_reached_fn,
+                const std::function<T(void)>& sampling_fn,
+                const std::function<std::vector<std::pair<T, int64_t>>(const T&, const T&)>& forward_propagation_fn,
+                const std::function<bool(void)>& termination_check_fn)
+        {
+            // Define a couple lambdas to let us use the generic multi-path planner as if it were a single-path planner
+            bool solution_found = false;
+            const std::function<bool(const T&)> real_goal_found_fn = [&](const T& state) { if (goal_reached_fn(state)) { solution_found = true; return true; } else {return false;} };
+            const std::function<bool(void)> real_termination_check_fn = [&](void) { if (!solution_found) { return termination_check_fn(); } else {return true;} };
+            const std::function<void(SimpleRRTPlannerState<T, Allocator>&)> dummy_goal_callback_fn = [](SimpleRRTPlannerState<T, Allocator>& state) {;};
+            // Call the planner
+            std::pair<std::vector<std::vector<T, Allocator>>, std::map<std::string, double>> planning_result = PlanMultiPath(start, nearest_neighbor_fn, real_goal_found_fn, dummy_goal_callback_fn, sampling_fn, forward_propagation_fn, real_termination_check_fn);
+            // Put together the return
+            std::vector<T, Allocator> planned_path;
+            if (planning_result.first.size() > 0)
+            {
+                planned_path = planning_result.first[0];
+            }
+            return std::pair<std::vector<T, Allocator>, std::map<std::string, double>>(planned_path, planning_result.second);
+        }
+
+        /* Template-based single-tree RRT planner
+         *
+         * Template type T is your state type (i.e. a configuration)
+         *
+         * Arguments:
+         * nodes - a mutable vector of planner states, used internally to store the planner tree.
+         *          This is provided to allow external use of the tree during and after planning.
+         * start - starting configuration
+         * nearest_neighbor_fn - given all nodes explored so far, and a new state, return the index of the "closest" node
+         * goal_reached_fn - return if a given state meets the goal conditions (for example, within a radius of a goal state)
+         * state_sampling_fn - returns a new state (randomly- or deterministically-sampled)
+         * forward_propagation_fn - given the nearest neighbor and a new target state, returns the states that would grow the tree towards the target
+         * termination_check_fn - returns if the planner should terminate (for example, if it has exceeded time/space limits)
+         *
+         * Returns:
+         * std::pair<path, statistics>
+         * path - vector of states corresponding to the planned path
+         * statistics - map of string keys/double values of planner statistics (i.e. run time, #states explored, #states in solution
+         */
+        template<typename T, typename Allocator=std::allocator<T>>
+        static std::pair<std::vector<T, Allocator>, std::map<std::string, double>> Plan(
+                std::vector<SimpleRRTPlannerState<T, Allocator>>& nodes,
+                const T& start,
+                const std::function<int64_t(const std::vector<SimpleRRTPlannerState<T, Allocator>>&, const T&)>& nearest_neighbor_fn,
+                const std::function<bool(const T&)>& goal_reached_fn,
+                const std::function<T(void)>& sampling_fn,
+                const std::function<std::vector<std::pair<T, int64_t>>(const T&, const T&)>& forward_propagation_fn,
+                const std::function<bool(void)>& termination_check_fn)
+        {
+            // Define a couple lambdas to let us use the generic multi-path planner as if it were a single-path planner
+            bool solution_found = false;
+            const std::function<bool(const T&)> real_goal_found_fn = [&](const T& state) { if (goal_reached_fn(state)) { solution_found = true; return true; } else {return false;} };
+            const std::function<bool(void)> real_termination_check_fn = [&](void) { if (!solution_found) { return termination_check_fn(); } else {return true;} };
+            const std::function<void(SimpleRRTPlannerState<T, Allocator>&)> dummy_goal_callback_fn = [](SimpleRRTPlannerState<T, Allocator>& state) {;};
+            // Call the planner
+            std::pair<std::vector<std::vector<T, Allocator>>, std::map<std::string, double>> planning_result = PlanMultiPath(nodes, start, nearest_neighbor_fn, real_goal_found_fn, dummy_goal_callback_fn, sampling_fn, forward_propagation_fn, real_termination_check_fn);
+            // Put together the return
+            std::vector<T, Allocator> planned_path;
+            if (planning_result.first.size() > 0)
+            {
+                planned_path = planning_result.first[0];
+            }
+            return std::pair<std::vector<T, Allocator>, std::map<std::string, double>>(planned_path, planning_result.second);
+        }
+
+        /* Template-based single-tree RRT planner
+         *
+         * Template type T is your state type (i.e. a configuration)
+         *
+         * Arguments:
+         * start - starting configuration
+         * nearest_neighbor_fn - given all nodes explored so far, and a new state, return the index of the "closest" node
+         * goal_reached_fn - return if a given state meets the goal conditions (for example, within a radius of a goal state)
+         * state_sampling_fn - returns a new state (randomly- or deterministically-sampled)
+         * forward_propagation_fn - given the nearest neighbor and a new target state, returns the states that would grow the tree towards the target
+         * termination_check_fn - returns if the planner should terminate (for example, if it has exceeded time/space limits)
+         *
+         * Returns:
+         * std::pair<paths, statistics>
+         * paths - vector of vector of states corresponding to the planned path(s)
+         * statistics - map of string keys/double values of planner statistics (i.e. run time, #states explored, #states in solution
+         */
+        template<typename T, typename Allocator=std::allocator<T>>
+        static std::pair<std::vector<std::vector<T, Allocator>>, std::map<std::string, double>> PlanMultiPath(
+                const T& start,
+                const std::function<int64_t(const std::vector<SimpleRRTPlannerState<T, Allocator>>&, const T&)>& nearest_neighbor_fn,
+                const std::function<bool(const T&)>& goal_reached_fn,
+                const std::function<void(SimpleRRTPlannerState<T, Allocator>&)>& goal_reached_callback_fn,
+                const std::function<T(void)>& sampling_fn,
+                const std::function<std::vector<std::pair<T, int64_t>>(const T&, const T&)>& forward_propagation_fn,
+                const std::function<bool(void)>& termination_check_fn)
+        {
+            // Keep track of states
+            std::vector<SimpleRRTPlannerState<T, Allocator>> nodes;
+            return PlanMultiPath(nodes, start, nearest_neighbor_fn, goal_reached_fn, goal_reached_callback_fn, sampling_fn, forward_propagation_fn, termination_check_fn);
+        }
+
+        /* Template-based single-tree RRT planner
+         *
+         * Template type T is your state type (i.e. a configuration)
+         *
+         * Arguments:
+         * nodes - a mutable vector of planner states, used internally to store the planner tree.
+         *          This is provided to allow external use of the tree during and after planning.
+         * start - starting configuration
+         * nearest_neighbor_fn - given all nodes explored so far, and a new state, return the index of the "closest" node
+         * goal_reached_fn - return if a given state meets the goal conditions (for example, within a radius of a goal state)
+         * state_sampling_fn - returns a new state (randomly- or deterministically-sampled)
+         * forward_propagation_fn - given the nearest neighbor and a new target state, returns the states that would grow the tree towards the target
+         * termination_check_fn - returns if the planner should terminate (for example, if it has exceeded time/space limits)
+         *
+         * Returns:
+         * std::pair<paths, statistics>
+         * paths - vector of vector of states corresponding to the planned path(s)
+         * statistics - map of string keys/double values of planner statistics (i.e. run time, #states explored, #states in solution
+         */
+        template<typename T, typename Allocator=std::allocator<T>>
+        static std::pair<std::vector<std::vector<T, Allocator>>, std::map<std::string, double>> PlanMultiPath(
+                std::vector<SimpleRRTPlannerState<T, Allocator>>& nodes,
+                const T& start,
+                const std::function<int64_t(const std::vector<SimpleRRTPlannerState<T, Allocator>>&, const T&)>& nearest_neighbor_fn,
+                const std::function<bool(const T&)>& goal_reached_fn,
+                const std::function<void(SimpleRRTPlannerState<T, Allocator>&)>& goal_reached_callback_fn,
+                const std::function<T(void)>& sampling_fn,
+                const std::function<std::vector<std::pair<T, int64_t>>(const T&, const T&)>& forward_propagation_fn,
+                const std::function<bool(void)>& termination_check_fn)
+        {
+            // Clear the tree we've been given
+            nodes.clear();
+            // Add the start state
+            SimpleRRTPlannerState<T, Allocator> start_state(start);
+            nodes.push_back(start_state);
+            // Call the planner
+            return PlanMultiPath(nodes, nearest_neighbor_fn, goal_reached_fn, goal_reached_callback_fn, sampling_fn, forward_propagation_fn, termination_check_fn);
+        }
+
+        /* Template-based single-tree RRT planner
+         *
+         * Template type T is your state type (i.e. a configuration)
+         *
+         * Arguments:
+         * nodes - a mutable vector of planner states, used internally to store the planner tree.
+         *          This is provided to allow external use of the tree during and after planning.
+         *          This contains either a SINGLE start state, or the tree resulting from previous planning.
+         * nearest_neighbor_fn - given all nodes explored so far, and a new state, return the index of the "closest" node
+         * goal_reached_fn - return if a given state meets the goal conditions (for example, within a radius of a goal state)
+         * state_sampling_fn - returns a new state (randomly- or deterministically-sampled)
+         * forward_propagation_fn - given the nearest neighbor and a new target state, returns the states that would grow the tree towards the target
+         * termination_check_fn - returns if the planner should terminate (for example, if it has exceeded time/space limits)
+         *
+         * Returns:
+         * std::pair<paths, statistics>
+         * paths - vector of vector of states corresponding to the planned path(s)
+         * statistics - map of string keys/double values of planner statistics (i.e. run time, #states explored, #states in solution
+         */
+        template<typename T, typename Allocator=std::allocator<T>>
+        static std::pair<std::vector<std::vector<T, Allocator>>, std::map<std::string, double>> PlanMultiPath(
+                std::vector<SimpleRRTPlannerState<T, Allocator>>& nodes,
+                const std::function<int64_t(const std::vector<SimpleRRTPlannerState<T, Allocator>>&, const T&)>& nearest_neighbor_fn,
+                const std::function<bool(const T&)>& goal_reached_fn,
+                const std::function<void(SimpleRRTPlannerState<T, Allocator>&)>& goal_reached_callback_fn,
+                const std::function<T(void)>& sampling_fn,
+                const std::function<std::vector<std::pair<T, int64_t>>(const T&, const T&)>& forward_propagation_fn,
+                const std::function<bool(void)>& termination_check_fn)
+        {
+            // Make a dummy state added function
+            const std::function<void(SimpleRRTPlannerState<T, Allocator>&, SimpleRRTPlannerState<T, Allocator>&)> dummy_state_added_fn = [] (SimpleRRTPlannerState<T, Allocator>& parent, SimpleRRTPlannerState<T, Allocator>& new_child) { ; };
+            // Call the planner
+            return PlanMultiPath(nodes, nearest_neighbor_fn, dummy_state_added_fn, goal_reached_fn, goal_reached_callback_fn, sampling_fn, forward_propagation_fn, termination_check_fn);
+        }
+
+        /* Template-based single-tree RRT planner
+         *
+         * Template type T is your state type (i.e. a configuration)
+         *
+         * Arguments:
+         * nodes - a mutable vector of planner states, used internally to store the planner tree.
+         *          This is provided to allow external use of the tree during and after planning.
+         *          This contains either a SINGLE start state, or the tree resulting from previous planning.
+         * nearest_neighbor_fn - given all nodes explored so far, and a new state, return the index of the "closest" node
+         * state_added_fn - callback function that takes (parent, child) for each extension
+         * goal_reached_fn - return if a given state meets the goal conditions (for example, within a radius of a goal state)
+         * state_sampling_fn - returns a new state (randomly- or deterministically-sampled)
+         * forward_propagation_fn - given the nearest neighbor and a new target state, returns the states that would grow the tree towards the target
+         * termination_check_fn - returns if the planner should terminate (for example, if it has exceeded time/space limits)
+         *
+         * Returns:
+         * std::pair<paths, statistics>
+         * paths - vector of vector of states corresponding to the planned path(s)
+         * statistics - map of string keys/double values of planner statistics (i.e. run time, #states explored, #states in solution
+         */
+        template<typename T, typename Allocator=std::allocator<T>>
+        static std::pair<std::vector<std::vector<T, Allocator>>, std::map<std::string, double>> PlanMultiPath(
+                std::vector<SimpleRRTPlannerState<T, Allocator>>& nodes,
+                const std::function<int64_t(const std::vector<SimpleRRTPlannerState<T, Allocator>>&, const T&)>& nearest_neighbor_fn,
+                const std::function<void(SimpleRRTPlannerState<T, Allocator>&, SimpleRRTPlannerState<T, Allocator>&)>& state_added_fn,
+                const std::function<bool(const T&)>& goal_reached_fn,
+                const std::function<void(SimpleRRTPlannerState<T, Allocator>&)>& goal_reached_callback_fn,
+                const std::function<T(void)>& sampling_fn,
+                const std::function<std::vector<std::pair<T, int64_t>>(const T&, const T&)>& forward_propagation_fn,
+                const std::function<bool(void)>& termination_check_fn)
+        {
+            // Make sure we've been given a start state
+            assert(nodes.size() > 0);
+            // Make sure the tree is properly linked
+            assert(CheckTreeLinkage(nodes));
+            // Keep track of statistics
+            std::map<std::string, double> statistics;
+            statistics["total_samples"] = 0.0;
+            statistics["successful_samples"] = 0.0;
+            statistics["failed_samples"] = 0.0;
+            // Storage for the goal states we reach
+            std::vector<int64_t> goal_state_indices;
+            // Safety check before doing real work
+            if (goal_reached_fn(nodes[0].GetValueImmutable()))
+            {
+                goal_state_indices.push_back(0);
+                std::cerr << "Start state meets goal conditions, returning default path [start]" << std::endl;
+                // Put together the results
+                std::vector<std::vector<T>> planned_paths = ExtractSolutionPaths(nodes, goal_state_indices);
+                statistics["planning_time"] = 0.0;
+                statistics["total_states"] = nodes.size();
+                statistics["solutions"] = (double)planned_paths.size();
+                return std::pair<std::vector<std::vector<T>>, std::map<std::string, double>>(planned_paths, statistics);
+            }
+            // Update the start time
+            std::chrono::time_point<std::chrono::steady_clock> start_time = std::chrono::steady_clock::now();
+            // Plan
+            while (!termination_check_fn())
+            {
+                // Sample a random goal
+                T random_target = sampling_fn();
+                // Get the nearest neighbor
+                int64_t nearest_neighbor_index = nearest_neighbor_fn(nodes, random_target);
+                assert((nearest_neighbor_index >= 0) && (nearest_neighbor_index < nodes.size()));
+                const T& nearest_neighbor = nodes.at(nearest_neighbor_index).GetValueImmutable();
+                // Forward propagate towards the goal
+                std::vector<std::pair<T, int64_t>> propagated = forward_propagation_fn(nearest_neighbor, random_target);
+                if (!propagated.empty())
+                {
+                    statistics["total_samples"] += 1.0;
+                    statistics["successful_samples"] += 1.0;
+                    for (size_t idx = 0; idx < propagated.size(); idx++)
+                    {
+                        const std::pair<T, int64_t>& current_propagation = propagated[idx];
+                        // Determine the parent index of the new state
+                        // This process deserves some explanation
+                        // The "current relative parent index" is the index of the parent, relative to the list of propagated nodes.
+                        // A negative value means the nearest neighbor in the tree, zero means the first propagated node, and so on.
+                        // NOTE - the relative parent index *must* be lower than the index in the list of prograted nodes
+                        // i.e. the first node must have a negative value, and so on.
+                        const int64_t& current_relative_parent_index = current_propagation.second;
+                        int64_t node_parent_index = nearest_neighbor_index;
+                        if (current_relative_parent_index >= 0)
+                        {
+                            const int64_t current_relative_index = (int64_t)idx;
+                            assert(current_relative_parent_index < current_relative_index);
+                            const int64_t current_relative_offset = current_relative_parent_index - current_relative_index;
+                            assert(current_relative_offset < 0);
+                            assert(current_relative_offset >= -(int64_t)propagated.size());
+                            const int64_t current_nodes_size = (int64_t)nodes.size();
+                            node_parent_index = current_nodes_size + current_relative_offset; // Offset is negative!
+                        }
+                        else
+                        {
+                            node_parent_index = nearest_neighbor_index; // Negative relative parent index means our parent index is the nearest neighbor index
+                        }
+                        // Build the new state
+                        const T& current_propagated = current_propagation.first;
+                        SimpleRRTPlannerState<T, Allocator> new_state(current_propagated, node_parent_index);
+                        // Add the state to the tree
+                        nodes.push_back(new_state);
+                        int64_t new_node_index = (int64_t)nodes.size() - 1;
+                        nodes[node_parent_index].AddChildIndex(new_node_index);
+                        // Call the state added callback
+                        state_added_fn(nodes[node_parent_index], nodes[new_node_index]);
+                        // Check if we've reached the goal
+                        if (goal_reached_fn(nodes[new_node_index].GetValueImmutable()))
+                        {
+                            goal_state_indices.push_back(new_node_index);
+                            goal_reached_callback_fn(nodes[new_node_index]);
+                        }
+                    }
+                }
+                else
+                {
+                    statistics["total_samples"] += 1.0;
+                    statistics["failed_samples"] += 1.0;
+                }
+            }
+            // Put together the results
+            // Make sure the tree is properly linked
+            assert(CheckTreeLinkage(nodes));
+            std::vector<std::vector<T, Allocator>> planned_paths = ExtractSolutionPaths(nodes, goal_state_indices);
+            std::chrono::time_point<std::chrono::steady_clock> cur_time = std::chrono::steady_clock::now();
+            std::chrono::duration<double> planning_time(cur_time - start_time);
+            statistics["planning_time"] = planning_time.count();
+            statistics["total_states"] = nodes.size();
+            statistics["solutions"] = (double)planned_paths.size();
+            return std::pair<std::vector<std::vector<T, Allocator>>, std::map<std::string, double>>(planned_paths, statistics);
+        }
+
+        /* Checks the planner tree to make sure the parent-child linkages are correct
+         */
+        template<typename T, typename Allocator=std::allocator<T>>
+        static bool CheckTreeLinkage(const std::vector<SimpleRRTPlannerState<T, Allocator>>& nodes)
+        {
+            // Step through each state in the tree. Make sure that the linkage to the parent and child states are correct
+            for (size_t current_index = 0; current_index < nodes.size(); current_index++)
+            {
+                // For every state, make sure all the parent<->child linkages are valid
+                const SimpleRRTPlannerState<T, Allocator>& current_state = nodes[current_index];
+                if (!current_state.IsInitialized())
+                {
+                    std::cerr << "Tree contains uninitialized node(s) " << current_index << std::endl;
+                    return false;
+                }
+                // Check the linkage to the parent state
+                const int64_t parent_index = current_state.GetParentIndex();
+                if ((parent_index >= 0) && (parent_index < (int64_t)nodes.size()))
+                {
+                    if (parent_index != (int64_t)current_index)
+                    {
+                        const SimpleRRTPlannerState<T, Allocator>& parent_state = nodes[parent_index];
+                        if (!parent_state.IsInitialized())
+                        {
+                            std::cerr << "Tree contains uninitialized node(s) " << parent_index << std::endl;
+                            return false;
+                        }
+                        // Make sure the corresponding parent contains the current node in the list of child indices
+                        const std::vector<int64_t>& parent_child_indices = parent_state.GetChildIndices();
+                        auto index_found = std::find(parent_child_indices.begin(), parent_child_indices.end(), (int64_t)current_index);
+                        if (index_found == parent_child_indices.end())
+                        {
+                            std::cerr << "Parent state " << parent_index << " does not contain child index for current node " << current_index << std::endl;
+                            return false;
+                        }
+                    }
+                    else
+                    {
+                        std::cerr << "Invalid parent index " << parent_index << " for state " << current_index << " [Indices can't be the same]" << std::endl;
+                        return false;
+                    }
+                }
+                else if (parent_index < -1)
+                {
+                    std::cerr << "Invalid parent index " << parent_index << " for state " << current_index << std::endl;
+                    return false;
+                }
+                // Check the linkage to the child states
+                const std::vector<int64_t>& current_child_indices = current_state.GetChildIndices();
+                for (size_t idx = 0; idx < current_child_indices.size(); idx++)
+                {
+                    // Get the current child index
+                    const int64_t current_child_index = current_child_indices[idx];
+                    if ((current_child_index > 0) && (current_child_index < (int64_t)nodes.size()))
+                    {
+                        if (current_child_index != (int64_t)current_index)
+                        {
+                            const SimpleRRTPlannerState<T, Allocator>& child_state = nodes[current_child_index];
+                            if (!child_state.IsInitialized())
+                            {
+                                std::cerr << "Tree contains uninitialized node(s) " << current_child_index << std::endl;
+                                return false;
+                            }
+                            // Make sure the child node points to us as the parent index
+                            const int64_t child_parent_index = child_state.GetParentIndex();
+                            if (child_parent_index != (int64_t)current_index)
+                            {
+                                std::cerr << "Parent index " << child_parent_index << " for current child state " << current_child_index << " does not match index " << current_index << " for current node " << std::endl;
+                                return false;
+                            }
+                        }
+                        else
+                        {
+                            std::cerr << "Invalid child index " << current_child_index << " for state " << current_index << " [Indices can't be the same]" << std::endl;
+                            return false;
+                        }
+                    }
+                    else
+                    {
+                        std::cerr << "Invalid child index " << current_child_index << " for state " << current_index << std::endl;
+                        return false;
+                    }
+                }
+            }
+            return true;
+        }
+
+        /* Extracts all the solution paths corresponding to the provided goal states
+         */
+        template<typename T, typename Allocator=std::allocator<T>>
+        static std::vector<std::vector<T, Allocator>> ExtractSolutionPaths(
+                const std::vector<SimpleRRTPlannerState<T, Allocator>>& nodes,
+                const std::vector<int64_t>& goal_state_indices)
+        {
+            std::vector<std::vector<T, Allocator>> solution_paths;
+            for (size_t idx = 0; idx < goal_state_indices.size(); idx++)
+            {
+                std::vector<T, Allocator> solution_path = ExtractSolutionPath(nodes, goal_state_indices[idx]);
+                solution_paths.push_back(solution_path);
+            }
+            return solution_paths;
+        }
+
+        /* Extracts a single solution path corresponding to the provided goal state
+         */
+        template<typename T, typename Allocator=std::allocator<T>>
+        static std::vector<T, Allocator> ExtractSolutionPath(
+                const std::vector<SimpleRRTPlannerState<T, Allocator>>& nodes,
+                const int64_t goal_state_index)
+        {
+            std::vector<T, Allocator> solution_path;
+            const SimpleRRTPlannerState<T, Allocator>& goal_state = nodes[goal_state_index];
+            solution_path.push_back(goal_state.GetValueImmutable());
+            int64_t parent_index = goal_state.GetParentIndex();
+            while (parent_index >= 0)
+            {
+                assert(parent_index < nodes.size());
+                const SimpleRRTPlannerState<T, Allocator>& parent_state = nodes[parent_index];
+                const T& parent = parent_state.GetValueImmutable();
+                solution_path.push_back(parent);
+                parent_index = parent_state.GetParentIndex();
+            }
+            // Put it in the right order
+            std::reverse(solution_path.begin(), solution_path.end());
+            return solution_path;
+        }
+
+        /* Template-based single-tree RRT planner
+         *
+         * Template type T is your state type (i.e. a configuration)
+         *
+         * Arguments:
+         * start - starting configuration
+         * nearest_neighbor_fn - given all nodes explored so far, and a new state, return the index of the "closest" node
+         * goal_reached_fn - return if a given state meets the goal conditions (for example, within a radius of a goal state)
+         * state_sampling_fn - returns a new state (randomly- or deterministically-sampled)
+         * forward_propagation_fn - given the nearest neighbor and a new target state, returns the states that would grow the tree towards the target
+         * time_limit - limit, in seconds, for the runtime of the planner
+         *
+         * Returns:
+         * std::pair<path, statistics>
+         * path - vector of states corresponding to the planned path
+         * statistics - map of string keys/double values of planner statistics (i.e. run time, #states explored, #states in solution
+         */
+        template<typename T, typename Allocator=std::allocator<T>>
+        static std::pair<std::vector<std::vector<T>>, std::map<std::string, double>> PlanMultiPath(
+                const T& start,
+                const std::function<void(const std::shared_ptr<SimpleRRTPlannerPointerState<T, Allocator>>&)>& register_nearest_neighbors_fn,
+                const std::function<const std::shared_ptr<SimpleRRTPlannerPointerState<T, Allocator>>&(const T&)>& get_nearest_neighbor_fn,
+                const std::function<std::vector<std::vector<T>>(void)>& extract_solution_paths,
+                const std::function<T(void)>& sampling_fn,
+                const std::function<bool(const T&)>& goal_reached_fn,
+                const std::function<void(const std::shared_ptr<SimpleRRTPlannerPointerState<T, Allocator>>&)>& register_goal_state_fn,
+                const std::function<std::vector<T>(const T&, const T&)>& forward_propagation_fn,
+                const std::function<bool(void)>& termination_check_fn)
+        {
+            // Keep track of statistics
+            std::map<std::string, double> statistics;
+            statistics["total_states"] = 0.0;
+            statistics["total_samples"] = 0.0;
+            statistics["successful_samples"] = 0.0;
+            statistics["failed_samples"] = 0.0;
+            // Add the start state
+            SimpleRRTPlannerState<T, Allocator> start_state(start);
+            register_nearest_neighbors_fn(start_state);
+            // Update the start time
+            std::chrono::time_point<std::chrono::steady_clock> start_time = std::chrono::steady_clock::now();
+            // Plan
+            while (!termination_check_fn())
+            {
+                // Sample a random goal
+                T random_target = sampling_fn();
+                // Get the nearest neighbor
+                const std::shared_ptr<SimpleRRTPlannerPointerState<T, Allocator>>& nearest_neighbor_ptr = get_nearest_neighbor_fn(random_target);
+                assert(nearest_neighbor_ptr);
+                const T& nearest_neighbor_value = nearest_neighbor_ptr->GetValueImmutable();
+                // Forward propagate towards the goal
+                std::vector<T> propagated = forward_propagation_fn(nearest_neighbor_value, random_target);
+                if (!propagated.empty())
+                {
+                    statistics["total_samples"] += 1.0;
+                    statistics["successful_samples"] += 1.0;
+                    std::shared_ptr<SimpleRRTPlannerPointerState<T, Allocator>> parent_ptr(nearest_neighbor_ptr);
+                    for (size_t idx = 0; idx < propagated.size(); idx++)
+                    {
+                        statistics["total_states"] += 1.0;
+                        const T& current_propagated = propagated[idx];
+                        std::shared_ptr<SimpleRRTPlannerPointerState<T, Allocator>> new_state_ptr(new SimpleRRTPlannerPointerState<T, Allocator>(current_propagated, parent_ptr));
+                        // If we've reached a goal, register it specially
+                        if (goal_reached_fn(current_propagated))
+                        {
+                            register_goal_state_fn(new_state_ptr);
+                            break;
+                        }
+                        // Otherwise, simply register it as a nearest neighbor
+                        else
+                        {
+                            register_nearest_neighbors_fn(new_state_ptr);
+                            parent_ptr = new_state_ptr;
+                        }
+                    }
+                }
+                else
+                {
+                    statistics["total_samples"] += 1.0;
+                    statistics["failed_samples"] += 1.0;
+                }
+            }
+            // Put together the results
+            std::chrono::time_point<std::chrono::steady_clock> cur_time = std::chrono::steady_clock::now();
+            std::chrono::duration<double> planning_time(cur_time - start_time);
+            std::vector<std::vector<T>> planned_paths = extract_solution_paths();
+            statistics["planning_time"] = planning_time.count();
+            statistics["solutions"] = (double)planned_paths.size();
+            return std::pair<std::vector<std::vector<T>>, std::map<std::string, double>>(planned_paths, statistics);
+        }
+    };
+
+}
+
+#endif // SIMPLE_RRT_PLANNER

flightlib/third_party/arc_utilities/include/arc_utilities/timing.hpp

@@ -0,0 +1,35 @@
+#ifndef ARC_UTILITIES_TIMING_HPP
+#define ARC_UTILITIES_TIMING_HPP
+
+#include <chrono>
+
+namespace arc_utilities
+{
+    enum StopwatchControl {RESET, READ};
+
+    class Stopwatch
+    {
+        public:
+            Stopwatch()
+                : start_time_(std::chrono::steady_clock::now())
+            {}
+
+            double operator() (const StopwatchControl control = READ)
+            {
+                const auto end_time = std::chrono::steady_clock::now();
+                if (control == RESET)
+                {
+                    start_time_ = end_time;
+                }
+
+                return std::chrono::duration<double>(end_time - start_time_).count();
+            }
+
+        private:
+            std::chrono::steady_clock::time_point start_time_;
+    };
+
+    double GlobalStopwatch(const StopwatchControl control = READ);
+}
+
+#endif // ARC_UTILITIES_TIMING_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/voxel_grid.hpp

@@ -0,0 +1,926 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <vector>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <algorithm>
+#include <Eigen/Geometry>
+#include <stdexcept>
+#include <arc_utilities/arc_helpers.hpp>
+#include <arc_utilities/serialization_eigen.hpp>
+#include <arc_utilities/eigen_helpers.hpp>
+
+#ifndef VOXEL_GRID_HPP
+#define VOXEL_GRID_HPP
+
+namespace VoxelGrid
+{
+    struct GRID_INDEX
+    {
+        int64_t x;
+        int64_t y;
+        int64_t z;
+
+        GRID_INDEX() : x(-1), y(-1), z(-1) {}
+
+        GRID_INDEX(const int64_t in_x, const int64_t in_y, const int64_t in_z) : x(in_x), y(in_y), z(in_z) {}
+
+        bool operator==(const GRID_INDEX& other) const
+        {
+            return (x == other.x && y == other.y && z == other.z);
+        }
+    };
+
+    template<typename T, typename Allocator=std::allocator<T>>
+    class VoxelGrid
+    {
+    protected:
+
+        Eigen::Isometry3d origin_transform_;
+        Eigen::Isometry3d inverse_origin_transform_;
+        T default_value_;
+        T oob_value_;
+        std::vector<T, Allocator> data_;
+        double cell_x_size_;
+        double cell_y_size_;
+        double cell_z_size_;
+        double inv_cell_x_size_;
+        double inv_cell_y_size_;
+        double inv_cell_z_size_;
+        double x_size_;
+        double y_size_;
+        double z_size_;
+        int64_t stride1_;
+        int64_t stride2_;
+        int64_t num_x_cells_;
+        int64_t num_y_cells_;
+        int64_t num_z_cells_;
+        bool initialized_;
+
+        inline int64_t GetDataIndex(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
+        {
+            return (x_index * stride1_) + (y_index * stride2_) + z_index;
+        }
+
+        inline void SetContents(const T& value)
+        {
+            data_.clear();
+            data_.resize(num_x_cells_ * num_y_cells_ * num_z_cells_, value);
+        }
+
+        inline void SafetyCheckSizes(const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size,const double y_size, const double z_size) const
+        {
+            if (cell_x_size <= 0.0)
+            {
+                throw std::invalid_argument("cell_x_size must be positive and non-zero");
+            }
+            if (std::isnan(cell_x_size))
+            {
+                throw std::invalid_argument("cell_x_size must not be NaN");
+            }
+            if (std::isinf(cell_x_size) != 0)
+            {
+                throw std::invalid_argument("cell_x_size must not be INF");
+            }
+            if (cell_y_size <= 0.0)
+            {
+                throw std::invalid_argument("cell_y_size must be positive and non-zero");
+            }
+            if (std::isnan(cell_y_size))
+            {
+                throw std::invalid_argument("cell_y_size must not be NaN");
+            }
+            if (std::isinf(cell_y_size) != 0)
+            {
+                throw std::invalid_argument("cell_y_size must not be INF");
+            }
+            if (cell_z_size <= 0.0)
+            {
+                throw std::invalid_argument("cell_z_size must be positive and non-zero");
+            }
+            if (std::isnan(cell_z_size))
+            {
+                throw std::invalid_argument("cell_z_size must not be NaN");
+            }
+            if (std::isinf(cell_z_size) != 0)
+            {
+                throw std::invalid_argument("cell_z_size must not be INF");
+            }
+            if (x_size <= 0.0)
+            {
+                throw std::invalid_argument("x_size must be positive and non-zero");
+            }
+            if (y_size <= 0.0)
+            {
+                throw std::invalid_argument("y_size must be positive and non-zero");
+            }
+            if (z_size <= 0.0)
+            {
+                throw std::invalid_argument("z_size must be positive and non-zero");
+            }
+            if (std::isnan(x_size))
+            {
+                throw std::invalid_argument("x_size must not be NaN");
+            }
+            if (std::isnan(y_size))
+            {
+                throw std::invalid_argument("y_size must not be NaN");
+            }
+            if (std::isnan(z_size))
+            {
+                throw std::invalid_argument("z_size must not be NaN");
+            }
+            if (std::isinf(x_size) != 0)
+            {
+                throw std::invalid_argument("x_size must not be INF");
+            }
+            if (std::isinf(y_size) != 0)
+            {
+                throw std::invalid_argument("y_size must not be INF");
+            }
+            if (std::isinf(z_size) != 0)
+            {
+                throw std::invalid_argument("z_size must not be INF");
+            }
+        }
+
+        inline void SafetyCheckSizes(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells,const int64_t num_y_cells, const int64_t num_z_cells) const
+        {
+            if (cell_x_size <= 0.0)
+            {
+                throw std::invalid_argument("cell_x_size must be positive and non-zero");
+            }
+            if (std::isnan(cell_x_size))
+            {
+                throw std::invalid_argument("cell_x_size must not be NaN");
+            }
+            if (std::isinf(cell_x_size) != 0)
+            {
+                throw std::invalid_argument("cell_x_size must not be INF");
+            }
+            if (cell_y_size <= 0.0)
+            {
+                throw std::invalid_argument("cell_y_size must be positive and non-zero");
+            }
+            if (std::isnan(cell_y_size))
+            {
+                throw std::invalid_argument("cell_y_size must not be NaN");
+            }
+            if (std::isinf(cell_y_size) != 0)
+            {
+                throw std::invalid_argument("cell_y_size must not be INF");
+            }
+            if (cell_z_size <= 0.0)
+            {
+                throw std::invalid_argument("cell_z_size must be positive and non-zero");
+            }
+            if (std::isnan(cell_z_size))
+            {
+                throw std::invalid_argument("cell_z_size must not be NaN");
+            }
+            if (std::isinf(cell_z_size) != 0)
+            {
+                throw std::invalid_argument("cell_z_size must not be INF");
+            }
+            if (num_x_cells <= 0)
+            {
+                throw std::invalid_argument("num_x_cells must be positive and non-zero");
+            }
+            if (num_y_cells <= 0)
+            {
+                throw std::invalid_argument("num_y_cells must be positive and non-zero");
+            }
+            if (num_z_cells <= 0)
+            {
+                throw std::invalid_argument("num_z_cells must be positive and non-zero");
+            }
+        }
+
+        inline void CoreInitialize(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
+        {
+            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells);
+            cell_x_size_ = fabs(cell_x_size);
+            cell_y_size_ = fabs(cell_y_size);
+            cell_z_size_ = fabs(cell_z_size);
+            inv_cell_x_size_ = 1.0 / cell_x_size_;
+            inv_cell_y_size_ = 1.0 / cell_y_size_;
+            inv_cell_z_size_ = 1.0 / cell_z_size_;
+            num_x_cells_ = num_x_cells;
+            num_y_cells_ = num_y_cells;
+            num_z_cells_ = num_z_cells;
+            x_size_ = (double)num_x_cells_ * cell_x_size_;
+            y_size_ = (double)num_y_cells_ * cell_y_size_;
+            z_size_ = (double)num_z_cells_ * cell_z_size_;
+            default_value_ = default_value;
+            oob_value_ = oob_value;
+            stride1_ = num_y_cells_ * num_z_cells_;
+            stride2_ = num_z_cells_;
+            SetContents(default_value_);
+        }
+
+    public:
+
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        inline static uint64_t Serialize(const VoxelGrid<T, Allocator>& grid, std::vector<uint8_t>& buffer, const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& value_serializer)
+        {
+            return grid.SerializeSelf(buffer, value_serializer);
+        }
+
+        inline static std::pair<VoxelGrid<T, Allocator>, uint64_t> Deserialize(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
+        {
+            VoxelGrid<T, Allocator> temp_grid;
+            const uint64_t bytes_read = temp_grid.DeserializeSelf(buffer, current, value_deserializer);
+            return std::make_pair(temp_grid, bytes_read);
+        }
+
+        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_size, const double x_size, const double y_size, double const z_size, const T& default_value)
+        {
+            Initialize(origin_transform, cell_size, cell_size, cell_size, x_size, y_size, z_size, default_value, default_value);
+        }
+
+        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_size, const double x_size, const double y_size, const double z_size, const T& default_value, const T& oob_value)
+        {
+            Initialize(origin_transform, cell_size, cell_size, cell_size, x_size, y_size, z_size, default_value, oob_value);
+        }
+
+        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, double const z_size, const T& default_value)
+        {
+            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size, default_value, default_value);
+        }
+
+        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, const double z_size, const T& default_value, const T& oob_value)
+        {
+            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size, default_value, oob_value);
+        }
+
+        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value)
+        {
+            Initialize(origin_transform, cell_size, cell_size, cell_size, num_x_cells, num_y_cells, num_z_cells, default_value, default_value);
+        }
+
+        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
+        {
+            Initialize(origin_transform, cell_size, cell_size, cell_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
+        }
+
+        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value)
+        {
+            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, default_value);
+        }
+
+        VoxelGrid(const Eigen::Isometry3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
+        {
+            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
+        }
+
+        VoxelGrid(const double cell_size, const double x_size, const double y_size, const double z_size, const T& default_value)
+        {
+            Initialize(cell_size, cell_size, cell_size, x_size, y_size, z_size, default_value, default_value);
+        }
+
+        VoxelGrid(const double cell_size, const double x_size, const double y_size, const double z_size, const T& default_value, const T& oob_value)
+        {
+            Initialize(cell_size, cell_size, cell_size, x_size, y_size, z_size, default_value, oob_value);
+        }
+
+        VoxelGrid(const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, const double z_size, const T& default_value)
+        {
+            Initialize(cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size, default_value, default_value);
+        }
+
+        VoxelGrid(const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, const double z_size, const T& default_value, const T& oob_value)
+        {
+            Initialize(cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size, default_value, oob_value);
+        }
+
+        VoxelGrid(const double cell_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value)
+        {
+            Initialize(cell_size, cell_size, cell_size, num_x_cells, num_y_cells, num_z_cells, default_value, default_value);
+        }
+
+        VoxelGrid(const double cell_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
+        {
+            Initialize(cell_size, cell_size, cell_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
+        }
+
+        VoxelGrid(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value)
+        {
+            Initialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, default_value);
+        }
+
+        VoxelGrid(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
+        {
+            Initialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
+        }
+
+        VoxelGrid()
+        {
+            origin_transform_ = Eigen::Isometry3d::Identity();
+            inverse_origin_transform_ = origin_transform_.inverse();
+            arc_helpers::RequireAlignment(origin_transform_, 16u);
+            arc_helpers::RequireAlignment(inverse_origin_transform_, 16u);
+            cell_x_size_ = 0.0;
+            cell_y_size_ = 0.0;
+            cell_z_size_ = 0.0;
+            inv_cell_x_size_ = 0.0;
+            inv_cell_y_size_ = 0.0;
+            inv_cell_z_size_ = 0.0;
+            x_size_ = 0.0;
+            y_size_ = 0.0;
+            z_size_ = 0.0;
+            num_x_cells_ = 0;
+            num_y_cells_ = 0;
+            num_z_cells_ = 0;
+            stride1_ = num_y_cells_ * num_z_cells_;
+            stride2_ = num_z_cells_;
+            data_.clear();
+            initialized_ = false;
+        }
+
+        inline void Initialize(const Eigen::Isometry3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, double const z_size, const T& default_value, const T& oob_value)
+        {
+            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size);
+            int64_t num_x_cells = (int64_t)(ceil(fabs(x_size) / fabs(cell_x_size)));
+            int64_t num_y_cells = (int64_t)(ceil(fabs(y_size) / fabs(cell_y_size)));
+            int64_t num_z_cells = (int64_t)(ceil(fabs(z_size) / fabs(cell_z_size)));
+            Initialize(origin_transform, cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
+        }
+
+        inline void Initialize(const Eigen::Isometry3d& origin_transform, const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
+        {
+            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells);
+            CoreInitialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
+            origin_transform_ = origin_transform;
+            inverse_origin_transform_ = origin_transform_.inverse();
+            arc_helpers::RequireAlignment(origin_transform_, 16u);
+            arc_helpers::RequireAlignment(inverse_origin_transform_, 16u);
+            initialized_ = true;
+        }
+
+        inline void Initialize(const double cell_x_size, const double cell_y_size, const double cell_z_size, const double x_size, const double y_size, double const z_size, const T& default_value, const T& oob_value)
+        {
+            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, x_size, y_size, z_size);
+            int64_t num_x_cells = (int64_t)(ceil(fabs(x_size) / fabs(cell_x_size)));
+            int64_t num_y_cells = (int64_t)(ceil(fabs(y_size) / fabs(cell_y_size)));
+            int64_t num_z_cells = (int64_t)(ceil(fabs(z_size) / fabs(cell_z_size)));
+            Initialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
+        }
+
+        inline void Initialize(const double cell_x_size, const double cell_y_size, const double cell_z_size, const int64_t num_x_cells, const int64_t num_y_cells, const int64_t num_z_cells, const T& default_value, const T& oob_value)
+        {
+            SafetyCheckSizes(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells);
+            CoreInitialize(cell_x_size, cell_y_size, cell_z_size, num_x_cells, num_y_cells, num_z_cells, default_value, oob_value);
+            const Eigen::Translation3d origin_translation(-x_size_ * 0.5, -y_size_ * 0.5, -z_size_ * 0.5);
+            const Eigen::Isometry3d origin_transform = origin_translation * Eigen::Quaterniond::Identity();
+            origin_transform_ = origin_transform;
+            inverse_origin_transform_ = origin_transform_.inverse();
+            arc_helpers::RequireAlignment(origin_transform_, 16u);
+            arc_helpers::RequireAlignment(inverse_origin_transform_, 16u);
+            initialized_ = true;
+        }
+
+        inline uint64_t SerializeSelf(std::vector<uint8_t>& buffer, const std::function<uint64_t(const T&, std::vector<uint8_t>&)>& value_serializer) const
+        {
+            const uint64_t start_buffer_size = buffer.size();
+            // Serialize the initialized
+            arc_utilities::SerializeFixedSizePOD<uint8_t>((uint8_t)initialized_, buffer);
+            // Serialize the transforms
+            arc_utilities::SerializeEigenType<Eigen::Isometry3d>(origin_transform_, buffer);
+            arc_utilities::SerializeEigenType<Eigen::Isometry3d>(inverse_origin_transform_, buffer);
+            // Serialize the data
+            arc_utilities::SerializeVector<T, Allocator>(data_, buffer, value_serializer);
+            // Serialize the cell sizes
+            arc_utilities::SerializeFixedSizePOD<double>(cell_x_size_, buffer);
+            arc_utilities::SerializeFixedSizePOD<double>(cell_y_size_, buffer);
+            arc_utilities::SerializeFixedSizePOD<double>(cell_z_size_, buffer);
+            arc_utilities::SerializeFixedSizePOD<double>(inv_cell_x_size_, buffer);
+            arc_utilities::SerializeFixedSizePOD<double>(inv_cell_y_size_, buffer);
+            arc_utilities::SerializeFixedSizePOD<double>(inv_cell_z_size_, buffer);
+            // Serialize the grid sizes
+            arc_utilities::SerializeFixedSizePOD<double>(x_size_, buffer);
+            arc_utilities::SerializeFixedSizePOD<double>(y_size_, buffer);
+            arc_utilities::SerializeFixedSizePOD<double>(z_size_, buffer);
+            // Serialize the control/bounds values
+            arc_utilities::SerializeFixedSizePOD<int64_t>(stride1_, buffer);
+            arc_utilities::SerializeFixedSizePOD<int64_t>(stride2_, buffer);
+            arc_utilities::SerializeFixedSizePOD<int64_t>(num_x_cells_, buffer);
+            arc_utilities::SerializeFixedSizePOD<int64_t>(num_y_cells_, buffer);
+            arc_utilities::SerializeFixedSizePOD<int64_t>(num_z_cells_, buffer);
+            // Serialize the default value
+            value_serializer(default_value_, buffer);
+            // Serialize the OOB value
+            value_serializer(oob_value_, buffer);
+            // Figure out how many bytes were written
+            const uint64_t end_buffer_size = buffer.size();
+            const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+            return bytes_written;
+        }
+
+        inline uint64_t DeserializeSelf(const std::vector<uint8_t>& buffer, const uint64_t current, const std::function<std::pair<T, uint64_t>(const std::vector<uint8_t>&, const uint64_t)>& value_deserializer)
+        {
+            uint64_t current_position = current;
+            // Deserialize the initialized
+            const std::pair<uint8_t, uint64_t> initialized_deserialized = arc_utilities::DeserializeFixedSizePOD<uint8_t>(buffer, current_position);
+            initialized_ = (bool)initialized_deserialized.first;
+            current_position += initialized_deserialized.second;
+            // Deserialize the transforms
+            const std::pair<Eigen::Isometry3d, uint64_t> origin_transform_deserialized = arc_utilities::DeserializeEigenType<Eigen::Isometry3d>(buffer, current_position);
+            origin_transform_ = origin_transform_deserialized.first;
+            current_position += origin_transform_deserialized.second;
+            const std::pair<Eigen::Isometry3d, uint64_t> inverse_origin_transform_deserialized = arc_utilities::DeserializeEigenType<Eigen::Isometry3d>(buffer, current_position);
+            inverse_origin_transform_ = inverse_origin_transform_deserialized.first;
+            current_position += inverse_origin_transform_deserialized.second;
+            // Deserialize the data
+            const std::pair<std::vector<T, Allocator>, uint64_t> data_deserialized = arc_utilities::DeserializeVector<T, Allocator>(buffer, current_position, value_deserializer);
+            data_ = data_deserialized.first;
+            current_position += data_deserialized.second;
+            // Deserialize the cell sizes
+            const std::pair<double, uint64_t> cell_x_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+            cell_x_size_ = cell_x_size_deserialized.first;
+            current_position += cell_x_size_deserialized.second;
+            const std::pair<double, uint64_t> cell_y_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+            cell_y_size_ = cell_y_size_deserialized.first;
+            current_position += cell_y_size_deserialized.second;
+            const std::pair<double, uint64_t> cell_z_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+            cell_z_size_ = cell_z_size_deserialized.first;
+            current_position += cell_z_size_deserialized.second;
+            const std::pair<double, uint64_t> inv_cell_x_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+            inv_cell_x_size_ = inv_cell_x_size_deserialized.first;
+            current_position += inv_cell_x_size_deserialized.second;
+            const std::pair<double, uint64_t> inv_cell_y_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+            inv_cell_y_size_ = inv_cell_y_size_deserialized.first;
+            current_position += inv_cell_y_size_deserialized.second;
+            const std::pair<double, uint64_t> inv_cell_z_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+            inv_cell_z_size_ = inv_cell_z_size_deserialized.first;
+            current_position += inv_cell_z_size_deserialized.second;
+            // Deserialize the grid sizes
+            const std::pair<double, uint64_t> x_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+            x_size_ = x_size_deserialized.first;
+            current_position += x_size_deserialized.second;
+            const std::pair<double, uint64_t> y_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+            y_size_ = y_size_deserialized.first;
+            current_position += y_size_deserialized.second;
+            const std::pair<double, uint64_t> z_size_deserialized = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+            z_size_ = z_size_deserialized.first;
+            current_position += z_size_deserialized.second;
+            // Deserialize the control/bounds values
+            const std::pair<int64_t, uint64_t> stride1_deserialized = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
+            stride1_ = stride1_deserialized.first;
+            current_position += stride1_deserialized.second;
+            const std::pair<int64_t, uint64_t> stride2_deserialized = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
+            stride2_ = stride2_deserialized.first;
+            current_position += stride2_deserialized.second;
+            const std::pair<int64_t, uint64_t> num_x_cells_deserialized = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
+            num_x_cells_ = num_x_cells_deserialized.first;
+            current_position += num_x_cells_deserialized.second;
+            const std::pair<int64_t, uint64_t> num_y_cells_deserialized = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
+            num_y_cells_ = num_y_cells_deserialized.first;
+            current_position += num_y_cells_deserialized.second;
+            const std::pair<int64_t, uint64_t> num_z_cells_deserialized = arc_utilities::DeserializeFixedSizePOD<int64_t>(buffer, current_position);
+            num_z_cells_ = num_z_cells_deserialized.first;
+            current_position += num_z_cells_deserialized.second;
+            // Deserialize the default value
+            const std::pair<T, uint64_t> default_value_deserialized = value_deserializer(buffer, current_position);
+            default_value_ = default_value_deserialized.first;
+            current_position += default_value_deserialized.second;
+            // Deserialize the OOB value
+            const std::pair<T, uint64_t> oob_value_deserialized = value_deserializer(buffer, current_position);
+            oob_value_ = oob_value_deserialized.first;
+            current_position += oob_value_deserialized.second;
+            // Figure out how many bytes were read
+            const uint64_t bytes_read = current_position - current;
+            return bytes_read;
+        }
+
+        inline bool IsInitialized() const
+        {
+            return initialized_;
+        }
+
+        inline void ResetWithDefault()
+        {
+            SetContents(default_value_);
+        }
+
+        inline void ResetWithNewValue(const T& new_value)
+        {
+            SetContents(new_value);
+        }
+
+        inline void ResetWithNewDefault(const T& new_default)
+        {
+            default_value_ = new_default;
+            SetContents(default_value_);
+        }
+
+        inline bool IndexInBounds(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
+        {
+            if (x_index >= 0 && y_index >= 0 && z_index >= 0 && x_index < num_x_cells_ && y_index < num_y_cells_ && z_index < num_z_cells_)
+            {
+                return true;
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        inline std::pair<const T&, bool> GetImmutable3d(const Eigen::Vector3d& location) const
+        {
+            assert(initialized_);
+            const std::vector<int64_t> indices = LocationToGridIndex3d(location);
+            if (indices.size() == 3)
+            {
+                return GetImmutable(indices[0], indices[1], indices[2]);
+            }
+            else
+            {
+                return std::pair<const T&, bool>(oob_value_, false);
+            }
+        }
+
+        inline std::pair<const T&, bool> GetImmutable4d(const Eigen::Vector4d& location) const
+        {
+            assert(initialized_);
+            const std::vector<int64_t> indices = LocationToGridIndex4d(location);
+            if (indices.size() == 3)
+            {
+                return GetImmutable(indices[0], indices[1], indices[2]);
+            }
+            else
+            {
+                return std::pair<const T&, bool>(oob_value_, false);
+            }
+        }
+
+        inline std::pair<const T&, bool> GetImmutable(const double x, const double y, const double z) const
+        {
+            const Eigen::Vector4d location(x, y, z, 1.0);
+            return GetImmutable4d(location);
+        }
+
+        inline std::pair<const T&, bool> GetImmutable(const GRID_INDEX& index) const
+        {
+            return GetImmutable(index.x, index.y, index.z);
+        }
+
+        inline std::pair<const T&, bool> GetImmutable(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
+        {
+            assert(initialized_);
+            if (IndexInBounds(x_index, y_index, z_index))
+            {
+                const int64_t data_index = GetDataIndex(x_index, y_index, z_index);
+                assert(data_index >= 0 && data_index < (int64_t)data_.size());
+                return std::pair<const T&, bool>(data_[data_index], true);
+            }
+            else
+            {
+                return std::pair<const T&, bool>(oob_value_, false);
+            }
+        }
+
+        inline std::pair<T&, bool> GetMutable3d(const Eigen::Vector3d& location)
+        {
+            assert(initialized_);
+            const std::vector<int64_t> indices = LocationToGridIndex3d(location);
+            if (indices.size() == 3)
+            {
+                return GetMutable(indices[0], indices[1], indices[2]);
+            }
+            else
+            {
+                return std::pair<T&, bool>(oob_value_, false);
+            }
+        }
+
+        inline std::pair<T&, bool> GetMutable4d(const Eigen::Vector4d& location)
+        {
+            assert(initialized_);
+            const std::vector<int64_t> indices = LocationToGridIndex4d(location);
+            if (indices.size() == 3)
+            {
+                return GetMutable(indices[0], indices[1], indices[2]);
+            }
+            else
+            {
+                return std::pair<T&, bool>(oob_value_, false);
+            }
+        }
+
+        inline std::pair<T&, bool> GetMutable(const double x, const double y, const double z)
+        {
+            const Eigen::Vector4d location(x, y, z, 1.0);
+            return GetMutable4d(location);
+        }
+
+        inline std::pair<T&, bool> GetMutable(const GRID_INDEX& index)
+        {
+            return GetMutable(index.x, index.y, index.z);
+        }
+
+        inline std::pair<T&, bool> GetMutable(const int64_t x_index, const int64_t y_index, const int64_t z_index)
+        {
+            assert(initialized_);
+            if (IndexInBounds(x_index, y_index, z_index))
+            {
+                const int64_t data_index = GetDataIndex(x_index, y_index, z_index);
+                assert(data_index >= 0 && data_index < (int64_t)data_.size());
+                return std::pair<T&, bool>(data_[data_index], true);
+            }
+            else
+            {
+                return std::pair<T&, bool>(oob_value_, false);
+            }
+        }
+
+        inline bool SetValue3d(const Eigen::Vector3d& location, const T& value)
+        {
+            assert(initialized_);
+            const std::vector<int64_t> indices = LocationToGridIndex3d(location);
+            if (indices.size() == 3)
+            {
+                return SetValue(indices[0], indices[1], indices[2], value);
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        inline bool SetValue4d(const Eigen::Vector4d& location, const T& value)
+        {
+            assert(initialized_);
+            const std::vector<int64_t> indices = LocationToGridIndex4d(location);
+            if (indices.size() == 3)
+            {
+                return SetValue(indices[0], indices[1], indices[2], value);
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        inline bool SetValue(const double x, const double y, const double z, const T& value)
+        {
+            const Eigen::Vector4d location(x, y, z, 1.0);
+            return SetValue4d(location, value);
+        }
+
+        inline bool SetValue(const GRID_INDEX& index, const T& value)
+        {
+            return SetValue(index.x, index.y, index.z, value);
+        }
+
+        inline bool SetValue(const int64_t x_index, const int64_t y_index, const int64_t z_index, const T& value)
+        {
+            assert(initialized_);
+            if (IndexInBounds(x_index, y_index, z_index))
+            {
+                const int64_t data_index = GetDataIndex(x_index, y_index, z_index);
+                assert(data_index >= 0 && data_index < (int64_t)data_.size());
+                data_[data_index] = value;
+                return true;
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        inline bool SetValue3d(const Eigen::Vector3d& location, T&& value)
+        {
+            assert(initialized_);
+            const std::vector<int64_t> indices = LocationToGridIndex3d(location);
+            if (indices.size() == 3)
+            {
+                return SetValue(indices[0], indices[1], indices[2], value);
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        inline bool SetValue4d(const Eigen::Vector4d& location, T&& value)
+        {
+            assert(initialized_);
+            const std::vector<int64_t> indices = LocationToGridIndex4d(location);
+            if (indices.size() == 3)
+            {
+                return SetValue(indices[0], indices[1], indices[2], value);
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        inline bool SetValue(const double x, const double y, const double z, T&& value)
+        {
+            const Eigen::Vector4d location(x, y, z, 1.0);
+            return SetValue4d(location, value);
+        }
+
+        inline bool SetValue(const GRID_INDEX& index, T&& value)
+        {
+            return SetValue(index.x, index.y, index.z, value);
+        }
+
+        inline bool SetValue(const int64_t x_index, const int64_t y_index, const int64_t z_index, T&& value)
+        {
+            assert(initialized_);
+            if (IndexInBounds(x_index, y_index, z_index))
+            {
+                const int64_t data_index = GetDataIndex(x_index, y_index, z_index);
+                assert(data_index >= 0 && data_index < (int64_t)data_.size());
+                data_[data_index] = value;
+                return true;
+            }
+            else
+            {
+                return false;
+            }
+        }
+
+        inline double GetXSize() const
+        {
+            return x_size_;
+        }
+
+        inline double GetYSize() const
+        {
+            return y_size_;
+        }
+
+        inline double GetZSize() const
+        {
+            return z_size_;
+        }
+
+        inline std::vector<double> GetCellSizes() const
+        {
+            return std::vector<double>{cell_x_size_, cell_y_size_, cell_z_size_};
+        }
+
+        inline T GetDefaultValue() const
+        {
+            return default_value_;
+        }
+
+        inline T GetOOBValue() const
+        {
+            return oob_value_;
+        }
+
+        inline void SetDefaultValue(const T& default_value)
+        {
+            default_value_ = default_value;
+        }
+
+        inline void SetOOBValue(const T& oob_value)
+        {
+            oob_value_ = oob_value;
+        }
+
+        inline int64_t GetNumXCells() const
+        {
+            return num_x_cells_;
+        }
+
+        inline int64_t GetNumYCells() const
+        {
+            return num_y_cells_;
+        }
+
+        inline int64_t GetNumZCells() const
+        {
+            return num_z_cells_;
+        }
+
+        inline const Eigen::Isometry3d& GetOriginTransform() const
+        {
+            return origin_transform_;
+        }
+
+        inline const Eigen::Isometry3d& GetInverseOriginTransform() const
+        {
+            return inverse_origin_transform_;
+        }
+
+        inline std::vector<int64_t> LocationToGridIndex(const double x, const double y, const double z) const
+        {
+            const Eigen::Vector4d point(x, y, z, 1.0);
+            return LocationToGridIndex4d(point);
+        }
+
+        inline std::vector<int64_t> LocationToGridIndex3d(const Eigen::Vector3d& location) const
+        {
+            assert(initialized_);
+            const Eigen::Vector3d point_in_grid_frame = inverse_origin_transform_ * location;
+            const int64_t x_cell = (int64_t)(point_in_grid_frame.x() * inv_cell_x_size_);
+            const int64_t y_cell = (int64_t)(point_in_grid_frame.y() * inv_cell_y_size_);
+            const int64_t z_cell = (int64_t)(point_in_grid_frame.z() * inv_cell_z_size_);
+            if (IndexInBounds(x_cell, y_cell, z_cell))
+            {
+                return std::vector<int64_t>{x_cell, y_cell, z_cell};
+            }
+            else
+            {
+                return std::vector<int64_t>();
+            }
+        }
+
+        inline std::vector<int64_t> LocationToGridIndex4d(const Eigen::Vector4d& location) const
+        {
+            assert(initialized_);
+            const Eigen::Vector4d point_in_grid_frame = inverse_origin_transform_ * location;
+            const int64_t x_cell = (int64_t)(point_in_grid_frame(0) * inv_cell_x_size_);
+            const int64_t y_cell = (int64_t)(point_in_grid_frame(1) * inv_cell_y_size_);
+            const int64_t z_cell = (int64_t)(point_in_grid_frame(2) * inv_cell_z_size_);
+            if (IndexInBounds(x_cell, y_cell, z_cell))
+            {
+                return std::vector<int64_t>{x_cell, y_cell, z_cell};
+            }
+            else
+            {
+                return std::vector<int64_t>();
+            }
+        }
+
+        inline std::vector<double> GridIndexToLocation(const GRID_INDEX& index) const
+        {
+            return GridIndexToLocation(index.x, index.y, index.z);
+        }
+
+        inline std::vector<double> GridIndexToLocation(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
+        {
+            assert(initialized_);
+            if (IndexInBounds(x_index, y_index, z_index))
+            {
+                const Eigen::Vector3d point_in_grid_frame(cell_x_size_ * ((double)x_index + 0.5), cell_y_size_ * ((double)y_index + 0.5), cell_z_size_ * ((double)z_index + 0.5));
+                const Eigen::Vector3d point = origin_transform_ * point_in_grid_frame;
+                return std::vector<double>{point.x(), point.y(), point.z()};
+            }
+            else
+            {
+                return std::vector<double>();
+            }
+        }
+
+        inline const std::vector<T>& GetRawData() const
+        {
+            return data_;
+        }
+
+        inline std::vector<T> CopyRawData() const
+        {
+            return data_;
+        }
+
+        inline bool SetRawData(std::vector<T>& data)
+        {
+            assert(initialized_);
+            const int64_t expected_length = num_x_cells_ * num_y_cells_ * num_z_cells_;
+            if ((int64_t)data.size() == expected_length)
+            {
+                data_ = data;
+                return true;
+            }
+            else
+            {
+                std::cerr << "Failed to load internal data - expected " << expected_length << " got " << data.size() << std::endl;
+                return false;
+            }
+        }
+
+        inline uint64_t HashDataIndex(const int64_t x_index, const int64_t y_index, const int64_t z_index) const
+        {
+            return (x_index * stride1_) + (y_index * stride2_) + z_index;
+        }
+    };
+}
+
+namespace std
+{
+    template <>
+    struct hash<VoxelGrid::GRID_INDEX>
+    {
+        std::size_t operator()(const VoxelGrid::GRID_INDEX& index) const
+        {
+            using std::size_t;
+            using std::hash;
+            return ((std::hash<int64_t>()(index.x) ^ (std::hash<int64_t>()(index.y) << 1) >> 1) ^ (std::hash<int64_t>()(index.z) << 1));
+        }
+    };
+}
+
+inline std::ostream& operator<<(std::ostream& strm, const VoxelGrid::GRID_INDEX& index)
+{
+    strm << "GridIndex: " << index.x << "," << index.y << "," << index.z;
+    return strm;
+}
+
+#endif // VOXEL_GRID_HPP

flightlib/third_party/arc_utilities/include/arc_utilities/zlib_helpers.hpp

@@ -0,0 +1,25 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <vector>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <fstream>
+#include <stdexcept>
+#include <zlib.h>
+
+#ifndef ZLIB_HELPERS_HPP
+#define ZLIB_HELPERS_HPP
+
+namespace ZlibHelpers
+{
+    std::vector<uint8_t> DecompressBytes(const std::vector<uint8_t>& compressed);
+
+    std::vector<uint8_t> CompressBytes(const std::vector<uint8_t>& uncompressed);
+
+    std::vector<uint8_t> LoadFromFileAndDecompress(const std::string& path);
+
+    void CompressAndWriteToFile(const std::vector<uint8_t>& uncompressed, const std::string& path);
+}
+
+#endif // ZLIB_HELPERS_HPP