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

flightlib/third_party/arc_utilities/.gitignore

@@ -0,0 +1,9 @@
+# editor temporary files
+*.swp
+*~
+
+# Vim Ctags file
+tags
+
+# Compiled python files
+*.pyc

flightlib/third_party/arc_utilities/CMakeLists.txt

@@ -0,0 +1,143 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(arc_utilities)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs geometry_msgs)
+find_package(cmake_modules REQUIRED)
+find_package(Eigen3 REQUIRED)
+set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS filesystem)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/groovy/api/catkin/html/user_guide/setup_dot_py.html
+catkin_python_setup()
+
+#######################################
+## Declare ROS messages and services ##
+#######################################
+
+## Generate messages in the 'msg' folder
+# add_message_files(DIRECTORY msg FILES )
+
+## Generate services in the 'srv' folder
+# add_service_files(DIRECTORY srv FILES )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(DEPENDENCIES geometry_msgs std_msgs)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS roscpp rospy std_msgs sensor_msgs geometry_msgs DEPENDS Eigen3)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(include SYSTEM ${catkin_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS})
+
+set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS} -O3 -Wall -Wextra -Wconversion -Werror")
+
+# Utility library
+add_library(${PROJECT_NAME}
+    include/${PROJECT_NAME}/log.hpp
+    include/${PROJECT_NAME}/maybe.hpp
+    include/${PROJECT_NAME}/ros_helpers.hpp
+    include/${PROJECT_NAME}/abb_irb1600_145_fk_fast.hpp
+    include/${PROJECT_NAME}/iiwa_7_fk_fast.hpp
+    include/${PROJECT_NAME}/iiwa_14_fk_fast.hpp
+    include/${PROJECT_NAME}/arc_exceptions.hpp
+    include/${PROJECT_NAME}/arc_helpers.hpp
+    include/${PROJECT_NAME}/pretty_print.hpp
+    include/${PROJECT_NAME}/zlib_helpers.hpp
+    include/${PROJECT_NAME}/base64_helpers.hpp
+    include/${PROJECT_NAME}/voxel_grid.hpp
+    include/${PROJECT_NAME}/dynamic_spatial_hashed_voxel_grid.hpp
+    include/${PROJECT_NAME}/aligned_eigen_types.hpp
+    include/${PROJECT_NAME}/eigen_helpers.hpp
+    include/${PROJECT_NAME}/eigen_helpers_conversions.hpp
+    include/${PROJECT_NAME}/simple_rrt_planner.hpp
+    include/${PROJECT_NAME}/simple_astar_planner.hpp
+    include/${PROJECT_NAME}/simple_prm_planner.hpp
+    include/${PROJECT_NAME}/simple_kmeans_clustering.hpp
+    include/${PROJECT_NAME}/simple_hierarchical_clustering.hpp
+    include/${PROJECT_NAME}/simple_hausdorff_distance.hpp
+    include/${PROJECT_NAME}/simple_dtw.hpp
+    include/${PROJECT_NAME}/timing.hpp
+    include/${PROJECT_NAME}/dijkstras.hpp
+    include/${PROJECT_NAME}/shortcut_smoothing.hpp
+    include/${PROJECT_NAME}/first_order_deformation.h
+    include/${PROJECT_NAME}/get_neighbours.hpp
+    include/${PROJECT_NAME}/serialization.hpp
+    include/${PROJECT_NAME}/serialization_eigen.hpp
+    include/${PROJECT_NAME}/serialization_ros.hpp
+    include/${PROJECT_NAME}/filesystem.hpp
+    src/${PROJECT_NAME}/zlib_helpers.cpp
+    src/${PROJECT_NAME}/base64_helpers.cpp
+    src/timing.cpp
+    src/${PROJECT_NAME}/first_order_deformation.cpp)
+add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} z)
+
+# Simple test node for simple_hierarchical_clustering
+add_executable(test_hierarchical_clustering src/test_hierarchical_clustering.cpp)
+add_dependencies(test_hierarchical_clustering ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
+target_link_libraries(test_hierarchical_clustering ${PROJECT_NAME} ${catkin_LIBRARIES})
+
+# Simple test node for utility functions
+add_executable(test_arc_utilities src/test_arc_utilities.cpp)
+add_dependencies(test_arc_utilities ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
+target_link_libraries(test_arc_utilities ${PROJECT_NAME} ${catkin_LIBRARIES})
+
+# # Simple test node for Eigen3 math
+# add_executable(test_eigen_math src/test_eigen_math.cpp)
+# add_dependencies(test_eigen_math ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
+# target_link_libraries(test_eigen_math ${PROJECT_NAME} ${catkin_LIBRARIES})
+
+# # Simple test node for averaging math
+# add_executable(test_averaging src/test_averaging.cpp)
+# add_dependencies(test_averaging ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
+# target_link_libraries(test_averaging ${PROJECT_NAME} ${catkin_LIBRARIES})
+
+# # Simple test node for Dijkstras
+# add_executable(test_dijkstras src/test_dijkstras.cpp)
+# add_dependencies(test_dijkstras ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
+# target_link_libraries(test_dijkstras ${PROJECT_NAME} ${catkin_LIBRARIES})
+
+# # Simple test node for shortcut smoothing
+# add_executable(test_shortcut_smoothing src/test_shortcut_smoothing.cpp)
+# add_dependencies(test_shortcut_smoothing ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
+# target_link_libraries(test_shortcut_smoothing ${PROJECT_NAME} ${catkin_LIBRARIES})
+
+# # Simple test node for DistanceToLine function
+# add_executable(test_closest_point src/test_closest_point.cpp)
+# add_dependencies(test_closest_point ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # ${PROJECT_NAME}_gencpp)
+# target_link_libraries(test_closest_point ${PROJECT_NAME} ${catkin_LIBRARIES})
+
+#############
+## Install ##
+#############
+
+## Mark library for installation
+install(TARGETS ${PROJECT_NAME}
+    ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+    RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+    DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+    PATTERN ".svn" EXCLUDE
+)

flightlib/third_party/arc_utilities/LICENSE

@@ -0,0 +1,24 @@
+Copyright (c) 2015, Autonomous Robotic Collaboration Laboratory
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+

flightlib/third_party/arc_utilities/README.md

@@ -0,0 +1,2 @@
+# arc_utilities
+C++ and Python utilities used in lab projects

flightlib/third_party/arc_utilities/__init__.py

@@ -0,0 +1 @@
+__author__ = 'calderpg'

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

flightlib/third_party/arc_utilities/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<package>
+  <name>arc_utilities</name>
+  <version>0.0.1</version>
+  <description>C++ and Python utilities used in lab projects</description>
+  <maintainer email="calder.pg@gmail.com">Calder Phillips-Grafflin</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

flightlib/third_party/arc_utilities/setup.py

@@ -0,0 +1,11 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup( 
+    packages=['arc_utilities'],
+    package_dir={'': 'src'}
+)
+
+setup(**d)

flightlib/third_party/arc_utilities/src/__init__.py

@@ -0,0 +1 @@
+__author__ = 'calderpg'

flightlib/third_party/arc_utilities/src/arc_utilities/__init__.py

@@ -0,0 +1,4 @@
+import color_mapping
+import transformation_helper
+import extra_functions_to_be_put_in_the_right_place
+import numpy_conversions

flightlib/third_party/arc_utilities/src/arc_utilities/base64_helpers.cpp

@@ -0,0 +1,97 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <array>
+#include <vector>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <fstream>
+#include <stdexcept>
+#include <zlib.h>
+#include "arc_utilities/base64_helpers.hpp"
+
+namespace Base64Helpers
+{
+    /*
+     * Implementations derived from post at http://stackoverflow.com/a/41094722
+     */
+
+    static const std::array<int32_t, 256> B64IndexTable = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                                                           0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                                                           0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 62, 63, 62, 62,
+                                                           63, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 0, 0,
+                                                           0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10,
+                                                           11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22,
+                                                           23, 24, 25, 0, 0, 0, 0, 63, 0, 26, 27, 28, 29,
+                                                           30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41,
+                                                           42, 43, 44, 45, 46, 47, 48, 49, 50, 51 };
+
+    std::vector<uint8_t> Decode(const std::string& encoded)
+    {
+        const size_t encoded_length = encoded.size();
+        const size_t pad = encoded_length > 0 && (encoded_length % 4 || encoded[encoded_length - 1] == '=');
+        const size_t L = ((encoded_length + 3) / 4 - pad) * 4;
+        std::vector<uint8_t> buffer(L / 4 * 3 + pad, 0x00);
+        for (size_t i = 0, j = 0; i < L; i += 4)
+        {
+            const int32_t n = B64IndexTable[encoded[i]] << 18 | B64IndexTable[encoded[i + 1]] << 12 | B64IndexTable[encoded[i + 2]] << 6 | B64IndexTable[encoded[i + 3]];
+            buffer[j++] = (uint8_t)(n >> 16);
+            buffer[j++] = (uint8_t)(n >> 8 & 0xFF);
+            buffer[j++] = (uint8_t)(n & 0xFF);
+        }
+        if (pad > 0)
+        {
+            int32_t n = B64IndexTable[encoded[L]] << 18 | B64IndexTable[encoded[L + 1]] << 12;
+            buffer[buffer.size() - 1] = (uint8_t)(n >> 16);
+            if (encoded_length > L + 2 && encoded[L + 2] != '=')
+            {
+                n |= B64IndexTable[encoded[L + 2]] << 6;
+                buffer.push_back((uint8_t)(n >> 8 & 0xFF));
+            }
+        }
+        return buffer;
+    }
+
+    static const std::array<uint8_t, 64> B64ValueTable = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P',
+                                                          'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', 'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f',
+                                                          'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v',
+                                                          'w', 'x', 'y', 'z', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '+', '/'};
+
+    std::string Encode(const std::vector<uint8_t>& binary)
+    {
+        const size_t binary_length = binary.size();
+        const size_t encoded_length = 4 * ((binary_length + 2) / 3); /* 3-byte blocks to 4-byte */
+        if (encoded_length < binary_length)
+        {
+            return std::string(); /* integer overflow */
+        }
+        std::string encoded;
+        encoded.resize(encoded_length);
+        size_t input_position = 0;
+        size_t output_position = 0;
+        while (binary_length - input_position >= 3)
+        {
+            encoded[output_position++] = B64ValueTable[binary[input_position + 0] >> 2];
+            encoded[output_position++] = B64ValueTable[((binary[input_position + 0] & 0x03) << 4) | (binary[input_position + 1] >> 4)];
+            encoded[output_position++] = B64ValueTable[((binary[input_position + 1] & 0x0f) << 2) | (binary[input_position + 2] >> 6)];
+            encoded[output_position++] = B64ValueTable[binary[input_position + 2] & 0x3f];
+            input_position += 3;
+        }
+        if (input_position < binary_length)
+        {
+            encoded[output_position++] = B64ValueTable[binary[input_position + 0] >> 2];
+            if ((binary_length - input_position) == 1)
+            {
+                encoded[output_position++] = B64ValueTable[(binary[input_position + 0] & 0x03) << 4];
+                encoded[output_position++] = '=';
+            }
+            else
+            {
+                encoded[output_position++] = B64ValueTable[((binary[input_position + 0] & 0x03) << 4) | (binary[input_position + 1] >> 4)];
+                encoded[output_position++] = B64ValueTable[(binary[input_position + 1] & 0x0f) << 2];
+            }
+            encoded[output_position++] = '=';
+        }
+        return encoded;
+    }
+}

flightlib/third_party/arc_utilities/src/arc_utilities/color_mapping.py

@@ -0,0 +1,199 @@
+#!/usr/bin/env python
+
+import math
+import numpy
+
+
+def convert_rgb_to_xyz(r, g, b):
+    rgb_np = numpy.array([r, g, b])
+    rgb_xyz_cvt_np = numpy.array([[0.4124, 0.2126, 0.0193], [0.3576, 0.7152, 0.1192], [0.1805, 0.0722, 0.9505]])
+    xyz_np = numpy.dot(rgb_np, rgb_xyz_cvt_np)
+    return [xyz_np[0], xyz_np[1], xyz_np[2]]
+
+
+def convert_xyz_to_rgb(x, y, z):
+    xyz_np = numpy.array([x, y, z])
+    rgb_xyz_cvt_np = numpy.array([[0.4124, 0.2126, 0.0193], [0.3576, 0.7152, 0.1192], [0.1805, 0.0722, 0.9505]])
+    xyz_rgb_cvt_np = numpy.linalg.inv(rgb_xyz_cvt_np)
+    rgb_np = numpy.dot(xyz_np, xyz_rgb_cvt_np)
+    return [rgb_np[0], rgb_np[1], rgb_np[2]]
+
+
+def cielab_f(val):
+    if val > (216.0 / 24389.0):
+        return math.pow(val, (1.0 / 3.0))
+    else:
+        return (841.0 / 108.0) * val + (16.0 / 116.0)
+
+
+def cielab_finv(val):
+    if val > (6.0 / 29.0):
+        return val ** 3
+    else:
+        return (108.0 / 841.0) * (val - (16.0 / 116.0))
+
+
+def get_ref_xyz():
+    xref = 95.047
+    yref = 100.0
+    zref = 108.883
+    return [xref, yref, zref]
+
+
+def convert_xyz_to_lab(x, y, z):
+    [xref, yref, zref] = get_ref_xyz()
+    l = 116.0 * (cielab_f(y / yref) - (16.0 / 116.0))
+    a = 500.0 * (cielab_f(x / xref) - cielab_f(y / yref))
+    b = 200.0 * (cielab_f(y / yref) - cielab_f(z / zref))
+    return [l, a, b]
+
+
+def convert_lab_to_xyz(l, a, b):
+    intermediate = (l + 16.0) * (1.0 / 116.0)
+    x = cielab_finv(intermediate + a * (1.0 / 500.0))
+    y = cielab_finv(intermediate)
+    z = cielab_finv(intermediate - b * (1.0 / 200.0))
+    return [x, y, z]
+
+
+def convert_lab_to_msh(l, a, b):
+    m = math.sqrt(l ** 2 + a ** 2 + b ** 2)
+    s = math.acos(l / m)
+    #h = math.atan2(b, a)
+    h = math.atan(b / a)
+    return [m, s, h]
+
+
+def convert_msh_to_lab(m, s, h):
+    l = m * math.cos(s)
+    a = m * math.sin(s) * math.cos(h)
+    b = m * math.sin(s) * math.sin(h)
+    return [l, a, b]
+
+
+def rgb_to_msh(r, g, b):
+    [x, y, z] = convert_rgb_to_xyz(r, g, b)
+    [l, a, b] = convert_xyz_to_lab(x, y, z)
+    [m, s, h] = convert_lab_to_msh(l, a, b)
+    return [m, s, h]
+
+
+def msh_to_rgb(m, s, h):
+    [l, a, b] = convert_msh_to_lab(m, s, h)
+    [x, y, z] = convert_lab_to_xyz(l, a, b)
+    [r, g, b] = convert_xyz_to_rgb(x, y, z)
+    return [r, g, b]
+
+
+def rad_diff(x, y):
+    diff = abs(x - y)
+    if diff > math.pi:
+        return (math.pi * 2.0) - diff
+    else:
+        return diff
+
+
+def adjust_hue((m_saturated, s_saturated, h_saturated), m_unsaturated):
+    if m_saturated >= m_unsaturated:
+        return h_saturated
+    else:
+        h_spin = (s_saturated * math.sqrt(m_unsaturated ** 2 - m_saturated ** 2)) / (m_saturated * math.sin(s_saturated))
+        if h_saturated > -(math.pi / 3.0):
+            return h_saturated + h_spin
+        else:
+            return h_saturated - h_spin
+
+
+def interpolate_coolwarm((r1, g1, b1), (r2, g2, b2), interpolation):
+    assert(0.0 <= interpolation <= 1.0)
+    [m1, s1, h1] = rgb_to_msh(r1, g1, b1)
+    [m2, s2, h2] = rgb_to_msh(r2, g2, b2)
+    # If points saturated and distinct, place white in middle
+    if (s1 > 0.05) and (s2 > 0.05) and (rad_diff(h1, h2) > (math.pi / 3.0)):
+        m_mid = max((m1, m2, 88.0))
+        if interpolation < 0.5:
+            m2 = m_mid
+            s2 = 0.0
+            h2 = 0.0
+            interpolation *= 2.0
+        else:
+            m1 = m_mid
+            s1 = 0.0
+            h1 = 0.0
+            interpolation = 2.0 * interpolation - 1.0
+    # Adjust hue of unsaturated colors
+    if (s1 < 0.05) and (s2 > 0.05):
+        h1 = adjust_hue((m2, s2, h2), m1)
+    elif (s2 < 0.05) and (s1 > 0.05):
+        h2 = adjust_hue((m1, s1, h1), m2)
+    # Linear interpolation on adjusted control points
+    m_mid = ((1.0 - interpolation) * m1) + (interpolation * m2)
+    s_mid = ((1.0 - interpolation) * s1) + (interpolation * s2)
+    h_mid = ((1.0 - interpolation) * h1) + (interpolation * h2)
+    return msh_to_rgb(m_mid, s_mid, h_mid)
+
+
+def jet_interpolate(value, y0, x0, y1, x1):
+    return (value - x0) * (y1 - y0) / (x1 - x0) + y0
+
+
+def jet_base(value):
+    if value <= -0.75:
+        return 0.0
+    elif value <= -0.25:
+        return jet_interpolate(value, 0.0, -0.75, 1.0, -0.25)
+    elif value <= 0.25:
+        return 1.0
+    elif value <= 0.75:
+        return jet_interpolate(value, 1.0, 0.25, 0.0, 0.75)
+    else:
+        return 0.0
+
+
+def interpolate_jet(value, use_negative_range=False):
+    if use_negative_range:
+        assert(-1.0 <= value <= 1.0)
+        r = jet_base(value - 0.5)
+        g = jet_base(value)
+        b = jet_base(value + 0.5)
+        return [r, g, b]
+    else:
+        assert(0.0 <= value <= 1.0)
+        if value > 0.5:
+            value = (value - 0.5) * 2.0
+        elif value < 0.5:
+            value = -(0.5 - value) * 2.0
+        else:
+            value = 0.0
+        r = jet_base(value - 0.5)
+        g = jet_base(value)
+        b = jet_base(value + 0.5)
+        return [r, g, b]
+
+
+def interpolate_hot_to_cold(value, min_value=0.0, max_value=1.0):
+    # Safety checks
+    assert(min_value < max_value)
+    if value < min_value:
+        value = min_value
+    elif value > max_value:
+        value = max_value
+    val_range = max_value - min_value
+    # Start with white
+    r = 1.0
+    g = 1.0
+    b = 1.0
+    # Interpolate
+    if value < (min_value + 0.25 * val_range):
+        r = 0.0
+        g = 4.0 * (value - min_value) / val_range
+    elif value < (min_value + 0.5 * val_range):
+        r = 0.0
+        b = 1.0 + 4.0 * (min_value + 0.25 * val_range - value) / val_range
+    elif value < (min_value + 0.75 * val_range):
+        r = 4 * (value - min_value - 0.5 * val_range) / val_range
+        b = 0.0
+    else:
+        g = 1.0 + 4.0 * (min_value + 0.75 * val_range - min_value) / val_range
+        b = 0.0
+    return [r, g, b]

flightlib/third_party/arc_utilities/src/arc_utilities/extra_functions_to_be_put_in_the_right_place.py

@@ -0,0 +1,82 @@
+from std_msgs.msg import *
+from geometry_msgs.msg import *
+from color_mapping import *
+
+def make_pose((px, py, pz), (rx, ry, rz, rw)):
+    new_pose = Pose()
+    new_pose.position.x = px
+    new_pose.position.y = py
+    new_pose.position.z = pz
+    new_pose.orientation.x = rx
+    new_pose.orientation.y = ry
+    new_pose.orientation.z = rz
+    new_pose.orientation.w = rw
+    return new_pose
+
+
+def make_vector(x, y, z):
+    new_vector = Vector3()
+    new_vector.x = x
+    new_vector.y = y
+    new_vector.z = z
+    return new_vector
+
+
+def make_point(x, y, z):
+    new_point = Point()
+    new_point.x = x
+    new_point.y = y
+    new_point.z = z
+    return new_point
+
+
+def make_plane((px, py, pz), (nx, ny, nz)):
+    new_plane = CollisionPlane()
+    new_plane.point = make_point(px, py, pz)
+    new_plane.normal = make_vector(nx, ny, nz)
+    return new_plane
+
+
+def make_unit_point(x, y, z):
+    mag = math.sqrt(x ** 2 + y ** 2 + z ** 2)
+    assert(mag > 0.0)
+    unit_point = Point()
+    unit_point.x = x / mag
+    unit_point.y = y / mag
+    unit_point.z = z / mag
+    return unit_point
+
+
+def normalize_point(raw_point):
+    return make_unit_point(raw_point.x, raw_point.y, raw_point.z)
+
+
+def make_plane_from_points(point, normal):
+    new_plane = CollisionPlane()
+    new_plane.point = point
+    new_plane.normal = normal
+    return new_plane
+
+
+def safe_color_val(val):
+    if val >= 1.0:
+        return 1.0
+    elif val <= 0.0:
+        return 0.0
+    else:
+        return val
+
+
+def make_color(r, g, b, a):
+    new_color = ColorRGBA()
+    new_color.r = safe_color_val(r)
+    new_color.g = safe_color_val(g)
+    new_color.b = safe_color_val(b)
+    new_color.a = safe_color_val(a)
+    return new_color
+
+
+def map_color(value):
+    # [r, g, b] = interpolate_jet(value)
+    [r, g, b] = interpolate_hot_to_cold(value)
+    return make_color(r, g, b, 1.0)

flightlib/third_party/arc_utilities/src/arc_utilities/first_order_deformation.cpp

@@ -0,0 +1,99 @@
+#include "arc_utilities/first_order_deformation.h"
+
+#include <queue>
+#include <Eigen/Core>
+
+using namespace arc_utilities;
+using namespace FirstOrderDeformation;
+
+static double ConfigTypeDistance(const ConfigType& c1, const ConfigType& c2)
+{
+    return Eigen::Vector2d((double)(c1.first - c2.first), (double)(c1.second - c2.second)).norm();
+}
+
+static std::vector<ConfigType> GetNeighbours(const ConfigType& config, const ssize_t rows, const ssize_t cols, const ValidityCheckFnType& validity_check_fn)
+{
+    std::vector<ConfigType> neighbours;
+    neighbours.reserve(8);
+
+    const ssize_t row_min = std::max(0L, config.first - 1);
+    const ssize_t row_max = std::min(rows - 1, config.first + 1);
+
+    const ssize_t col_min = std::max(0L, config.second - 1);
+    const ssize_t col_max = std::min(cols - 1, config.second + 1);
+
+    for (ssize_t col = col_min; col <= col_max; col++)
+    {
+        for (ssize_t row = row_min; row <= row_max; row++)
+        {
+            if (!(row == config. first && col == config.second) && validity_check_fn(row, col) == true)
+            {
+                neighbours.push_back(ConfigType(row, col));
+            }
+        }
+    }
+
+    return neighbours;
+}
+
+bool FirstOrderDeformation::CheckFirstOrderDeformation(const ssize_t rows, const ssize_t cols, const ValidityCheckFnType& validity_check_fn)
+{
+    struct BestFirstSearchComparator
+    {
+        public:
+            // Defines a "less" operation"; by using "greater" the smallest element will appear at the top of the priority queue
+            bool operator()(const ConfigAndDistType& c1, const ConfigAndDistType& c2) const
+            {
+                // We want to explore the one with the smaller expected distance
+                return (c1.second > c2.second);
+            }
+    };
+
+    assert(rows > 0 && cols > 0);
+    typedef Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> ArrayXb;
+
+    // Setup the strucutres needed to keep track of the search
+    std::priority_queue<ConfigAndDistType, std::vector<ConfigAndDistType>, BestFirstSearchComparator> frontier;
+    ArrayXb explored = ArrayXb::Constant(rows, cols, false);
+
+    // Setup the start and goal
+    const ConfigType start(0, 0), goal(rows - 1, cols - 1);
+    const double start_heuristic_distance = ConfigTypeDistance(start, goal);
+    frontier.push(ConfigAndDistType(start, start_heuristic_distance));
+
+    // Perform the best first search
+    bool path_found = false;
+    while (!path_found && frontier.size() > 0)
+    {
+        const ConfigAndDistType current = frontier.top();
+        frontier.pop();
+        const ConfigType& current_config = current.first;
+
+        if (current_config.first == goal.first && current_config.second == goal.second)
+        {
+            path_found = 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(current_config.first, current_config.second) == false)
+        {
+            explored(current_config.first, current_config.second) = true;
+
+            // Expand the node to find all neighbours, adding them to the frontier if we have not already explored them
+            const auto neighbours = GetNeighbours(current_config, rows, cols, validity_check_fn);
+            for (const auto neighbour : neighbours)
+            {
+                // Check if we've already explored this neighbour to avoid re-adding it to the frontier
+                if (explored(neighbour.first, neighbour.second) == false)
+                {
+                    // We only need the heuristic distance as we are doing a best first search
+                    const double neighbour_heuristic_distance = ConfigTypeDistance(neighbour, goal);
+                    frontier.push(ConfigAndDistType(neighbour, neighbour_heuristic_distance));
+                }
+            }
+        }
+    }
+
+    return path_found;
+}

flightlib/third_party/arc_utilities/src/arc_utilities/numpy_conversions.py

@@ -0,0 +1,19 @@
+import numpy as np
+
+def ListPointsToNpArray(points, transform=None):
+    """
+    :param points: Anything that is itterable, with each element having x, y, and z elements
+    :type points: :class:`list[geometry_msgs.msgs.Point]`
+    :param transform:
+    :type transform: :class:`np.array`
+    :return:
+    """
+    arr = np.empty(shape=(3, len(points)))
+    if transform is not None:
+        for ind in range(len(points)):
+            point = transform.dot([points[ind].x, points[ind].y, points[ind].z, 1])
+            arr[:, ind] = point[0:3]
+    else:
+        for ind in range(len(points)):
+            arr[:, ind] = [points[ind].x, points[ind].y, points[ind].z]
+    return arr

flightlib/third_party/arc_utilities/src/arc_utilities/ros_helpers.py

@@ -0,0 +1,109 @@
+#! /usr/bin/env python
+
+import rospy
+import time
+from threading import Lock
+from sensor_msgs.msg import Joy
+
+class Listener:
+    def __init__(self, topic_name, topic_type, wait_for_data=False):
+        """
+        Listener is a wrapper around a subscriber where the callback simply records the latest msg.
+
+        Listener does not consume the message 
+            (for consuming behavior, use the standard ros callback pattern)
+        Listener does not check timestamps of message headers
+
+        Parameters:
+            topic_name (str):      name of topic to subscribe to
+            topic_type (msg_type): type of message received on topic
+            wait_for_data (bool):  block constructor until a message has been received
+        """
+
+        self.data = None
+        self.lock = Lock()
+            
+        self.subscriber = rospy.Subscriber(topic_name, topic_type, self.callback)
+        self.get(wait_for_data)
+
+        
+    def callback(self, msg):
+        with self.lock:
+            self.data = msg
+
+    def get(self, block_until_data=True):
+        """
+        Returns the latest msg from the subscribed topic
+
+        Parameters:
+            block_until_data (bool): block if no message has been received yet. 
+                                     Guarantees a msg is returned (not None)
+        """
+        wait_for(lambda: not (block_until_data and self.data is None))
+            
+        with self.lock:
+            return self.data
+
+    
+def wait_for(func):
+    """
+    Waits for function evaluation to be true. Exits cleanly from ros.
+
+    Introduces sleep delay, not recommended for time critical operations
+    """
+    
+    while not func() and not rospy.is_shutdown():
+        time.sleep(0.01)
+
+        
+def joy_to_xbox(joy):
+    """
+    Transforms a joystick sensor_msg to a XBox controller for easier code readability
+    
+    Parameters:
+    joy (sensor_msgs/Joy): xbox msg
+
+    Returns:
+    xbox struct where fields are the button names
+    """
+    class Xbox_msg():
+        pass
+    x = Xbox_msg()
+    x.A, x.B, x.X, x.Y, x.LB, x.RB, \
+        x.back, x.start, x.power,\
+        x.stick_button_left, x.stick_button_right, \
+        x.DL, x.DR, x.DU, x.DD = joy.buttons
+    x.LH, x.LV, x.LT, x.RH, x.RV, x.RT, x.DH, x.DV = joy.axes
+    return x
+
+
+class Xbox():
+    def __init__(self, joystick_topic="joy"):
+        self.xbox_listener = Listener(joystick_topic, Joy)
+
+    def get_buttons_state(self):
+        """
+        Returns an xbox struct of the last joystick message received
+        """
+        return joy_to_xbox(self.xbox_listener.get())
+
+    def get_button(self, button):
+        """
+        Return value of button or axis of the controller
+        0 or 1 for buttons
+        -1.0 to 1.0 (at most) for axes
+        """
+        return getattr(self.get_buttons_state(), button)
+
+    def wait_for_button(self, button, message=True):
+        """
+        Waits for button press on xbox.
+
+        Parameters:
+        button (str):   Name of xbox button. "A", "B", "X", ...
+        message (bool): log a message informing the user?
+        """
+        if message:
+            rospy.loginfo("Waiting for xbox button: " + button)
+            
+        wait_for(lambda: not self.get_button(button) == 0)

flightlib/third_party/arc_utilities/src/arc_utilities/transformation_helper.py

@@ -0,0 +1,407 @@
+#!/usr/bin/env python
+
+#   Calder Phillips-Grafflin -- ARC Lab
+#
+#   Various helper functions for operations with transformations
+#   using quaternions + vectors and 4x4 matrices
+#
+####################################################################################################
+#
+# A note to the user - this code was written because the last time I needed to do one of the
+# operations provided in this API (composing a pose with a transform), it required ~20 lines of code
+#
+# It relies exclusively on the powerful tranformations.py library and numpy linear algebra library
+#
+####################################################################################################
+
+import rospy
+import math
+import numpy
+from numpy import *
+from tf.transformations import *
+from geometry_msgs.msg import *
+
+####################################################################################################
+#
+#   API Documentation
+#
+#   Storage conventions:
+#
+#   translations = [x,y,z]
+#   quaternions = [x,y,z,w]
+#
+##################################################
+# Math functions
+#
+# geometry_msgs/Transform composed = ComposeTransforms(geometry_msgs/Transform transform1, geometry_msgs/Transform transform2)
+# geometry_msgs/Pose composed = ComposePoses(geometry_msgs/Pose pose1, geometry_msgs/Pose pose2)
+#
+# geometry_msgs/Transform inverted = InvertTransform(geometry_msgs/Transform old_transform)
+# geometry_msgs/Pose inverted = InvertPose(geometry_msgs/Pose old_pose)
+#
+# numpy/array composed = ComposeMatrices(numpy/array matrix1, numpy/array matrix2)
+# numpy/array inverted = InvertMatrix(numpy/array old_matrix)
+#
+##################################################
+# Assist functions
+#
+# list quaternion = NormalizeQuaternion(list quaternion)
+#
+# geometry_msgs/Quaternion quaternion = NormalizeQuaternionRos(geometry_msgs/Quaternion quaternion)
+#
+##################################################
+# Conversion functions
+#
+# geometry_msgs/Transform new_transform = TransformFromComponents(list translation, list quaternion)
+# [list translation, list quaternion] = ComponentsFromTransform(geometry_msgs/Transform old_transform)
+#
+# geometry_msgs/Pose new_pose = PoseFromTransform(geometry_msgs/Transform old_transform)
+# geometry_msgs/Transform new_transform = PoseToTransform(geometry_msgs/Pose old_pose)
+#
+# numpy/array tfmatrix = TransformToMatrix(geometry_msgs/Transform old_transform)
+# geometry_msgs/Transform new_transform = TransformFromMatrix(numpy/array old_matrix)
+#
+# numpy/array tfmatrix = PoseToMatrix(geometry_msgs/Pose old_pose)
+# geometry_msgs/Pose new_pose = PoseFromMatrix(numpy/array old_matrix)
+#
+# [numpy/array rotation, list translation] = ExtractRawFromMatrix(numpy/array old_matrix)
+# numpy/array tfmatrix = BuildRawMatrix(numpy/array rotation, list translation)
+#
+# [list translation, list quaternion] = ExtractFromMatrix(numpy/array old_matrix)
+# numpy/array tfmatrix = BuildMatrix(list translation, list quaternion)
+#
+##################################################
+# Generation functions -- these generate various transform representations from D-H parameters
+#                         we recommend against using these functions unless necessary
+#
+# numpy/array tfmatrix = BuildMatrixFromDH(float d, float a, float theta, float alpha)
+# [list translation, list quaternion] = ExtractFromDH(float d, float a, float theta, float alpha)
+# geometry_msgs/Transform new_transform = TransformFromDH(float d, float a, float theta, float alpha)
+# geometry_msgs/Pose new_pose = PoseFromDH(float d, float a, float theta, float alpha)
+#
+####################################################################################################
+
+'''High-level functions'''
+
+
+def AddPoints(point1, point2):
+    sum_point = Point()
+    sum_point.x = point2.x + point1.x
+    sum_point.y = point2.y + point1.y
+    sum_point.z = point2.z + point1.z
+    return sum_point
+
+
+def SubtractPoints(point1, point2):
+    difference = Point()
+    difference.x = point2.x - point1.x
+    difference.y = point2.y - point1.y
+    difference.z = point2.z - point1.z
+    return difference
+
+
+def TranslationNorm(trans):
+    return math.sqrt(trans.x**2 + trans.y**2 + trans.z**2)
+
+
+def ComposePoseWithPoint(pose, point):
+    matrix = TransformToMatrix(PoseToTransform(pose))
+    vector = PointToVector(point)
+    composed = numpy.dot(matrix, vector)
+    composed_point = PointFromVector(composed)
+    return composed_point
+
+
+def ComposeTransformWithPoint(transform, point):
+    matrix = TransformToMatrix(transform)
+    vector = PointToVector(point)
+    composed = numpy.dot(matrix, vector)
+    composed_point = PointFromVector(composed)
+    return composed_point
+
+
+def ComposeTransforms(transform1, transform2):
+    tfmatrix1 = TransformToMatrix(transform1)
+    tfmatrix2 = TransformToMatrix(transform2)
+    composedmatrix = dot(tfmatrix1, tfmatrix2)
+    composed = TransformFromMatrix(composedmatrix)
+    return composed
+
+
+def ComposePoses(pose1, pose2):
+    transform1 = PoseToTransform(pose1)
+    transform2 = PoseToTransform(pose2)
+    composed = PoseFromTransform(ComposeTransforms(transform1, transform2))
+    return composed
+
+
+def InvertTransform(old_transform):
+    tfmatrix = TransformToMatrix(old_transform)
+    xirtamft = numpy.linalg.inv(tfmatrix)
+    inverted = TransformFromMatrix(xirtamft)
+    return inverted
+
+
+def InvertPose(old_pose):
+    old_transform = PoseToTransform(old_pose)
+    inverted = InvertTransform(old_transform)
+    posed = PoseFromTransform(inverted)
+    return posed
+
+
+def ComposeMatrices(matrix1, matrix2):
+    return dot(matrix1, matrix2)
+
+
+def InvertMatrix(old_matrix):
+    return numpy.linalg.inv(old_matrix)
+
+
+def ComposeQuaternions(q1, q2):
+    nq1 = NormalizeQuaternion(q1)
+    nq2 = NormalizeQuaternion(q2)
+    x = nq1[3]*nq2[0] + nq2[3]*nq1[0] + nq1[1]*nq2[2] - nq2[1]*nq1[2]
+    y = nq1[3]*nq2[1] + nq2[3]*nq1[1] + nq2[0]*nq1[2] - nq1[0]*nq2[2]
+    z = nq1[3]*nq2[2] + nq2[3]*nq1[2] + nq1[0]*nq2[1] - nq2[0]*nq1[1]
+    w = nq1[3]*nq2[3] - nq1[0]*nq2[0] - nq1[1]*nq2[1] - nq1[2]*nq2[2]
+    return NormalizeQuaternion([x, y, z, w])
+
+
+def AxisFromQuaternion(q):
+    nq = NormalizeQuaternion(q)
+    a = math.acos(nq[3]) * 2.0
+    sina2 = math.sin(a * 0.5)
+    if abs(sina2) > 0.000000001:
+        i = nq[0] / sina2
+        j = nq[1] / sina2
+        k = nq[2] / sina2
+        return [a, [i, j, k]]
+    else:
+        return [0.0, [0.0, 0.0, 1.0]]
+
+
+def NormalizeVector3(axis):
+    assert(len(axis) == 3)
+    mag = math.sqrt(axis[0] ** 2 + axis[1] ** 2 + axis[2] ** 2)
+    assert(mag > 0.0)
+    return [axis[0] / mag, axis[1] / mag, axis[2] / mag]
+
+
+def QuaternionFromAxisAngle(axis, angle):
+    try:
+        naxis = NormalizeVector3(axis)
+        w = math.cos(angle * 0.5)
+        x = math.sin(angle * 0.5) * naxis[0]
+        y = math.sin(angle * 0.5) * naxis[1]
+        z = math.sin(angle * 0.5) * naxis[2]
+        return NormalizeQuaternion([x, y, z, w])
+    except AssertionError:
+        print("Vector normalize error, returning identity quaternion")
+        return [0.0, 0.0, 0.0, 1.0]
+
+
+def AngleBetweenQuaternions(q1, q2):
+    nq1 = NormalizeQuaternion(q1)
+    nq2 = NormalizeQuaternion(q2)
+    dot_product = abs(nq1[0] * nq2[0] + nq1[1] * nq2[1] + nq1[2] * nq2[2] + nq1[3] * nq2[3])
+    if dot_product < 0.9999:
+        return math.acos(2.0 * (dot_product ** 2) - 1.0)
+    else:
+        return 0.0
+
+
+def AngleBetweenQuaternionsRos(q1, q2):
+    nq1 = NormalizeQuaternionRos(q1)
+    nq2 = NormalizeQuaternionRos(q2)
+    dot_product = abs(nq1.x * nq2.x + nq1.y * nq2.y + nq1.z * nq2.z + nq1.z * nq2.w)
+    if dot_product < 0.9999:
+        return math.acos(2.0 * (dot_product ** 2) - 1.0)
+    else:
+        return 0
+
+
+'''Assist functions'''
+
+
+def NormalizeQuaternion(q_raw):
+    magnitude = math.sqrt(q_raw[0]**2 + q_raw[1]**2 + q_raw[2]**2 + q_raw[3]**2)
+    x = q_raw[0] / magnitude
+    y = q_raw[1] / magnitude
+    z = q_raw[2] / magnitude
+    w = q_raw[3] / magnitude
+    return [x, y, z, w]
+
+
+def NormalizeQuaternionRos(q_raw):
+    magnitude = math.sqrt(q_raw.x**2 + q_raw.y**2 + q_raw.z**2 + q_raw.w**2)
+    q_unit = Quaternion()
+    q_unit.x = q_raw.x / magnitude
+    q_unit.y = q_raw.y / magnitude
+    q_unit.z = q_raw.z / magnitude
+    q_unit.w = q_raw.w / magnitude
+    return q_unit
+
+
+'''Conversion functions'''
+
+
+def PointToVector(point):
+    return numpy.array([point.x, point.y, point.z, 1.0]).transpose()
+
+
+def PointFromVector(vector):
+    new_point = Point()
+    new_point.x = float(vector[0])
+    new_point.y = float(vector[1])
+    new_point.z = float(vector[2])
+    return new_point
+
+
+def PoseFromTransform(old_transform):
+    posed = Pose()
+    posed.position.x = old_transform.translation.x
+    posed.position.y = old_transform.translation.y
+    posed.position.z = old_transform.translation.z
+    posed.orientation.x = old_transform.rotation.x
+    posed.orientation.y = old_transform.rotation.y
+    posed.orientation.z = old_transform.rotation.z
+    posed.orientation.w = old_transform.rotation.w
+    return posed
+
+
+def PoseToTransform(old_pose):
+    transformed = Transform()
+    transformed.translation.x = old_pose.position.x
+    transformed.translation.y = old_pose.position.y
+    transformed.translation.z = old_pose.position.z
+    transformed.rotation.x = old_pose.orientation.x
+    transformed.rotation.y = old_pose.orientation.y
+    transformed.rotation.z = old_pose.orientation.z
+    transformed.rotation.w = old_pose.orientation.w
+    return transformed
+
+
+def PoseFromComponents(translation, quaternion):
+    posed = Pose()
+    posed.position.x = translation[0]
+    posed.position.y = translation[1]
+    posed.position.z = translation[2]
+    posed.orientation.x = quaternion[0]
+    posed.orientation.y = quaternion[1]
+    posed.orientation.z = quaternion[2]
+    posed.orientation.w = quaternion[3]
+    return posed
+
+
+def ComponentsFromPose(old_pose):
+    translation = [old_pose.position.x, old_pose.position.y, old_pose.position.z]
+    quaternion = [old_pose.orientation.x, old_pose.orientation.y, old_pose.orientation.z, old_pose.orientation.w]
+    return [translation, quaternion]
+
+
+def TransformFromComponents(translation, quaternion):
+    transformed = Transform()
+    transformed.translation.x = translation[0]
+    transformed.translation.y = translation[1]
+    transformed.translation.z = translation[2]
+    transformed.rotation.x = quaternion[0]
+    transformed.rotation.y = quaternion[1]
+    transformed.rotation.z = quaternion[2]
+    transformed.rotation.w = quaternion[3]
+    return transformed
+
+
+def ComponentsFromTransform(old_transform):
+    translation = [old_transform.translation.x, old_transform.translation.y, old_transform.translation.z]
+    quaternion = [old_transform.rotation.x, old_transform.rotation.y, old_transform.rotation.z, old_transform.rotation.w]
+    return [translation, quaternion]
+
+
+def TransformToMatrix(old_transform):
+    [translation, quaternion] = ComponentsFromTransform(old_transform)
+    tfmatrix = BuildMatrix(translation, quaternion)
+    return tfmatrix
+
+
+def TransformFromMatrix(old_matrix):
+    [translation, quaternion] = ExtractFromMatrix(old_matrix)
+    transformed = TransformFromComponents(translation, quaternion)
+    return transformed
+
+
+def PoseToMatrix(old_pose):
+    old_transform = PoseToTransform(old_pose)
+    [translation, quaternion] = ComponentsFromTransform(old_transform)
+    tfmatrix = BuildMatrix(translation, quaternion)
+    return tfmatrix
+
+
+def PoseFromMatrix(old_matrix):
+    [translation, quaternion] = ExtractFromMatrix(old_matrix)
+    transformed = TransformFromComponents(translation, quaternion)
+    posed = PoseFromTransform(transformed)
+    return posed
+
+
+def ExtractRawFromMatrix(tm):
+    rmat = array([[tm[0][0], tm[0][1], tm[0][2]], [tm[1][0], tm[1][1], tm[1][2]], [tm[2][0], tm[2][1], tm[2][2]]])
+    tvec = [tm[0][3], tm[1][3], tm[2][3]]
+    return [rmat, tvec]
+
+
+def BuildRawMatrix(rm, tv):
+    tfmatrix = array([[rm[0][0], rm[0][1], rm[0][2], tv[0]], [rm[1][0], rm[1][1], rm[1][2], tv[1]], [rm[2][0], rm[2][1], rm[2][2], tv[2]], [0.0, 0.0, 0.0, 1.0]])
+    return tfmatrix
+
+
+def ExtractFromMatrix(old_matrix):
+    quaternion = quaternion_from_matrix(old_matrix)
+    translation = [old_matrix[0][3], old_matrix[1][3], old_matrix[2][3]]
+    return [translation, quaternion]
+
+
+def BuildMatrix(translation, quaternion):
+    tfmatrix = quaternion_matrix(quaternion)
+    tfmatrix[0][3] = translation[0]
+    tfmatrix[1][3] = translation[1]
+    tfmatrix[2][3] = translation[2]
+    return tfmatrix
+
+
+def BuildMatrixRos(translation, quaternion):
+    quat = [quaternion.x, quaternion.y, quaternion.z, quaternion.w]
+    trans = [translation.x, translation.y, translation.z]
+    return BuildMatrix(trans, quat)
+
+
+''' Generation Functions '''
+
+def BuildMatrixFromTransRot(trans, rot):
+    transform = numpy.empty(shape=[4, 4])
+    transform[0:3, 0:3] = rot
+    transform[0:3, 3] = trans
+    transform[3, 0:3] = 0
+    transform[3, 3] = 1
+    return transform
+
+
+def BuildMatrixFromDH(d, a, theta, alpha):
+    #Do math here
+    tfmatrix = array([[math.cos(theta), -sin(theta) * math.cos(alpha), math.sin(theta) * math.sin(alpha), alpha * math.cos(theta)], [math.sin(theta), math.cos(theta) * math.cos(alpha), -math.cos(theta) * math.sin(alpha), alpha * math.sin(theta)], [0.0, math.sin(alpha), math.cos(alpha), d], [0.0, 0.0, 0.0, 1.0]])
+    return tfmatrix
+
+
+def ExtractFromDH(d, a, theta, alpha):
+    tfmatrix = BuildMatrixFromDH(d, a, theta, alpha)
+    [translation, quaternion] = ExtractFromMatrix(tfmatrix)
+    return [translation, quaternion]
+
+
+def TransformFromDH(d, a, theta, alpha):
+    tfmatrix = BuildMatrixFromDH(d, a, theta, alpha)
+    return TransformFromMatrix(tfmatrix)
+
+
+def PoseFromDH(d, a, theta, alpha):
+    tfmatrix = BuildMatrixFromDH(d, a, theta, alpha)
+    return PoseFromMatrix(tfmatrix)

flightlib/third_party/arc_utilities/src/arc_utilities/zlib_helpers.cpp

@@ -0,0 +1,141 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <vector>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <fstream>
+#include <stdexcept>
+#include <zlib.h>
+#include "arc_utilities/arc_exceptions.hpp"
+#include "arc_utilities/zlib_helpers.hpp"
+
+namespace ZlibHelpers
+{
+    // MAKE SURE THE INPUT BUFFER IS LESS THAN 4GB IN SIZE
+    std::vector<uint8_t> DecompressBytes(const std::vector<uint8_t>& compressed)
+    {
+        z_stream strm;
+        std::vector<uint8_t> buffer;
+        const size_t BUFSIZE = 1024 * 1024;
+        uint8_t temp_buffer[BUFSIZE];
+        strm.zalloc = Z_NULL;
+        strm.zfree = Z_NULL;
+        strm.opaque = Z_NULL;
+        int ret = inflateInit(&strm);
+        if (ret != Z_OK)
+        {
+            (void)inflateEnd(&strm);
+            std::cerr << "ZLIB unable to init inflate stream" << std::endl;
+            throw std::invalid_argument("ZLIB unable to init inflate stream");
+        }
+        strm.avail_in = (uint32_t)compressed.size();
+        strm.next_in = const_cast<uint8_t*>(reinterpret_cast<const uint8_t*>(compressed.data()));
+        do
+        {
+            strm.next_out = temp_buffer;
+            strm.avail_out = BUFSIZE;
+            ret = inflate(&strm, Z_NO_FLUSH);
+            if (buffer.size() < strm.total_out)
+            {
+                buffer.insert(buffer.end(), temp_buffer, temp_buffer + BUFSIZE - strm.avail_out);
+            }
+        }
+        while (ret == Z_OK);
+        if (ret != Z_STREAM_END)
+        {
+            (void)inflateEnd(&strm);
+            std::cerr << "ZLIB unable to inflate stream with ret=" << ret << std::endl;
+            throw std::invalid_argument("ZLIB unable to inflate stream");
+        }
+        (void)inflateEnd(&strm);
+        std::vector<uint8_t> decompressed(buffer);
+        return decompressed;
+    }
+
+    // MAKE SURE THE INPUT BUFFER IS LESS THAN 4GB IN SIZE
+    std::vector<uint8_t> CompressBytes(const std::vector<uint8_t>& uncompressed)
+    {
+        z_stream strm;
+        std::vector<uint8_t> buffer;
+        const size_t BUFSIZE = 1024 * 1024;
+        uint8_t temp_buffer[BUFSIZE];
+        strm.zalloc = Z_NULL;
+        strm.zfree = Z_NULL;
+        strm.opaque = Z_NULL;
+        strm.avail_in = (uint32_t)uncompressed.size();
+        strm.next_in = const_cast<uint8_t*>(reinterpret_cast<const uint8_t*>(uncompressed.data()));
+        strm.next_out = temp_buffer;
+        strm.avail_out = BUFSIZE;
+        int ret = deflateInit(&strm, Z_BEST_SPEED);
+        if (ret != Z_OK)
+        {
+            (void)deflateEnd(&strm);
+            std::cerr << "ZLIB unable to init deflate stream" << std::endl;
+            throw std::invalid_argument("ZLIB unable to init deflate stream");
+        }
+        while (strm.avail_in != 0)
+        {
+            ret = deflate(&strm, Z_NO_FLUSH);
+            if (ret != Z_OK)
+            {
+                (void)deflateEnd(&strm);
+                std::cerr << "ZLIB unable to deflate stream" << std::endl;
+                throw std::invalid_argument("ZLIB unable to deflate stream");
+            }
+            if (strm.avail_out == 0)
+            {
+                buffer.insert(buffer.end(), temp_buffer, temp_buffer + BUFSIZE);
+                strm.next_out = temp_buffer;
+                strm.avail_out = BUFSIZE;
+            }
+        }
+        int deflate_ret = Z_OK;
+        while (deflate_ret == Z_OK)
+        {
+            if (strm.avail_out == 0)
+            {
+                buffer.insert(buffer.end(), temp_buffer, temp_buffer + BUFSIZE);
+                strm.next_out = temp_buffer;
+                strm.avail_out = BUFSIZE;
+            }
+            deflate_ret = deflate(&strm, Z_FINISH);
+        }
+        if (deflate_ret != Z_STREAM_END)
+        {
+            (void)deflateEnd(&strm);
+            std::cerr << "ZLIB unable to deflate stream" << std::endl;
+            throw std::invalid_argument("ZLIB unable to deflate stream");
+        }
+        buffer.insert(buffer.end(), temp_buffer, temp_buffer + BUFSIZE - strm.avail_out);
+        (void)deflateEnd(&strm);
+        std::vector<uint8_t> compressed(buffer);
+        return compressed;
+    }
+
+    std::vector<uint8_t> LoadFromFileAndDecompress(const std::string& path)
+    {
+        std::ifstream input_file(path, std::ios::binary | std::ios::in | std::ios::ate);
+        if (!input_file.is_open())
+        {
+            throw_arc_exception(std::runtime_error, "Couldn't open file " + path);
+        }
+        std::streamsize size = input_file.tellg();
+        input_file.seekg(0, std::ios::beg);
+        std::vector<uint8_t> file_buffer((size_t)size);
+        if (!(input_file.read(reinterpret_cast<char*>(file_buffer.data()), size)))
+        {
+            throw_arc_exception(std::runtime_error, "Unable to read entire contents of file");
+        }
+        const std::vector<uint8_t> decompressed = ZlibHelpers::DecompressBytes(file_buffer);
+        return decompressed;
+    }
+
+    void CompressAndWriteToFile(const std::vector<uint8_t>& uncompressed, const std::string& path)
+    {
+        const auto compressed = CompressBytes(uncompressed);
+        std::ofstream output_file(path, std::ios::out | std::ios::binary);
+        output_file.write(reinterpret_cast<const char*>(compressed.data()), (std::streamsize)compressed.size());
+        output_file.close();
+    }
+}

flightlib/third_party/arc_utilities/src/test_arc_utilities.cpp

@@ -0,0 +1,101 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <random>
+#include <chrono>
+#include <arc_utilities/arc_helpers.hpp>
+#include <arc_utilities/eigen_helpers.hpp>
+#include <arc_utilities/pretty_print.hpp>
+#include <arc_utilities/abb_irb1600_145_fk_fast.hpp>
+#include <arc_utilities/iiwa_7_fk_fast.hpp>
+#include <arc_utilities/iiwa_14_fk_fast.hpp>
+#include <arc_utilities/simple_dtw.hpp>
+#include <arc_utilities/simple_prm_planner.hpp>
+
+int main(int argc, char** argv)
+{
+    printf("%d arguments\n", argc);
+    for (int idx = 0; idx < argc; idx++)
+    {
+        printf("Argument %d: %s\n", idx, argv[idx]);
+    }
+    std::cout << "Testing PrettyPrints..." << std::endl;
+    std::cout << PrettyPrint::PrettyPrint(Eigen::Isometry3d::Identity()) << std::endl;
+    std::cout << PrettyPrint::PrettyPrint(Eigen::Vector3d(0.0, 0.0, 0.0)) << std::endl;
+    std::cout << PrettyPrint::PrettyPrint(std::vector<bool>{true, false, true, false}) << std::endl;
+    std::cout << "...done" << std::endl;
+    const std::vector<double> abb_irb_1600_145_base_config = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    const EigenHelpers::VectorIsometry3d abb_irb_1600_145_link_transforms = ABB_IRB1600_145_FK_FAST::GetLinkTransforms(abb_irb_1600_145_base_config);
+    std::cout << "ABB IRB1600-145 Link transforms:\n" << PrettyPrint::PrettyPrint(abb_irb_1600_145_link_transforms, false, "\n") << std::endl;
+    const std::vector<double> iiwa_7_base_config = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    const EigenHelpers::VectorIsometry3d iiwa_7_link_transforms = IIWA_7_FK_FAST::GetLinkTransforms(iiwa_7_base_config);
+    std::cout << "IIWA 7 Link transforms:\n" << PrettyPrint::PrettyPrint(iiwa_7_link_transforms, false, "\n") << std::endl;
+    const std::vector<double> iiwa_14_base_config = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+    const EigenHelpers::VectorIsometry3d iiwa_14_link_transforms = IIWA_14_FK_FAST::GetLinkTransforms(iiwa_14_base_config);
+    std::cout << "IIWA 14 Link transforms:\n" << PrettyPrint::PrettyPrint(iiwa_14_link_transforms, false, "\n") << std::endl;
+    std::cout << std::endl;
+
+    // Test Vector3d averaging
+    EigenHelpers::VectorVector3d testvecs(8, Eigen::Vector3d::Zero());
+    testvecs[0] = Eigen::Vector3d(-1.0, -1.0, -1.0);
+    testvecs[1] = Eigen::Vector3d(-1.0, -1.0, 1.0);
+    testvecs[2] = Eigen::Vector3d(-1.0, 1.0, -1.0);
+    testvecs[3] = Eigen::Vector3d(-1.0, 1.0, 1.0);
+    testvecs[4] = Eigen::Vector3d(1.0, -1.0, -1.0);
+    testvecs[5] = Eigen::Vector3d(1.0, -1.0, 1.0);
+    testvecs[6] = Eigen::Vector3d(1.0, 1.0, -1.0);
+    testvecs[7] = Eigen::Vector3d(1.0, 1.0, 1.0);
+    std::cout << "Individual vectors:\n" << PrettyPrint::PrettyPrint(testvecs, true, "\n") << std::endl;
+    Eigen::Vector3d averagevec = EigenHelpers::AverageEigenVector3d(testvecs);
+    std::cout << "Average vector: " << PrettyPrint::PrettyPrint(averagevec) << std::endl;
+    std::cout << std::endl;
+    assert(averagevec.isMuchSmallerThan(10^-10) && "The result of this average should be (0, 0, 0)");
+
+
+    // Test weighted dot product functions
+    Eigen::Vector3d weights(1.0, 2.0, 3.0);
+    std::cout << "Vector: " << PrettyPrint::PrettyPrint(testvecs[0]) << " Weighted norm: " << EigenHelpers::WeightedNorm(testvecs[0], weights) << std::endl;
+    std::cout << "Vector: " << PrettyPrint::PrettyPrint(testvecs[7]) << " Weighted norm: " << EigenHelpers::WeightedNorm(testvecs[7], weights) << std::endl;
+    std::cout << "Weighted angle between vectors: " << EigenHelpers::WeightedAngleBetweenVectors(testvecs[0], testvecs[7], weights) << std::endl;
+    std::cout << "Unweighted angle between vectors: " << EigenHelpers::WeightedAngleBetweenVectors(testvecs[0], testvecs[7], Eigen::Vector3d::Ones()) << std::endl;
+    std::cout << "Vector: " << PrettyPrint::PrettyPrint(testvecs[1]) << " Weighted norm: " << EigenHelpers::WeightedNorm(testvecs[1], weights) << std::endl;
+    std::cout << "Vector: " << PrettyPrint::PrettyPrint(testvecs[2]) << " Weighted norm: " << EigenHelpers::WeightedNorm(testvecs[2], weights) << std::endl;
+    std::cout << "Weighted angle between vectors: " << EigenHelpers::WeightedAngleBetweenVectors(testvecs[1], testvecs[2], weights) << std::endl;
+    std::cout << "Unweighted angle between vectors: " << EigenHelpers::WeightedAngleBetweenVectors(testvecs[1], testvecs[2], Eigen::Vector3d::Ones()) << std::endl;
+    std::cout << std::endl;
+
+    // Test truncated normal distribution
+    auto seed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
+    std::mt19937_64 prng(seed);
+    arc_helpers::TruncatedNormalDistribution dist(0.0, 1.0, -5.0, 5.0);
+    std::vector<double> test_trunc_normals(1000, 0.0);
+    for (size_t idx = 0; idx < test_trunc_normals.size(); idx++)
+    {
+        test_trunc_normals[idx] = dist(prng);
+    }
+    std::cout << "Truncated normal test:\n" << PrettyPrint::PrettyPrint(test_trunc_normals, false, ",") << std::endl;
+    std::cout << std::endl;
+
+    // Test multivariate gaussian
+    std::cout << "MVN Gaussian test:\n";
+    Eigen::Vector2d mean(0.0, 1.0);
+    Eigen::Matrix2d covar;
+    covar << 10.0, 5.0,
+             5.0, 10.0;
+    arc_helpers::MultivariteGaussianDistribution mvn_dist(mean, covar);
+    std::vector<Eigen::VectorXd> mvn_gaussians(3000);
+    for (size_t idx = 0; idx < mvn_gaussians.size(); idx++)
+    {
+        mvn_gaussians[idx] = mvn_dist(prng);
+        std::cout << mvn_gaussians[idx].transpose() << std::endl;
+    }
+    std::cout << std::endl;
+
+    // Test DTW
+    std::vector<double> reversed_test_trunc_normals = test_trunc_normals;
+    std::reverse(reversed_test_trunc_normals.begin(), reversed_test_trunc_normals.end());
+    std::function<double(const double&, const double&)> double_distance_fn = [] (const double& d1, const double& d2) { return std::abs(d1 - d2); };
+    const double test_dist = simple_dtw::ComputeDTWDistance(test_trunc_normals, test_trunc_normals, double_distance_fn);
+    const double reversed_test_dist = simple_dtw::ComputeDTWDistance(test_trunc_normals, reversed_test_trunc_normals, double_distance_fn);
+    std::cout << "DTW distance test = " << test_dist << ", reversed = " << reversed_test_dist << std::endl;
+    std::cout << std::endl;
+}

flightlib/third_party/arc_utilities/src/test_averaging.cpp

@@ -0,0 +1,47 @@
+#include "arc_utilities/eigen_helpers.hpp"
+#include "arc_utilities/pretty_print.hpp"
+
+using namespace Eigen;
+using namespace EigenHelpers;
+
+int main(int argc, char* argv[])
+{
+    (void)argc;
+    (void)argv;
+
+    const std::vector<double> weights = {1.0, 2.0, 3.0};
+
+    VectorVector3d test_vectors(3);
+    test_vectors[0] = Vector3d::UnitX();
+    test_vectors[1] = Vector3d::UnitY();
+    test_vectors[2] = Vector3d::UnitZ();
+
+    const Vector3d weighted_average = AverageEigenVector(test_vectors, weights);
+    std::cout << "Test weights: " << PrettyPrint::PrettyPrint(weights) << std::endl;
+    std::cout << "Test vectors:\n" << PrettyPrint::PrettyPrint(test_vectors, true, "\n") << std::endl;
+    std::cout << "Weighted Average:\n" << PrettyPrint::PrettyPrint(weighted_average) << std::endl << std::endl;
+    assert(weighted_average.isApprox(Eigen::Vector3d(1.0, 2.0, 3.0) / 6.0));
+
+
+    VectorVector3d repeated_test_vectors;
+    for (size_t idx = 0; idx < test_vectors.size(); ++idx)
+    {
+        repeated_test_vectors.insert(repeated_test_vectors.end(), (size_t)weights[idx], test_vectors[idx]);
+    }
+
+    const Vector3d repeated_average = AverageEigenVector(repeated_test_vectors);
+    std::cout << "Repeated vectors:\n" << PrettyPrint::PrettyPrint(repeated_test_vectors, true, "\n") << std::endl;
+    std::cout << "Repeated Average:\n" << PrettyPrint::PrettyPrint(repeated_average) << std::endl << std::endl;;
+    assert(repeated_average.isApprox(Eigen::Vector3d(1.0, 2.0, 3.0) / 6.0));
+
+
+
+    const std::vector<double> all_zero_weights = {0.0, 0.0, 0.0};
+    const Vector3d weighted_average_all_zero_weights = AverageEigenVector(test_vectors, all_zero_weights);
+    std::cout << "Test weights: " << PrettyPrint::PrettyPrint(all_zero_weights) << std::endl;
+    std::cout << "Test vectors:\n" << PrettyPrint::PrettyPrint(test_vectors, true, "\n") << std::endl;
+    std::cout << "All Zero Weights Average:\n" << PrettyPrint::PrettyPrint(weighted_average_all_zero_weights) << std::endl << std::endl;
+    assert(weighted_average_all_zero_weights.isMuchSmallerThan(10^-10) && "The result of this average should be (0, 0, 0)");
+
+    return 0;
+}

flightlib/third_party/arc_utilities/src/test_closest_point.cpp

@@ -0,0 +1,27 @@
+#include "arc_utilities/eigen_helpers.hpp"
+#include "arc_utilities/pretty_print.hpp"
+
+using namespace Eigen;
+using namespace EigenHelpers;
+
+int main(int argc, char* argv[])
+{
+    (void)argc;
+    (void)argv;
+
+    const Vector3d base_point(-0.2, 0.035, 0.2055);
+    const Vector3d normal(0, 0, 1);
+
+    const Vector3d test_point(-0.198425, 0.0342693, 0.0417458);
+
+    const auto distances = DistanceToLine(base_point, normal, test_point);
+
+    std::cout << "Distance to line: " << distances.first << " DISPLACEMENT along line (can be negative): " << distances.second << std::endl;
+
+    const Vector3d point_on_line = base_point + distances.second * normal;
+
+    std::cout << "Point on line: " << point_on_line.transpose() << std::endl;
+    std::cout << "Dot product: " << normal.dot(test_point - point_on_line) << std::endl;
+
+    return 0;
+}

flightlib/third_party/arc_utilities/src/test_dijkstras.cpp

@@ -0,0 +1,124 @@
+#include <iostream>
+
+#include <arc_utilities/dijkstras.hpp>
+#include <arc_utilities/pretty_print.hpp>
+
+int main(int argc, char* argv[])
+{
+    (void)argc;
+    (void)argv;
+
+    std::cout << "Creating graph with nodes indices:\n\n";
+    std::cout << "  2 | 5 | 8\n"
+              << "  --+---+--\n"
+              << "  1 | 4 | 7\n"
+              << "  --+---+--\n"
+              << "  0 | 3 | 6\n\n";
+
+    arc_dijkstras::Graph<Eigen::Vector2d> graph(9);
+    for (double x = -1; x <= 1; x += 1)
+    {
+        for (double y = -1; y <= 1; y += 1)
+        {
+            graph.AddNode(Eigen::Vector2d(x, y));
+        }
+    }
+
+    // Y-direction edges
+    graph.AddEdgeBetweenNodes(0, 1, 1.0);
+    graph.AddEdgesBetweenNodes(1, 2, 1.0);
+    graph.AddEdgesBetweenNodes(3, 4, 1.0);
+    graph.AddEdgesBetweenNodes(4, 5, 1.0);
+    graph.AddEdgesBetweenNodes(6, 7, 1.0);
+    graph.AddEdgesBetweenNodes(7, 8, 1.0);
+
+    // X-direction edges
+    graph.AddEdgesBetweenNodes(0, 3, 1.0);
+    graph.AddEdgesBetweenNodes(3, 6, 1.0);
+    graph.AddEdgesBetweenNodes(1, 4, 1.0);
+    graph.AddEdgesBetweenNodes(4, 7, 1.0);
+    graph.AddEdgesBetweenNodes(2, 5, 1.0);
+    graph.AddEdgesBetweenNodes(5, 8, 1.0);
+
+    assert(graph.CheckGraphLinkage());
+
+
+    auto dijkstras_result_4connected = arc_dijkstras::SimpleDijkstrasAlgorithm<Eigen::Vector2d, std::allocator<Eigen::Vector2d>>::PerformDijkstrasAlgorithm(graph, 0);
+
+    std::cout << "4-connected edges\n"
+              << "Node index            : 0, 1, 2, 3, 4, 5, 6, 7, 8\n";
+    std::cout << "Previous graph indices: " << PrettyPrint::PrettyPrint(dijkstras_result_4connected.second.first) << std::endl;
+    std::cout << "Distance              : " << PrettyPrint::PrettyPrint(dijkstras_result_4connected.second.second) << std::endl;
+
+    // Diagonal edges
+    graph.AddEdgesBetweenNodes(0, 4, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(1, 5, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(3, 7, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(4, 8, std::sqrt(2));
+
+    graph.AddEdgesBetweenNodes(1, 3, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(2, 4, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(4, 6, std::sqrt(2));
+    graph.AddEdgesBetweenNodes(5, 7, std::sqrt(2));
+
+    assert(graph.CheckGraphLinkage());
+    auto dijkstras_result_8connected = arc_dijkstras::SimpleDijkstrasAlgorithm<Eigen::Vector2d, std::allocator<Eigen::Vector2d>>::PerformDijkstrasAlgorithm(graph, 0);
+
+    std::cout << "\n8-connected edges\n"
+              << "Node index            : 0, 1, 2, 3, 4, 5, 6, 7, 8\n";
+    std::cout << "Previous graph indices: " << PrettyPrint::PrettyPrint(dijkstras_result_8connected.second.first) << std::endl;
+    std::cout << "Distance              : " << PrettyPrint::PrettyPrint(dijkstras_result_8connected.second.second) << std::endl;
+
+    std::cout << "\nSerialization test... ";
+
+    arc_dijkstras::Graph<Eigen::Vector2d> serialization_test_graph(2);
+    serialization_test_graph.AddNode(Eigen::Vector2d(0,0));
+    serialization_test_graph.AddNode(Eigen::Vector2d(1,1));
+    serialization_test_graph.AddEdgesBetweenNodes(0, 1, 1.0);
+
+    // Define the graph value serialization function
+    const auto value_serializer_fn = [] (const Eigen::Vector2d& value, std::vector<uint8_t>& buffer)
+    {
+        const uint64_t start_buffer_size = buffer.size();
+        uint64_t running_total = 0;
+
+        running_total += arc_utilities::SerializeFixedSizePOD<double>(value(0), buffer);
+        running_total += arc_utilities::SerializeFixedSizePOD<double>(value(1), buffer);
+
+        const uint64_t end_buffer_size = buffer.size();
+        const uint64_t bytes_written = end_buffer_size - start_buffer_size;
+
+        assert(running_total == bytes_written);
+
+        return bytes_written;
+    };
+
+    // Define the graph value serialization function
+    const auto value_deserializer_fn = [] (const std::vector<uint8_t>& buffer, const uint64_t current)
+    {
+        uint64_t current_position = current;
+
+        // Deserialze 2 doubles
+        std::pair<double, uint64_t> x = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+        current_position += x.second;
+        std::pair<double, uint64_t> y = arc_utilities::DeserializeFixedSizePOD<double>(buffer, current_position);
+        current_position += y.second;
+
+        const Eigen::Vector2d deserialized(x.first, y.first);
+
+        // Figure out how many bytes were read
+        const uint64_t bytes_read = current_position - current;
+        return std::make_pair(deserialized, bytes_read);
+    };
+
+    // Serialze the graph
+    std::vector<uint8_t> buffer;
+    serialization_test_graph.SerializeSelf(buffer, value_serializer_fn);
+
+    auto deserialized_result = arc_dijkstras::Graph<Eigen::Vector2d>::Deserialize(buffer, 0, value_deserializer_fn);
+    assert(deserialized_result.first.CheckGraphLinkage());
+
+    std::cout << "passed" << std::endl;
+
+    return 0;
+}

flightlib/third_party/arc_utilities/src/test_eigen_math.cpp

@@ -0,0 +1,96 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <random>
+#include <chrono>
+#include <arc_utilities/eigen_helpers.hpp>
+#include <arc_utilities/aligned_eigen_types.hpp>
+#include <arc_utilities/pretty_print.hpp>
+
+inline void TestVector3d(const ssize_t iterations, const Eigen::Isometry3d& base_transform, Eigen::MatrixXd& results)
+{
+    std::chrono::time_point<std::chrono::steady_clock> vector3_start_time = std::chrono::steady_clock::now();
+    for (ssize_t idx = 0; idx < iterations; idx++)
+    {
+        Eigen::Vector3d test_vector(1.0 * (double)idx, 2.0 * (double)idx, 3.0 * (double)idx);
+        results.block<3, 1>(idx * 4, 0) = base_transform * test_vector;
+    }
+    std::chrono::time_point<std::chrono::steady_clock> vector3_end_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> vector3_test_time(vector3_end_time - vector3_start_time);
+    std::cout << "Isometry3d * Vector3d test - " << vector3_test_time.count() << "s for " << iterations << " iterations to produce " << results.block<4, 1>(100000, 0) << std::endl;
+}
+
+inline void TestVector4d(const ssize_t iterations, const Eigen::Isometry3d& base_transform, Eigen::MatrixXd& results)
+{
+    std::chrono::time_point<std::chrono::steady_clock> vector4_start_time = std::chrono::steady_clock::now();
+    for (ssize_t idx = 0; idx < iterations; idx++)
+    {
+        Eigen::Vector4d test_vector(1.0 * (double)idx, 2.0 * (double)idx, 3.0 * (double)idx, 1.0);
+        results.block<4, 1>(idx * 4, 0) = base_transform * test_vector;
+    }
+    std::chrono::time_point<std::chrono::steady_clock> vector4_end_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> vector4_test_time(vector4_end_time - vector4_start_time);
+    std::cout << "Isometry3d * Vector4d test - " << vector4_test_time.count() << "s for " << iterations << " iterations to produce " << results.block<4, 1>(100000, 0) << std::endl;
+}
+
+inline void TestAlignedVector3d(const ssize_t iterations, const Eigen::Isometry3d& base_transform, Eigen::MatrixXd& results)
+{
+    std::chrono::time_point<std::chrono::steady_clock> alignedvector3_start_time = std::chrono::steady_clock::now();
+    for (ssize_t idx = 0; idx < iterations; idx++)
+    {
+        Eigen::Aligned4Vector3<double> test_vector(1.0 * (double)idx, 2.0 * (double)idx, 3.0 * (double)idx);
+        results.block<3, 1>(idx * 4, 0) = base_transform * test_vector;
+    }
+    std::chrono::time_point<std::chrono::steady_clock> alignedvector3_end_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> alignedvector3_test_time(alignedvector3_end_time - alignedvector3_start_time);
+    std::cout << "Isometry3d * AlignedVector3d test - " << alignedvector3_test_time.count() << "s for " << iterations << " iterations to produce " << results.block<4, 1>(100000, 0) << std::endl;
+}
+
+inline void TestManual(const ssize_t iterations, const Eigen::Isometry3d& base_transform, Eigen::MatrixXd& results)
+{
+    const Eigen::Matrix4d& base_transform_matrix = base_transform.matrix();
+    std::chrono::time_point<std::chrono::steady_clock> manual_start_time = std::chrono::steady_clock::now();
+    for (ssize_t idx = 0; idx < iterations; idx++)
+    {
+        Eigen::Vector4d test_vector(1.0 * (double)idx, 2.0 * (double)idx, 3.0 * (double)idx, 1.0);
+        results.block<4, 1>(idx * 4, 0) = base_transform_matrix * test_vector;
+    }
+    std::chrono::time_point<std::chrono::steady_clock> manual_end_time = std::chrono::steady_clock::now();
+    std::chrono::duration<double> manual_test_time(manual_end_time - manual_start_time);
+    std::cout << "Matrix4d * Vector4d test - " << manual_test_time.count() << "s for " << iterations << " iterations to produce " << results.block<4, 1>(100000, 0) << std::endl;
+}
+
+int main(int argc, char** argv)
+{
+    printf("%d arguments\n", argc);
+    for (int idx = 0; idx < argc; idx++)
+    {
+        printf("Argument %d: %s\n", idx, argv[idx]);
+    }
+    const ssize_t iterations = 100000000;
+    const Eigen::Isometry3d base_transform = Eigen::Translation3d(10.0, 10.0, 10.0) * EigenHelpers::QuaternionFromRPY(0.25, 0.5, 0.75);
+    Eigen::MatrixXd results = Eigen::MatrixXd::Ones(iterations * 4, 1);
+    TestVector3d(iterations, base_transform, results);
+    TestVector3d(iterations, base_transform, results);
+    TestVector3d(iterations, base_transform, results);
+    TestVector3d(iterations, base_transform, results);
+    TestVector3d(iterations, base_transform, results);
+    //
+    TestVector4d(iterations, base_transform, results);
+    TestVector4d(iterations, base_transform, results);
+    TestVector4d(iterations, base_transform, results);
+    TestVector4d(iterations, base_transform, results);
+    TestVector4d(iterations, base_transform, results);
+    //
+    TestAlignedVector3d(iterations, base_transform, results);
+    TestAlignedVector3d(iterations, base_transform, results);
+    TestAlignedVector3d(iterations, base_transform, results);
+    TestAlignedVector3d(iterations, base_transform, results);
+    TestAlignedVector3d(iterations, base_transform, results);
+    //
+    TestManual(iterations, base_transform, results);
+    TestManual(iterations, base_transform, results);
+    TestManual(iterations, base_transform, results);
+    TestManual(iterations, base_transform, results);
+    TestManual(iterations, base_transform, results);
+    return 0;
+}

flightlib/third_party/arc_utilities/src/test_hierarchical_clustering.cpp

@@ -0,0 +1,64 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <random>
+#include <chrono>
+#include <iostream>
+#include <fstream>
+#include <arc_utilities/arc_helpers.hpp>
+#include <arc_utilities/eigen_helpers.hpp>
+#include <arc_utilities/pretty_print.hpp>
+#include <arc_utilities/simple_hierarchical_clustering.hpp>
+#include <arc_utilities/abb_irb1600_145_fk_fast.hpp>
+
+int main(int argc, char** argv)
+{
+    printf("%d arguments\n", argc);
+    for (int idx = 0; idx < argc; idx++)
+    {
+        printf("Argument %d: %s\n", idx, argv[idx]);
+    }
+    const size_t num_points = (argc >= 2) ? (size_t)(atoi(argv[1])) : 1000;
+    std::cout << "Generating " << num_points << " random points..." << std::endl;
+    const auto seed = std::chrono::high_resolution_clock::now().time_since_epoch().count();
+    std::mt19937_64 prng(seed);
+    std::uniform_real_distribution<double> dist(0.0, 10.0);
+    EigenHelpers::VectorVector3d random_points(num_points);
+    std::vector<size_t> indices(num_points);
+    for (size_t idx = 0; idx < num_points; idx++)
+    {
+        const double x = dist(prng);
+        const double y = dist(prng);
+        random_points[idx] = Eigen::Vector3d(x, y, 0.0);
+        indices[idx] = idx;
+    }
+    std::cout << "Clustering " << num_points << " points..." << std::endl;
+    std::function<double(const Eigen::Vector3d&, const Eigen::Vector3d&)> distance_fn = [] (const Eigen::Vector3d& v1, const Eigen::Vector3d& v2) { return EigenHelpers::Distance(v1, v2); };
+    const Eigen::MatrixXd distance_matrix = arc_helpers::BuildDistanceMatrix(random_points, distance_fn);
+    const std::vector<std::vector<size_t>> clusters = simple_hierarchical_clustering::SimpleHierarchicalClustering::Cluster(indices, distance_matrix, 1.0).first;
+    for (size_t cluster_idx = 0; cluster_idx < clusters.size(); cluster_idx++)
+    {
+        const std::vector<size_t>& current_cluster = clusters[cluster_idx];
+        const double cluster_num = 1.0 + (double)cluster_idx;
+        for (size_t element_idx = 0; element_idx < current_cluster.size(); element_idx++)
+        {
+            const size_t index = current_cluster[element_idx];
+            random_points[index].z() = cluster_num;
+        }
+    }
+    std::cout << "Saving to CSV..." << std::endl;
+    const std::string log_file_name = (argc >=3 ) ? std::string(argv[2]) : "/tmp/test_hierarchical_clustering.csv";
+    std::ofstream log_file(log_file_name, std::ios_base::out);
+    if (!log_file.is_open())
+    {
+        std::cerr << "\x1b[31;1m Unable to create folder/file to log to: " << log_file_name << "\x1b[0m \n";
+        throw std::invalid_argument("Log filename must be write-openable");
+    }
+    for (size_t idx = 0; idx < num_points; idx++)
+    {
+        const Eigen::Vector3d& point = random_points[idx];
+        log_file << point.x() << "," << point.y() << "," << point.z() << std::endl;
+    }
+    log_file.close();
+    std::cout << "Done saving, you can import into matlab and draw with \"scatter3(x, y, z, 50, z, '.')\"" << std::endl;
+    return 0;
+}

flightlib/third_party/arc_utilities/src/test_shortcut_smoothing.cpp

@@ -0,0 +1,26 @@
+#include <iostream>
+#include "arc_utilities/shortcut_smoothing.hpp"
+#include "arc_utilities/pretty_print.hpp"
+
+int main(int argc, char** argv)
+{
+    (void)argc;
+    (void)argv;
+
+    EigenHelpers::VectorVector3d vec(10);
+    for (int i = 0; i < 10; ++i)
+    {
+        vec[i] = Eigen::Vector3d(i, 0.0, 0.0);
+    }
+
+    const double step_size = 0.1;
+    const auto collision_fn = [] (const Eigen::Vector3d& location) { (void)location; return location.norm() > 4.0; };
+
+    auto vec_smoothed = shortcut_smoothing::InterpolateWithCollisionCheck(vec, 1, 4, step_size, collision_fn);
+
+
+    std::cout << "Original:\n" << PrettyPrint::PrettyPrint(vec, true, "\n") << std::endl << std::endl;
+    std::cout << "Smoothed:\n" << PrettyPrint::PrettyPrint(vec_smoothed, true, "\n") << std::endl << std::endl;
+
+    return 0;
+}

flightlib/third_party/arc_utilities/src/timing.cpp

@@ -0,0 +1,7 @@
+#include "arc_utilities/timing.hpp"
+
+double arc_utilities::GlobalStopwatch(const StopwatchControl control)
+{
+    static arc_utilities::Stopwatch global_stopwatch;
+    return global_stopwatch(control);
+}

flightlib/third_party/sdf_tools/CMakeLists.txt

@@ -0,0 +1,138 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(sdf_tools)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS rospy std_msgs sensor_msgs visualization_msgs image_transport cv_bridge arc_utilities message_generation)
+find_package(cmake_modules REQUIRED)
+find_package(Eigen3 REQUIRED)
+set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
+find_package(OpenCV REQUIRED)
+# MoveIt! isn't a required dependency
+find_package(moveit_msgs)
+find_package(moveit_core)
+find_package(moveit_ros_planning)
+find_package(PCL 1.10 REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/groovy/api/catkin/html/user_guide/setup_dot_py.html
+#catkin_python_setup()
+
+#######################################
+## Declare ROS messages and services ##
+#######################################
+
+## Generate messages in the 'msg' folder
+add_message_files(DIRECTORY msg FILES SDF.msg CollisionMap.msg TaggedObjectCollisionMap.msg)
+
+## Generate services in the 'srv' folder
+add_service_files(DIRECTORY srv FILES ComputeSDF.srv)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(DEPENDENCIES geometry_msgs std_msgs)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+if(moveit_core_FOUND)
+    catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS rospy std_msgs sensor_msgs moveit_core moveit_ros_planning visualization_msgs moveit_msgs image_transport cv_bridge arc_utilities message_runtime DEPENDS Eigen3 OpenCV)
+else()
+    catkin_package(INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} CATKIN_DEPENDS rospy std_msgs sensor_msgs visualization_msgs image_transport cv_bridge arc_utilities message_runtime DEPENDS Eigen3 OpenCV)
+endif()
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+if(moveit_core_FOUND)
+    include_directories(include SYSTEM ${catkin_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${moveit_core_INCLUDE_DIRS} ${moveit_msgs_INCLUDE_DIRS} ${moveit_ros_planning_INCLUDE_DIRS})
+else()
+    include_directories(include SYSTEM ${catkin_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS})
+endif()
+
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+
+set(CMAKE_CXX_FLAGS "-std=c++14 ${CMAKE_CXX_FLAGS} -flto -O3 -Wall -Wextra -Werror")
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
+
+if(moveit_core_FOUND)
+    message(STATUS "MoveIt! found. Building SDF Tools library with MoveIt! integration.")
+    # SDF library
+    add_library(${PROJECT_NAME}
+        include/${PROJECT_NAME}/collision_map.hpp
+        include/${PROJECT_NAME}/tagged_object_collision_map.hpp
+        include/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.hpp
+        include/${PROJECT_NAME}/sdf.hpp
+        include/${PROJECT_NAME}/sdf_builder.hpp
+        src/${PROJECT_NAME}/collision_map.cpp
+        src/${PROJECT_NAME}/tagged_object_collision_map.cpp
+        src/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.cpp
+        src/${PROJECT_NAME}/sdf.cpp
+        src/${PROJECT_NAME}/sdf_builder.cpp)
+    add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp)
+    target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${moveit_core_LIBRARIES} ${moveit_msgs_LIBRARIES} ${moveit_ros_planning_LIBRARIES})
+    # SDF generation node
+    add_executable(sdf_generation_node src/sdf_generation_node.cpp)
+    add_dependencies(sdf_generation_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp)
+    target_link_libraries(sdf_generation_node ${catkin_LIBRARIES} ${PROJECT_NAME} ${moveit_core_LIBRARIES} ${moveit_msgs_LIBRARIES} ${moveit_ros_planning_LIBRARIES})
+else()
+    message(STATUS "MoveIt! not found. Building SDF Tools library without MoveIt! integration.")
+    # SDF library
+    add_library(${PROJECT_NAME}
+        include/${PROJECT_NAME}/collision_map.hpp
+        include/${PROJECT_NAME}/tagged_object_collision_map.hpp
+        include/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.hpp
+        include/${PROJECT_NAME}/sdf.hpp
+        src/${PROJECT_NAME}/collision_map.cpp
+        src/${PROJECT_NAME}/tagged_object_collision_map.cpp
+        src/${PROJECT_NAME}/dynamic_spatial_hashed_collision_map.cpp
+        src/${PROJECT_NAME}/sdf.cpp)
+    add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp)
+    target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+endif()
+
+# Simple test node for voxel_grid
+# add_executable(voxel_grid_test src/test_voxel_grid.cpp)
+# add_dependencies(voxel_grid_test ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp)
+# target_link_libraries(voxel_grid_test ${catkin_LIBRARIES} ${PROJECT_NAME})
+
+# Image to SDF node
+# add_executable(image_2d_sdf_node src/image_2d_sdf_node.cpp)
+# add_dependencies(image_2d_sdf_node ${catkin_EXPORTED_TARGETS})
+# target_link_libraries(image_2d_sdf_node ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
+
+# Tutorial on how to use the components of the sdf_tools library
+# add_executable(sdf_tools_tutorial src/sdf_tools_tutorial.cpp)
+# add_dependencies(sdf_tools_tutorial ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencpp)
+# target_link_libraries(sdf_tools_tutorial ${catkin_LIBRARIES} ${PROJECT_NAME})
+
+#############
+## Install ##
+#############
+
+## Mark library for installation
+install(TARGETS ${PROJECT_NAME}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  ARCHIVE DESTINATION ${PROJECT_SOURCE_DIR}/lib
+  RUNTIME DESTINATION ${PROJECT_SOURCE_DIR}/lib
+  LIBRARY DESTINATION ${PROJECT_SOURCE_DIR}/lib
+)
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${PROJECT_SOURCE_DIR}/include/sdf_tools
+  PATTERN ".svn" EXCLUDE
+)

flightlib/third_party/sdf_tools/LICENSE

@@ -0,0 +1,23 @@
+Copyright (c) 2014, Autonomous Robotic Collaboration Laboratory
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

flightlib/third_party/sdf_tools/README.md

@@ -0,0 +1,6 @@
+sdf_tools
+=========
+
+Builds 2D signed distance fields from images, 3D signed distance fields from pointclouds, 3D signed distance fields from Octomap, provides a lightweight signed distance field library, message types for signed distance fields, and tools to compress signed distance fields for transport.
+
+A simple example/tutorial is provided, see the [Wiki](https://github.com/WPI-ARC/sdf_tools/wiki).

flightlib/third_party/sdf_tools/include/sdf_tools/CollisionMap.h

@@ -0,0 +1,323 @@
+// Generated by gencpp from file sdf_tools/CollisionMap.msg
+// DO NOT EDIT!
+
+
+#ifndef SDF_TOOLS_MESSAGE_COLLISIONMAP_H
+#define SDF_TOOLS_MESSAGE_COLLISIONMAP_H
+
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Transform.h>
+#include <geometry_msgs/Vector3.h>
+
+namespace sdf_tools
+{
+template <class ContainerAllocator>
+struct CollisionMap_
+{
+  typedef CollisionMap_<ContainerAllocator> Type;
+
+  CollisionMap_()
+    : header()
+    , origin_transform()
+    , dimensions()
+    , cell_size(0.0)
+    , OOB_occupancy_value(0.0)
+    , OOB_component_value(0)
+    , number_of_components(0)
+    , components_valid(false)
+    , initialized(false)
+    , data()  {
+    }
+  CollisionMap_(const ContainerAllocator& _alloc)
+    : header(_alloc)
+    , origin_transform(_alloc)
+    , dimensions(_alloc)
+    , cell_size(0.0)
+    , OOB_occupancy_value(0.0)
+    , OOB_component_value(0)
+    , number_of_components(0)
+    , components_valid(false)
+    , initialized(false)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::Header_<ContainerAllocator>  _header_type;
+  _header_type header;
+
+   typedef  ::geometry_msgs::Transform_<ContainerAllocator>  _origin_transform_type;
+  _origin_transform_type origin_transform;
+
+   typedef  ::geometry_msgs::Vector3_<ContainerAllocator>  _dimensions_type;
+  _dimensions_type dimensions;
+
+   typedef double _cell_size_type;
+  _cell_size_type cell_size;
+
+   typedef float _OOB_occupancy_value_type;
+  _OOB_occupancy_value_type OOB_occupancy_value;
+
+   typedef uint32_t _OOB_component_value_type;
+  _OOB_component_value_type OOB_component_value;
+
+   typedef uint32_t _number_of_components_type;
+  _number_of_components_type number_of_components;
+
+   typedef uint8_t _components_valid_type;
+  _components_valid_type components_valid;
+
+   typedef uint8_t _initialized_type;
+  _initialized_type initialized;
+
+   typedef std::vector<uint8_t, typename ContainerAllocator::template rebind<uint8_t>::other >  _data_type;
+  _data_type data;
+
+
+
+
+  typedef boost::shared_ptr< ::sdf_tools::CollisionMap_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::sdf_tools::CollisionMap_<ContainerAllocator> const> ConstPtr;
+
+}; // struct CollisionMap_
+
+typedef ::sdf_tools::CollisionMap_<std::allocator<void> > CollisionMap;
+
+typedef boost::shared_ptr< ::sdf_tools::CollisionMap > CollisionMapPtr;
+typedef boost::shared_ptr< ::sdf_tools::CollisionMap const> CollisionMapConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::sdf_tools::CollisionMap_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::sdf_tools::CollisionMap_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+} // namespace sdf_tools
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True}
+// {'std_msgs': ['/opt/ros/indigo/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/indigo/share/geometry_msgs/cmake/../msg'], 'sdf_tools': ['/home/zby/workspaces/temp_ws/src/sdf_tools/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sdf_tools::CollisionMap_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sdf_tools::CollisionMap_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sdf_tools::CollisionMap_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sdf_tools::CollisionMap_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sdf_tools::CollisionMap_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sdf_tools::CollisionMap_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::sdf_tools::CollisionMap_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "69b7e5097be57c5900575a10000bd373";
+  }
+
+  static const char* value(const ::sdf_tools::CollisionMap_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0x69b7e5097be57c59ULL;
+  static const uint64_t static_value2 = 0x00575a10000bd373ULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::sdf_tools::CollisionMap_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "sdf_tools/CollisionMap";
+  }
+
+  static const char* value(const ::sdf_tools::CollisionMap_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::sdf_tools::CollisionMap_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "std_msgs/Header header\n\
+geometry_msgs/Transform origin_transform\n\
+geometry_msgs/Vector3 dimensions\n\
+float64 cell_size\n\
+float32 OOB_occupancy_value\n\
+uint32 OOB_component_value\n\
+uint32 number_of_components\n\
+bool components_valid\n\
+bool initialized\n\
+uint8[] data\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/Header\n\
+# Standard metadata for higher-level stamped data types.\n\
+# This is generally used to communicate timestamped data \n\
+# in a particular coordinate frame.\n\
+# \n\
+# sequence ID: consecutively increasing ID \n\
+uint32 seq\n\
+#Two-integer timestamp that is expressed as:\n\
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
+# time-handling sugar is provided by the client library\n\
+time stamp\n\
+#Frame this data is associated with\n\
+# 0: no frame\n\
+# 1: global frame\n\
+string frame_id\n\
+\n\
+================================================================================\n\
+MSG: geometry_msgs/Transform\n\
+# This represents the transform between two coordinate frames in free space.\n\
+\n\
+Vector3 translation\n\
+Quaternion rotation\n\
+\n\
+================================================================================\n\
+MSG: geometry_msgs/Vector3\n\
+# This represents a vector in free space. \n\
+# It is only meant to represent a direction. Therefore, it does not\n\
+# make sense to apply a translation to it (e.g., when applying a \n\
+# generic rigid transformation to a Vector3, tf2 will only apply the\n\
+# rotation). If you want your data to be translatable too, use the\n\
+# geometry_msgs/Point message instead.\n\
+\n\
+float64 x\n\
+float64 y\n\
+float64 z\n\
+================================================================================\n\
+MSG: geometry_msgs/Quaternion\n\
+# This represents an orientation in free space in quaternion form.\n\
+\n\
+float64 x\n\
+float64 y\n\
+float64 z\n\
+float64 w\n\
+";
+  }
+
+  static const char* value(const ::sdf_tools::CollisionMap_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::sdf_tools::CollisionMap_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.header);
+      stream.next(m.origin_transform);
+      stream.next(m.dimensions);
+      stream.next(m.cell_size);
+      stream.next(m.OOB_occupancy_value);
+      stream.next(m.OOB_component_value);
+      stream.next(m.number_of_components);
+      stream.next(m.components_valid);
+      stream.next(m.initialized);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct CollisionMap_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::sdf_tools::CollisionMap_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sdf_tools::CollisionMap_<ContainerAllocator>& v)
+  {
+    s << indent << "header: ";
+    s << std::endl;
+    Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + "  ", v.header);
+    s << indent << "origin_transform: ";
+    s << std::endl;
+    Printer< ::geometry_msgs::Transform_<ContainerAllocator> >::stream(s, indent + "  ", v.origin_transform);
+    s << indent << "dimensions: ";
+    s << std::endl;
+    Printer< ::geometry_msgs::Vector3_<ContainerAllocator> >::stream(s, indent + "  ", v.dimensions);
+    s << indent << "cell_size: ";
+    Printer<double>::stream(s, indent + "  ", v.cell_size);
+    s << indent << "OOB_occupancy_value: ";
+    Printer<float>::stream(s, indent + "  ", v.OOB_occupancy_value);
+    s << indent << "OOB_component_value: ";
+    Printer<uint32_t>::stream(s, indent + "  ", v.OOB_component_value);
+    s << indent << "number_of_components: ";
+    Printer<uint32_t>::stream(s, indent + "  ", v.number_of_components);
+    s << indent << "components_valid: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.components_valid);
+    s << indent << "initialized: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.initialized);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer<uint8_t>::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // SDF_TOOLS_MESSAGE_COLLISIONMAP_H

flightlib/third_party/sdf_tools/include/sdf_tools/ComputeSDF.h

@@ -0,0 +1,123 @@
+// Generated by gencpp from file sdf_tools/ComputeSDF.msg
+// DO NOT EDIT!
+
+
+#ifndef SDF_TOOLS_MESSAGE_COMPUTESDF_H
+#define SDF_TOOLS_MESSAGE_COMPUTESDF_H
+
+#include <ros/service_traits.h>
+
+
+#include <sdf_tools/ComputeSDFRequest.h>
+#include <sdf_tools/ComputeSDFResponse.h>
+
+
+namespace sdf_tools
+{
+
+struct ComputeSDF
+{
+
+typedef ComputeSDFRequest Request;
+typedef ComputeSDFResponse Response;
+Request request;
+Response response;
+
+typedef Request RequestType;
+typedef Response ResponseType;
+
+}; // struct ComputeSDF
+} // namespace sdf_tools
+
+
+namespace ros
+{
+namespace service_traits
+{
+
+
+template<>
+struct MD5Sum< ::sdf_tools::ComputeSDF > {
+  static const char* value()
+  {
+    return "567be5b04fd66c34e03a51193aff2d4a";
+  }
+
+  static const char* value(const ::sdf_tools::ComputeSDF&) { return value(); }
+};
+
+template<>
+struct DataType< ::sdf_tools::ComputeSDF > {
+  static const char* value()
+  {
+    return "sdf_tools/ComputeSDF";
+  }
+
+  static const char* value(const ::sdf_tools::ComputeSDF&) { return value(); }
+};
+
+
+// service_traits::MD5Sum< ::sdf_tools::ComputeSDFRequest> should match 
+// service_traits::MD5Sum< ::sdf_tools::ComputeSDF > 
+template<>
+struct MD5Sum< ::sdf_tools::ComputeSDFRequest>
+{
+  static const char* value()
+  {
+    return MD5Sum< ::sdf_tools::ComputeSDF >::value();
+  }
+  static const char* value(const ::sdf_tools::ComputeSDFRequest&)
+  {
+    return value();
+  }
+};
+
+// service_traits::DataType< ::sdf_tools::ComputeSDFRequest> should match 
+// service_traits::DataType< ::sdf_tools::ComputeSDF > 
+template<>
+struct DataType< ::sdf_tools::ComputeSDFRequest>
+{
+  static const char* value()
+  {
+    return DataType< ::sdf_tools::ComputeSDF >::value();
+  }
+  static const char* value(const ::sdf_tools::ComputeSDFRequest&)
+  {
+    return value();
+  }
+};
+
+// service_traits::MD5Sum< ::sdf_tools::ComputeSDFResponse> should match 
+// service_traits::MD5Sum< ::sdf_tools::ComputeSDF > 
+template<>
+struct MD5Sum< ::sdf_tools::ComputeSDFResponse>
+{
+  static const char* value()
+  {
+    return MD5Sum< ::sdf_tools::ComputeSDF >::value();
+  }
+  static const char* value(const ::sdf_tools::ComputeSDFResponse&)
+  {
+    return value();
+  }
+};
+
+// service_traits::DataType< ::sdf_tools::ComputeSDFResponse> should match 
+// service_traits::DataType< ::sdf_tools::ComputeSDF > 
+template<>
+struct DataType< ::sdf_tools::ComputeSDFResponse>
+{
+  static const char* value()
+  {
+    return DataType< ::sdf_tools::ComputeSDF >::value();
+  }
+  static const char* value(const ::sdf_tools::ComputeSDFResponse&)
+  {
+    return value();
+  }
+};
+
+} // namespace service_traits
+} // namespace ros
+
+#endif // SDF_TOOLS_MESSAGE_COMPUTESDF_H

flightlib/third_party/sdf_tools/include/sdf_tools/ComputeSDFRequest.h

@@ -0,0 +1,186 @@
+// Generated by gencpp from file sdf_tools/ComputeSDFRequest.msg
+// DO NOT EDIT!
+
+
+#ifndef SDF_TOOLS_MESSAGE_COMPUTESDFREQUEST_H
+#define SDF_TOOLS_MESSAGE_COMPUTESDFREQUEST_H
+
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+
+namespace sdf_tools
+{
+template <class ContainerAllocator>
+struct ComputeSDFRequest_
+{
+  typedef ComputeSDFRequest_<ContainerAllocator> Type;
+
+  ComputeSDFRequest_()
+    : request_new(false)  {
+    }
+  ComputeSDFRequest_(const ContainerAllocator& _alloc)
+    : request_new(false)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef uint8_t _request_new_type;
+  _request_new_type request_new;
+
+
+
+
+  typedef boost::shared_ptr< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> const> ConstPtr;
+
+}; // struct ComputeSDFRequest_
+
+typedef ::sdf_tools::ComputeSDFRequest_<std::allocator<void> > ComputeSDFRequest;
+
+typedef boost::shared_ptr< ::sdf_tools::ComputeSDFRequest > ComputeSDFRequestPtr;
+typedef boost::shared_ptr< ::sdf_tools::ComputeSDFRequest const> ComputeSDFRequestConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+} // namespace sdf_tools
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': True, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/opt/ros/indigo/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/indigo/share/geometry_msgs/cmake/../msg'], 'sdf_tools': ['/home/zby/workspaces/temp_ws/src/sdf_tools/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "70082b15c0185876dcce41c4eb98be14";
+  }
+
+  static const char* value(const ::sdf_tools::ComputeSDFRequest_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0x70082b15c0185876ULL;
+  static const uint64_t static_value2 = 0xdcce41c4eb98be14ULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "sdf_tools/ComputeSDFRequest";
+  }
+
+  static const char* value(const ::sdf_tools::ComputeSDFRequest_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "bool request_new\n\
+";
+  }
+
+  static const char* value(const ::sdf_tools::ComputeSDFRequest_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.request_new);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct ComputeSDFRequest_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::sdf_tools::ComputeSDFRequest_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sdf_tools::ComputeSDFRequest_<ContainerAllocator>& v)
+  {
+    s << indent << "request_new: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.request_new);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // SDF_TOOLS_MESSAGE_COMPUTESDFREQUEST_H

flightlib/third_party/sdf_tools/include/sdf_tools/ComputeSDFResponse.h

@@ -0,0 +1,255 @@
+// Generated by gencpp from file sdf_tools/ComputeSDFResponse.msg
+// DO NOT EDIT!
+
+
+#ifndef SDF_TOOLS_MESSAGE_COMPUTESDFRESPONSE_H
+#define SDF_TOOLS_MESSAGE_COMPUTESDFRESPONSE_H
+
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+#include <sdf_tools/SDF.h>
+
+namespace sdf_tools
+{
+template <class ContainerAllocator>
+struct ComputeSDFResponse_
+{
+  typedef ComputeSDFResponse_<ContainerAllocator> Type;
+
+  ComputeSDFResponse_()
+    : is_valid(false)
+    , sdf()  {
+    }
+  ComputeSDFResponse_(const ContainerAllocator& _alloc)
+    : is_valid(false)
+    , sdf(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef uint8_t _is_valid_type;
+  _is_valid_type is_valid;
+
+   typedef  ::sdf_tools::SDF_<ContainerAllocator>  _sdf_type;
+  _sdf_type sdf;
+
+
+
+
+  typedef boost::shared_ptr< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> const> ConstPtr;
+
+}; // struct ComputeSDFResponse_
+
+typedef ::sdf_tools::ComputeSDFResponse_<std::allocator<void> > ComputeSDFResponse;
+
+typedef boost::shared_ptr< ::sdf_tools::ComputeSDFResponse > ComputeSDFResponsePtr;
+typedef boost::shared_ptr< ::sdf_tools::ComputeSDFResponse const> ComputeSDFResponseConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+} // namespace sdf_tools
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': False}
+// {'std_msgs': ['/opt/ros/indigo/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/indigo/share/geometry_msgs/cmake/../msg'], 'sdf_tools': ['/home/zby/workspaces/temp_ws/src/sdf_tools/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "fbf70ecbf2634799341a7255b0c416e3";
+  }
+
+  static const char* value(const ::sdf_tools::ComputeSDFResponse_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0xfbf70ecbf2634799ULL;
+  static const uint64_t static_value2 = 0x341a7255b0c416e3ULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "sdf_tools/ComputeSDFResponse";
+  }
+
+  static const char* value(const ::sdf_tools::ComputeSDFResponse_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "bool is_valid\n\
+sdf_tools/SDF sdf\n\
+\n\
+\n\
+================================================================================\n\
+MSG: sdf_tools/SDF\n\
+std_msgs/Header header\n\
+geometry_msgs/Transform origin_transform\n\
+geometry_msgs/Vector3 dimensions\n\
+float64 sdf_cell_size\n\
+float32 OOB_value\n\
+bool initialized\n\
+bool locked\n\
+uint8[] data\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/Header\n\
+# Standard metadata for higher-level stamped data types.\n\
+# This is generally used to communicate timestamped data \n\
+# in a particular coordinate frame.\n\
+# \n\
+# sequence ID: consecutively increasing ID \n\
+uint32 seq\n\
+#Two-integer timestamp that is expressed as:\n\
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
+# time-handling sugar is provided by the client library\n\
+time stamp\n\
+#Frame this data is associated with\n\
+# 0: no frame\n\
+# 1: global frame\n\
+string frame_id\n\
+\n\
+================================================================================\n\
+MSG: geometry_msgs/Transform\n\
+# This represents the transform between two coordinate frames in free space.\n\
+\n\
+Vector3 translation\n\
+Quaternion rotation\n\
+\n\
+================================================================================\n\
+MSG: geometry_msgs/Vector3\n\
+# This represents a vector in free space. \n\
+# It is only meant to represent a direction. Therefore, it does not\n\
+# make sense to apply a translation to it (e.g., when applying a \n\
+# generic rigid transformation to a Vector3, tf2 will only apply the\n\
+# rotation). If you want your data to be translatable too, use the\n\
+# geometry_msgs/Point message instead.\n\
+\n\
+float64 x\n\
+float64 y\n\
+float64 z\n\
+================================================================================\n\
+MSG: geometry_msgs/Quaternion\n\
+# This represents an orientation in free space in quaternion form.\n\
+\n\
+float64 x\n\
+float64 y\n\
+float64 z\n\
+float64 w\n\
+";
+  }
+
+  static const char* value(const ::sdf_tools::ComputeSDFResponse_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.is_valid);
+      stream.next(m.sdf);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct ComputeSDFResponse_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::sdf_tools::ComputeSDFResponse_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sdf_tools::ComputeSDFResponse_<ContainerAllocator>& v)
+  {
+    s << indent << "is_valid: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.is_valid);
+    s << indent << "sdf: ";
+    s << std::endl;
+    Printer< ::sdf_tools::SDF_<ContainerAllocator> >::stream(s, indent + "  ", v.sdf);
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // SDF_TOOLS_MESSAGE_COMPUTESDFRESPONSE_H

flightlib/third_party/sdf_tools/include/sdf_tools/SDF.h

@@ -0,0 +1,305 @@
+// Generated by gencpp from file sdf_tools/SDF.msg
+// DO NOT EDIT!
+
+
+#ifndef SDF_TOOLS_MESSAGE_SDF_H
+#define SDF_TOOLS_MESSAGE_SDF_H
+
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Transform.h>
+#include <geometry_msgs/Vector3.h>
+
+namespace sdf_tools
+{
+template <class ContainerAllocator>
+struct SDF_
+{
+  typedef SDF_<ContainerAllocator> Type;
+
+  SDF_()
+    : header()
+    , origin_transform()
+    , dimensions()
+    , sdf_cell_size(0.0)
+    , OOB_value(0.0)
+    , initialized(false)
+    , locked(false)
+    , data()  {
+    }
+  SDF_(const ContainerAllocator& _alloc)
+    : header(_alloc)
+    , origin_transform(_alloc)
+    , dimensions(_alloc)
+    , sdf_cell_size(0.0)
+    , OOB_value(0.0)
+    , initialized(false)
+    , locked(false)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::Header_<ContainerAllocator>  _header_type;
+  _header_type header;
+
+   typedef  ::geometry_msgs::Transform_<ContainerAllocator>  _origin_transform_type;
+  _origin_transform_type origin_transform;
+
+   typedef  ::geometry_msgs::Vector3_<ContainerAllocator>  _dimensions_type;
+  _dimensions_type dimensions;
+
+   typedef double _sdf_cell_size_type;
+  _sdf_cell_size_type sdf_cell_size;
+
+   typedef float _OOB_value_type;
+  _OOB_value_type OOB_value;
+
+   typedef uint8_t _initialized_type;
+  _initialized_type initialized;
+
+   typedef uint8_t _locked_type;
+  _locked_type locked;
+
+   typedef std::vector<uint8_t, typename ContainerAllocator::template rebind<uint8_t>::other >  _data_type;
+  _data_type data;
+
+
+
+
+  typedef boost::shared_ptr< ::sdf_tools::SDF_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::sdf_tools::SDF_<ContainerAllocator> const> ConstPtr;
+
+}; // struct SDF_
+
+typedef ::sdf_tools::SDF_<std::allocator<void> > SDF;
+
+typedef boost::shared_ptr< ::sdf_tools::SDF > SDFPtr;
+typedef boost::shared_ptr< ::sdf_tools::SDF const> SDFConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::sdf_tools::SDF_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::sdf_tools::SDF_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+} // namespace sdf_tools
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True}
+// {'std_msgs': ['/opt/ros/indigo/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/indigo/share/geometry_msgs/cmake/../msg'], 'sdf_tools': ['/home/zby/workspaces/temp_ws/src/sdf_tools/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sdf_tools::SDF_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sdf_tools::SDF_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sdf_tools::SDF_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sdf_tools::SDF_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sdf_tools::SDF_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sdf_tools::SDF_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::sdf_tools::SDF_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "a7c1a3fc5ebce39f4d97049e22031ffc";
+  }
+
+  static const char* value(const ::sdf_tools::SDF_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0xa7c1a3fc5ebce39fULL;
+  static const uint64_t static_value2 = 0x4d97049e22031ffcULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::sdf_tools::SDF_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "sdf_tools/SDF";
+  }
+
+  static const char* value(const ::sdf_tools::SDF_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::sdf_tools::SDF_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "std_msgs/Header header\n\
+geometry_msgs/Transform origin_transform\n\
+geometry_msgs/Vector3 dimensions\n\
+float64 sdf_cell_size\n\
+float32 OOB_value\n\
+bool initialized\n\
+bool locked\n\
+uint8[] data\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/Header\n\
+# Standard metadata for higher-level stamped data types.\n\
+# This is generally used to communicate timestamped data \n\
+# in a particular coordinate frame.\n\
+# \n\
+# sequence ID: consecutively increasing ID \n\
+uint32 seq\n\
+#Two-integer timestamp that is expressed as:\n\
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
+# time-handling sugar is provided by the client library\n\
+time stamp\n\
+#Frame this data is associated with\n\
+# 0: no frame\n\
+# 1: global frame\n\
+string frame_id\n\
+\n\
+================================================================================\n\
+MSG: geometry_msgs/Transform\n\
+# This represents the transform between two coordinate frames in free space.\n\
+\n\
+Vector3 translation\n\
+Quaternion rotation\n\
+\n\
+================================================================================\n\
+MSG: geometry_msgs/Vector3\n\
+# This represents a vector in free space. \n\
+# It is only meant to represent a direction. Therefore, it does not\n\
+# make sense to apply a translation to it (e.g., when applying a \n\
+# generic rigid transformation to a Vector3, tf2 will only apply the\n\
+# rotation). If you want your data to be translatable too, use the\n\
+# geometry_msgs/Point message instead.\n\
+\n\
+float64 x\n\
+float64 y\n\
+float64 z\n\
+================================================================================\n\
+MSG: geometry_msgs/Quaternion\n\
+# This represents an orientation in free space in quaternion form.\n\
+\n\
+float64 x\n\
+float64 y\n\
+float64 z\n\
+float64 w\n\
+";
+  }
+
+  static const char* value(const ::sdf_tools::SDF_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::sdf_tools::SDF_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.header);
+      stream.next(m.origin_transform);
+      stream.next(m.dimensions);
+      stream.next(m.sdf_cell_size);
+      stream.next(m.OOB_value);
+      stream.next(m.initialized);
+      stream.next(m.locked);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct SDF_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::sdf_tools::SDF_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sdf_tools::SDF_<ContainerAllocator>& v)
+  {
+    s << indent << "header: ";
+    s << std::endl;
+    Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + "  ", v.header);
+    s << indent << "origin_transform: ";
+    s << std::endl;
+    Printer< ::geometry_msgs::Transform_<ContainerAllocator> >::stream(s, indent + "  ", v.origin_transform);
+    s << indent << "dimensions: ";
+    s << std::endl;
+    Printer< ::geometry_msgs::Vector3_<ContainerAllocator> >::stream(s, indent + "  ", v.dimensions);
+    s << indent << "sdf_cell_size: ";
+    Printer<double>::stream(s, indent + "  ", v.sdf_cell_size);
+    s << indent << "OOB_value: ";
+    Printer<float>::stream(s, indent + "  ", v.OOB_value);
+    s << indent << "initialized: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.initialized);
+    s << indent << "locked: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.locked);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer<uint8_t>::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // SDF_TOOLS_MESSAGE_SDF_H

flightlib/third_party/sdf_tools/include/sdf_tools/TaggedObjectCollisionMap.h

@@ -0,0 +1,327 @@
+// Generated by gencpp from file sdf_tools/TaggedObjectCollisionMap.msg
+// DO NOT EDIT!
+
+
+#ifndef SDF_TOOLS_MESSAGE_TAGGEDOBJECTCOLLISIONMAP_H
+#define SDF_TOOLS_MESSAGE_TAGGEDOBJECTCOLLISIONMAP_H
+
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Transform.h>
+#include <geometry_msgs/Vector3.h>
+
+namespace sdf_tools
+{
+template <class ContainerAllocator>
+struct TaggedObjectCollisionMap_
+{
+  typedef TaggedObjectCollisionMap_<ContainerAllocator> Type;
+
+  TaggedObjectCollisionMap_()
+    : header()
+    , origin_transform()
+    , dimensions()
+    , cell_size(0.0)
+    , number_of_components(0)
+    , components_valid(false)
+    , convex_segments_valid(false)
+    , initialized(false)
+    , OOB_value()
+    , data()  {
+    }
+  TaggedObjectCollisionMap_(const ContainerAllocator& _alloc)
+    : header(_alloc)
+    , origin_transform(_alloc)
+    , dimensions(_alloc)
+    , cell_size(0.0)
+    , number_of_components(0)
+    , components_valid(false)
+    , convex_segments_valid(false)
+    , initialized(false)
+    , OOB_value(_alloc)
+    , data(_alloc)  {
+  (void)_alloc;
+    }
+
+
+
+   typedef  ::std_msgs::Header_<ContainerAllocator>  _header_type;
+  _header_type header;
+
+   typedef  ::geometry_msgs::Transform_<ContainerAllocator>  _origin_transform_type;
+  _origin_transform_type origin_transform;
+
+   typedef  ::geometry_msgs::Vector3_<ContainerAllocator>  _dimensions_type;
+  _dimensions_type dimensions;
+
+   typedef double _cell_size_type;
+  _cell_size_type cell_size;
+
+   typedef uint32_t _number_of_components_type;
+  _number_of_components_type number_of_components;
+
+   typedef uint8_t _components_valid_type;
+  _components_valid_type components_valid;
+
+   typedef uint8_t _convex_segments_valid_type;
+  _convex_segments_valid_type convex_segments_valid;
+
+   typedef uint8_t _initialized_type;
+  _initialized_type initialized;
+
+   typedef std::vector<uint8_t, typename ContainerAllocator::template rebind<uint8_t>::other >  _OOB_value_type;
+  _OOB_value_type OOB_value;
+
+   typedef std::vector<uint8_t, typename ContainerAllocator::template rebind<uint8_t>::other >  _data_type;
+  _data_type data;
+
+
+
+
+  typedef boost::shared_ptr< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> const> ConstPtr;
+
+}; // struct TaggedObjectCollisionMap_
+
+typedef ::sdf_tools::TaggedObjectCollisionMap_<std::allocator<void> > TaggedObjectCollisionMap;
+
+typedef boost::shared_ptr< ::sdf_tools::TaggedObjectCollisionMap > TaggedObjectCollisionMapPtr;
+typedef boost::shared_ptr< ::sdf_tools::TaggedObjectCollisionMap const> TaggedObjectCollisionMapConstPtr;
+
+// constants requiring out of line definition
+
+
+
+template<typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> & v)
+{
+ros::message_operations::Printer< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >::stream(s, "", v);
+return s;
+}
+
+} // namespace sdf_tools
+
+namespace ros
+{
+namespace message_traits
+{
+
+
+
+// BOOLTRAITS {'IsFixedSize': False, 'IsMessage': True, 'HasHeader': True}
+// {'std_msgs': ['/opt/ros/indigo/share/std_msgs/cmake/../msg'], 'geometry_msgs': ['/opt/ros/indigo/share/geometry_msgs/cmake/../msg'], 'sdf_tools': ['/home/zby/workspaces/temp_ws/src/sdf_tools/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__', '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__', '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name', 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+
+
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> const>
+  : FalseType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct IsMessage< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
+  : TrueType
+  { };
+
+template <class ContainerAllocator>
+struct HasHeader< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> const>
+  : TrueType
+  { };
+
+
+template<class ContainerAllocator>
+struct MD5Sum< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "320371317f699b0048968a467deb0a13";
+  }
+
+  static const char* value(const ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator>&) { return value(); }
+  static const uint64_t static_value1 = 0x320371317f699b00ULL;
+  static const uint64_t static_value2 = 0x48968a467deb0a13ULL;
+};
+
+template<class ContainerAllocator>
+struct DataType< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "sdf_tools/TaggedObjectCollisionMap";
+  }
+
+  static const char* value(const ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator>&) { return value(); }
+};
+
+template<class ContainerAllocator>
+struct Definition< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "std_msgs/Header header\n\
+geometry_msgs/Transform origin_transform\n\
+geometry_msgs/Vector3 dimensions\n\
+float64 cell_size\n\
+uint32 number_of_components\n\
+bool components_valid\n\
+bool convex_segments_valid\n\
+bool initialized\n\
+uint8[] OOB_value\n\
+uint8[] data\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/Header\n\
+# Standard metadata for higher-level stamped data types.\n\
+# This is generally used to communicate timestamped data \n\
+# in a particular coordinate frame.\n\
+# \n\
+# sequence ID: consecutively increasing ID \n\
+uint32 seq\n\
+#Two-integer timestamp that is expressed as:\n\
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
+# time-handling sugar is provided by the client library\n\
+time stamp\n\
+#Frame this data is associated with\n\
+# 0: no frame\n\
+# 1: global frame\n\
+string frame_id\n\
+\n\
+================================================================================\n\
+MSG: geometry_msgs/Transform\n\
+# This represents the transform between two coordinate frames in free space.\n\
+\n\
+Vector3 translation\n\
+Quaternion rotation\n\
+\n\
+================================================================================\n\
+MSG: geometry_msgs/Vector3\n\
+# This represents a vector in free space. \n\
+# It is only meant to represent a direction. Therefore, it does not\n\
+# make sense to apply a translation to it (e.g., when applying a \n\
+# generic rigid transformation to a Vector3, tf2 will only apply the\n\
+# rotation). If you want your data to be translatable too, use the\n\
+# geometry_msgs/Point message instead.\n\
+\n\
+float64 x\n\
+float64 y\n\
+float64 z\n\
+================================================================================\n\
+MSG: geometry_msgs/Quaternion\n\
+# This represents an orientation in free space in quaternion form.\n\
+\n\
+float64 x\n\
+float64 y\n\
+float64 z\n\
+float64 w\n\
+";
+  }
+
+  static const char* value(const ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator>&) { return value(); }
+};
+
+} // namespace message_traits
+} // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+
+  template<class ContainerAllocator> struct Serializer< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
+  {
+    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
+    {
+      stream.next(m.header);
+      stream.next(m.origin_transform);
+      stream.next(m.dimensions);
+      stream.next(m.cell_size);
+      stream.next(m.number_of_components);
+      stream.next(m.components_valid);
+      stream.next(m.convex_segments_valid);
+      stream.next(m.initialized);
+      stream.next(m.OOB_value);
+      stream.next(m.data);
+    }
+
+    ROS_DECLARE_ALLINONE_SERIALIZER
+  }; // struct TaggedObjectCollisionMap_
+
+} // namespace serialization
+} // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+
+template<class ContainerAllocator>
+struct Printer< ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator> >
+{
+  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sdf_tools::TaggedObjectCollisionMap_<ContainerAllocator>& v)
+  {
+    s << indent << "header: ";
+    s << std::endl;
+    Printer< ::std_msgs::Header_<ContainerAllocator> >::stream(s, indent + "  ", v.header);
+    s << indent << "origin_transform: ";
+    s << std::endl;
+    Printer< ::geometry_msgs::Transform_<ContainerAllocator> >::stream(s, indent + "  ", v.origin_transform);
+    s << indent << "dimensions: ";
+    s << std::endl;
+    Printer< ::geometry_msgs::Vector3_<ContainerAllocator> >::stream(s, indent + "  ", v.dimensions);
+    s << indent << "cell_size: ";
+    Printer<double>::stream(s, indent + "  ", v.cell_size);
+    s << indent << "number_of_components: ";
+    Printer<uint32_t>::stream(s, indent + "  ", v.number_of_components);
+    s << indent << "components_valid: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.components_valid);
+    s << indent << "convex_segments_valid: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.convex_segments_valid);
+    s << indent << "initialized: ";
+    Printer<uint8_t>::stream(s, indent + "  ", v.initialized);
+    s << indent << "OOB_value[]" << std::endl;
+    for (size_t i = 0; i < v.OOB_value.size(); ++i)
+    {
+      s << indent << "  OOB_value[" << i << "]: ";
+      Printer<uint8_t>::stream(s, indent + "  ", v.OOB_value[i]);
+    }
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer<uint8_t>::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+} // namespace message_operations
+} // namespace ros
+
+#endif // SDF_TOOLS_MESSAGE_TAGGEDOBJECTCOLLISIONMAP_H

flightlib/third_party/sdf_tools/include/sdf_tools/collision_map.hpp

@@ -0,0 +1,168 @@
+#include <stdlib.h>
+#include <vector>
+#include <string>
+#include <Eigen/Geometry>
+#include <visualization_msgs/Marker.h>
+#include <arc_utilities/arc_helpers.hpp>
+#include <arc_utilities/voxel_grid.hpp>
+#include <sdf_tools/sdf.hpp>
+#include <sdf_tools/CollisionMap.h>
+#include <pcl/common/common.h>
+
+#ifndef COLLISION_MAP_HPP
+#define COLLISION_MAP_HPP
+
+#define ENABLE_UNORDERED_MAP_SIZE_HINTS
+
+namespace sdf_tools
+{
+    struct COLLISION_CELL
+    {
+        float occupancy;
+        uint32_t component;
+
+        COLLISION_CELL(const float in_occupancy = 0.0, const uint32_t in_component = 0);
+    };
+
+    std::vector<uint8_t> CollisionCellToBinary(const COLLISION_CELL& value);
+
+    COLLISION_CELL CollisionCellFromBinary(const std::vector<uint8_t>& binary);
+
+    class CollisionMapGrid
+    {
+    protected:
+
+        static std_msgs::ColorRGBA GenerateComponentColor(const uint32_t component, const float alpha=1.0f);
+
+        bool IsSurfaceIndex(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
+
+        typedef struct
+        {
+            uint32_t location[3];
+            uint32_t closest_point[3];
+            double distance_square;
+            int32_t update_direction;
+        } bucket_cell;
+
+        typedef VoxelGrid::VoxelGrid<bucket_cell> DistanceField;
+
+        DistanceField BuildDistanceField(const std::vector<VoxelGrid::GRID_INDEX>& points) const;
+
+        std::vector<std::vector<std::vector<std::vector<int>>>> MakeNeighborhoods() const;
+
+        int GetDirectionNumber(const int dx, const int dy, const int dz) const;
+
+        double ComputeDistanceSquared(const int32_t x1, const int32_t y1, const int32_t z1, const int32_t x2, const int32_t y2, const int32_t z2) const;
+
+        VoxelGrid::VoxelGrid<COLLISION_CELL> collision_field_;
+        uint32_t number_of_components_;
+        std::string frame_;
+        bool initialized_;
+        bool components_valid_;
+
+        std::vector<uint8_t> PackBinaryRepresentation(std::vector<COLLISION_CELL>& raw);
+
+        std::vector<COLLISION_CELL> UnpackBinaryRepresentation(std::vector<uint8_t>& packed);
+
+        int64_t MarkConnectedComponent(int64_t x_index, int64_t y_index, int64_t z_index, uint32_t connected_component);
+
+    public:
+
+        CollisionMapGrid(const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const COLLISION_CELL& default_value, const COLLISION_CELL& OOB_value);
+
+        CollisionMapGrid(const Eigen::Isometry3d& origin_transform, const std::string& frame, const double resolution, const double x_size, double y_size, const double z_size, const COLLISION_CELL& default_value, const COLLISION_CELL& OOB_value);
+
+        CollisionMapGrid(const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const COLLISION_CELL& OOB_default_value);
+
+        CollisionMapGrid(const Eigen::Isometry3d& origin_transform, const std::string& frame, const double resolution, const double x_size, double y_size, const double z_size, const COLLISION_CELL& OOB_default_value);
+
+        CollisionMapGrid();
+
+        bool IsInitialized() const;
+
+        bool AreComponentsValid() const;
+
+        std::pair<COLLISION_CELL, bool> Get3d(const Eigen::Vector3d& location) const;
+
+        std::pair<COLLISION_CELL, bool> Get4d(const Eigen::Vector4d& location) const;
+
+        std::pair<COLLISION_CELL, bool> Get(const double x, const double y, const double z) const;
+
+        std::pair<COLLISION_CELL, bool> Get(const VoxelGrid::GRID_INDEX& index) const;
+
+        std::pair<COLLISION_CELL, bool> Get(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
+
+        bool Set(const double x, const double y, const double z, COLLISION_CELL value);
+
+        bool Set3d(const Eigen::Vector3d& location, COLLISION_CELL value);
+
+        bool Set4d(const Eigen::Vector4d& location, COLLISION_CELL value);
+
+        bool Set(const int64_t x_index, const int64_t y_index, const int64_t z_index, COLLISION_CELL value);
+
+        bool Set(const VoxelGrid::GRID_INDEX& index, COLLISION_CELL value);
+
+        double GetXSize() const;
+
+        double GetYSize() const;
+
+        double GetZSize() const;
+
+        double GetResolution() const;
+
+        COLLISION_CELL GetDefaultValue() const;
+
+        COLLISION_CELL GetOOBValue() const;
+
+        int64_t GetNumXCells() const;
+
+        int64_t GetNumYCells() const;
+
+        int64_t GetNumZCells() const;
+
+        const Eigen::Isometry3d& GetOriginTransform() const;
+
+        const Eigen::Isometry3d& GetInverseOriginTransform() const;
+
+        std::string GetFrame() const;
+
+        std::pair<uint32_t, bool> GetNumConnectedComponents() const;
+
+        std::vector<int64_t> LocationToGridIndex3d(const Eigen::Vector3d& location) const;
+
+        std::vector<int64_t> LocationToGridIndex4d(const Eigen::Vector4d& location) const;
+
+        std::vector<int64_t> LocationToGridIndex(double x, double y, double z) const;
+
+        std::vector<double> GridIndexToLocation(int64_t x_index, int64_t y_index, int64_t z_index) const;
+
+        bool SaveToFile(const std::string& filepath);
+
+        bool LoadFromFile(const std::string &filepath);
+
+        sdf_tools::CollisionMap GetMessageRepresentation();
+
+        bool LoadFromMessageRepresentation(sdf_tools::CollisionMap& message);
+
+        uint32_t UpdateConnectedComponents();
+
+        std::map<uint32_t, std::pair<int32_t, int32_t>> ComputeComponentTopology(bool ignore_empty_components, bool recompute_connected_components, bool verbose);
+
+        std::map<uint32_t, std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>> ExtractComponentSurfaces(const bool ignore_empty_components) const;
+
+        std::pair<int32_t, int32_t> ComputeHolesInSurface(const uint32_t component, const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface, const bool verbose) const;
+
+        int32_t ComputeConnectivityOfSurfaceVertices(const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface_vertex_connectivity) const;
+
+        std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> ExtractSignedDistanceField(const float oob_value) const;
+
+        visualization_msgs::Marker ExportForDisplay(const std_msgs::ColorRGBA& collision_color, const std_msgs::ColorRGBA& free_color, const std_msgs::ColorRGBA& unknown_color) const;
+        
+        void DisplayPCL(pcl::PointCloud<pcl::PointXYZ> &occ_cloud);
+        void DisplayLocalPCL(pcl::PointCloud<pcl::PointXYZ> &occ_cloud, Eigen::Vector3d pose);
+        
+        visualization_msgs::Marker ExportConnectedComponentsForDisplay(bool color_unknown_components) const;
+    };
+}
+
+#endif // COLLISION_MAP_HPP

flightlib/third_party/sdf_tools/include/sdf_tools/dynamic_spatial_hashed_collision_map.hpp

@@ -0,0 +1,55 @@
+#include <stdlib.h>
+#include <vector>
+#include <string>
+#include <Eigen/Geometry>
+#include <visualization_msgs/Marker.h>
+#include <arc_utilities/voxel_grid.hpp>
+#include <arc_utilities/dynamic_spatial_hashed_voxel_grid.hpp>
+#include <sdf_tools/collision_map.hpp>
+
+#ifndef DYNAMIC_SPATIAL_HASHED_COLLISION_MAP_HPP
+#define DYNAMIC_SPATIAL_HASHED_COLLISION_MAP_HPP
+
+namespace sdf_tools
+{
+    class DynamicSpatialHashedCollisionMapGrid
+    {
+    protected:
+
+        VoxelGrid::DynamicSpatialHashedVoxelGrid<COLLISION_CELL> collision_field_;
+        uint32_t number_of_components_;
+        std::string frame_;
+        bool initialized_;
+        bool components_valid_;
+
+    public:
+
+        DynamicSpatialHashedCollisionMapGrid(std::string frame, double resolution, int64_t chunk_x_size, int64_t chunk_y_size, int64_t chunk_z_size, COLLISION_CELL OOB_value);
+
+        DynamicSpatialHashedCollisionMapGrid(Eigen::Isometry3d origin_transform, std::string frame, double resolution, int64_t chunk_x_size, int64_t chunk_y_size, int64_t chunk_z_size, COLLISION_CELL OOB_value);
+
+        DynamicSpatialHashedCollisionMapGrid();
+
+        bool IsInitialized() const;
+
+        bool AreComponentsValid() const;
+
+        std::pair<COLLISION_CELL, VoxelGrid::FOUND_STATUS> Get(const double x, const double y, const double z) const;
+
+        std::pair<COLLISION_CELL, VoxelGrid::FOUND_STATUS> Get(const Eigen::Vector3d& location) const;
+
+        VoxelGrid::SET_STATUS SetCell(const double x, const double y, const double z, COLLISION_CELL value);
+
+        VoxelGrid::SET_STATUS SetCell(const Eigen::Vector3d& location, COLLISION_CELL value);
+
+        VoxelGrid::SET_STATUS SetChunk(const double x, const double y, const double z, COLLISION_CELL value);
+
+        VoxelGrid::SET_STATUS SetChunk(const Eigen::Vector3d& location, COLLISION_CELL value);
+
+        Eigen::Isometry3d GetOriginTransform() const;
+
+        std::vector<visualization_msgs::Marker> ExportForDisplay(const std_msgs::ColorRGBA& collision_color, const std_msgs::ColorRGBA& free_color, const std_msgs::ColorRGBA& unknown_color) const;
+    };
+}
+
+#endif // DYNAMIC_SPATIAL_HASHED_COLLISION_MAP_HPP

flightlib/third_party/sdf_tools/include/sdf_tools/sdf.hpp

@@ -0,0 +1,224 @@
+#ifndef SDF_HPP
+#define SDF_HPP
+
+#include <stdlib.h>
+#include <vector>
+#include <string>
+#include <Eigen/Geometry>
+#include <visualization_msgs/Marker.h>
+#include <arc_utilities/eigen_helpers.hpp>
+#include <arc_utilities/voxel_grid.hpp>
+#include <sdf_tools/SDF.h>
+#include <pcl/common/common.h>
+
+namespace sdf_tools
+{
+    std::vector<uint8_t> FloatToBinary(float value);
+
+    float FloatFromBinary(std::vector<uint8_t>& binary);
+
+    class SignedDistanceField
+    {
+    protected:
+
+        VoxelGrid::VoxelGrid<float> distance_field_;
+        std::string frame_;
+        bool initialized_;
+        bool locked_;
+
+        std::vector<uint8_t> GetInternalBinaryRepresentation(const std::vector<float> &field_data);
+
+        std::vector<float> UnpackFieldFromBinaryRepresentation(const std::vector<uint8_t>& binary);
+
+        /*
+         * You *MUST* provide valid indices to this function, hence why it is protected (there are safe wrappers available - use them!)
+         */
+        void FollowGradientsToLocalMaximaUnsafe(VoxelGrid::VoxelGrid<Eigen::Vector3d>& watershed_map, const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
+
+    public:
+
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        SignedDistanceField(std::string frame, double resolution, double x_size, double y_size, double z_size, float OOB_value);
+
+        SignedDistanceField(Eigen::Isometry3d origin_transform, std::string frame, double resolution, double x_size, double y_size, double z_size, float OOB_value);
+
+        SignedDistanceField();
+
+        bool IsInitialized() const;
+
+        bool IsLocked() const;
+
+        void Lock();
+
+        void Unlock();
+
+        float Get(const double x, const double y, const double z) const;
+
+        float Get3d(const Eigen::Vector3d& location) const;
+
+        float Get4d(const Eigen::Vector4d& location) const;
+
+        float Get(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
+
+        std::pair<float, bool> GetSafe(const double x, const double y, const double z) const;
+
+        std::pair<float, bool> GetSafe3d(const Eigen::Vector3d& location) const;
+
+        std::pair<float, bool> GetSafe4d(const Eigen::Vector4d& location) const;
+
+        std::pair<float, bool> GetSafe(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
+
+        /*
+         * Setter functions MUST be used carefully - If you arbitrarily change SDF values, it is not a proper SDF any more!
+         *
+         * Use of these functions can be prevented by calling SignedDistanceField::Lock() on the SDF, at which point these functions
+         * will fail with a warning printed to std_err.
+         */
+        bool Set(const double x, const double y, const double z, float value);
+
+        bool Set3d(const Eigen::Vector3d& location, float value);
+
+        bool Set4d(const Eigen::Vector4d& location, float value);
+
+        bool Set(const int64_t x_index, const int64_t y_index, const int64_t z_index, const float value);
+
+        bool Set(const VoxelGrid::GRID_INDEX& index, const float value);
+
+        bool CheckInBounds3d(const Eigen::Vector3d& location) const;
+
+        bool CheckInBounds4d(const Eigen::Vector4d& location) const;
+
+        bool CheckInBounds(const double x, const double y, const double z) const;
+
+        bool CheckInBounds(const VoxelGrid::GRID_INDEX& index) const;
+
+        bool CheckInBounds(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
+
+        double GetXSize() const;
+
+        double GetYSize() const;
+
+        double GetZSize() const;
+
+        double GetResolution() const;
+
+        float GetOOBValue() const;
+
+        int64_t GetNumXCells() const;
+
+        int64_t GetNumYCells() const;
+
+        int64_t GetNumZCells() const;
+
+    protected:
+
+        std::pair<Eigen::Vector3d, double> GetPrimaryComponentsVector(const Eigen::Vector3d& raw_vector) const;
+
+        double ComputeAxisMatch(const double axis_value, const double check_value) const;
+
+        Eigen::Vector3d GetBestMatchSurfaceVector(const Eigen::Vector3d& possible_surfaces_vector, const Eigen::Vector3d& center_to_location_vector) const;
+
+        /**
+         * @brief GetPrimaryEntrySurfaceVector Estimates the real distance of the provided point, comparing it with the cell center location and gradient vector
+         * @param boundary_direction_vector
+         * @param center_to_location_vector
+         * @return vector from center of voxel to primary entry surface, and magnitude of that vector
+         */
+        std::pair<Eigen::Vector3d, double> GetPrimaryEntrySurfaceVector(const Eigen::Vector3d& boundary_direction_vector, const Eigen::Vector3d& center_to_location_vector) const;
+
+        double EstimateDistanceInternal(const double x, const double y, const double z, const int64_t x_idx, const int64_t y_idx, const int64_t z_idx) const;
+
+    public:
+
+        std::pair<double, bool> EstimateDistance(const double x, const double y, const double z) const;
+
+        std::pair<double, bool> EstimateDistance3d(const Eigen::Vector3d& location) const;
+
+        std::pair<double, bool> EstimateDistance4d(const Eigen::Vector4d& location) const;
+
+        // Estimate the distance between the given point and the outer boundary of the SDF
+        std::pair<double, bool> DistanceToBoundary(const double x, const double y, const double z) const;
+
+        std::pair<double, bool> DistanceToBoundary3d(const Eigen::Vector3d& location) const;
+
+        std::pair<double, bool> DistanceToBoundary4d(const Eigen::Vector4d& location) const;
+
+        std::vector<double> GetGradient(const double x, const double y, const double z, const bool enable_edge_gradients = false) const;
+
+        std::vector<double> GetGradient3d(const Eigen::Vector3d& location, const bool enable_edge_gradients = false) const;
+
+        std::vector<double> GetGradient4d(const Eigen::Vector4d& location, const bool enable_edge_gradients = false) const;
+
+        std::vector<double> GetGradient(const VoxelGrid::GRID_INDEX& index, const bool enable_edge_gradients = false) const;
+
+        std::vector<double> GetGradient(const int64_t x_index, const int64_t y_index, const int64_t z_index, const bool enable_edge_gradients = false) const;
+
+        Eigen::Vector3d ProjectOutOfCollision(const double x, const double y, const double z, const double stepsize_multiplier = 1.0 / 8.0) const;
+
+        Eigen::Vector3d ProjectOutOfCollisionToMinimumDistance(const double x, const double y, const double z, const double minimum_distance, const double stepsize_multiplier = 1.0 / 8.0) const;
+
+        Eigen::Vector3d ProjectOutOfCollision3d(const Eigen::Vector3d& location, const double stepsize_multiplier = 1.0 / 8.0) const;
+
+        Eigen::Vector3d ProjectOutOfCollisionToMinimumDistance3d(const Eigen::Vector3d& location, const double minimum_distance, const double stepsize_multiplier = 1.0 / 8.0) const;
+
+        Eigen::Vector4d ProjectOutOfCollision4d(const Eigen::Vector4d& location, const double stepsize_multiplier = 1.0 / 8.0) const;
+
+        Eigen::Vector4d ProjectOutOfCollisionToMinimumDistance4d(const Eigen::Vector4d& location, const double minimum_distance, const double stepsize_multiplier = 1.0 / 8.0) const;
+
+        Eigen::Vector3d ProjectIntoValidVolume(const double x, const double y, const double z) const;
+
+        Eigen::Vector3d ProjectIntoValidVolumeToMinimumDistance(const double x, const double y, const double z, const double minimum_distance) const;
+
+        Eigen::Vector3d ProjectIntoValidVolume3d(const Eigen::Vector3d& location) const;
+
+        Eigen::Vector3d ProjectIntoValidVolumeToMinimumDistance3d(const Eigen::Vector3d& location, const double minimum_distance) const;
+
+        Eigen::Vector4d ProjectIntoValidVolume4d(const Eigen::Vector4d& location) const;
+
+        Eigen::Vector4d ProjectIntoValidVolumeToMinimumDistance4d(const Eigen::Vector4d& location, const double minimum_distance) const;
+
+        const Eigen::Isometry3d& GetOriginTransform() const;
+
+        const Eigen::Isometry3d& GetInverseOriginTransform() const;
+
+        std::string GetFrame() const;
+
+        std::vector<int64_t> LocationToGridIndex3d(const Eigen::Vector3d& location) const;
+
+        std::vector<int64_t> LocationToGridIndex4d(const Eigen::Vector4d& location) const;
+
+        std::vector<int64_t> LocationToGridIndex(const double x, const double y, const double z) const;
+
+        std::vector<double> GridIndexToLocation(const VoxelGrid::GRID_INDEX& index) const;
+
+        std::vector<double> GridIndexToLocation(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
+
+        bool SaveToFile(const std::string& filepath);
+
+        bool LoadFromFile(const std::string& filepath);
+
+        sdf_tools::SDF GetMessageRepresentation();
+
+        bool LoadFromMessageRepresentation(const sdf_tools::SDF& message);
+
+        visualization_msgs::Marker ExportForDisplay(const float alpha = 0.01f, float vis_level = FLT_MAX) const;
+
+        visualization_msgs::Marker ExportForDisplayCollisionOnly(const float alpha = 0.01f) const;
+
+        void DisplayPCL(pcl::PointCloud<pcl::PointXYZI> &sdf_cloud, float vis_level = 0.0);
+
+        visualization_msgs::Marker ExportForDebug(const float alpha = 0.5f) const;
+
+        /*
+         * The following function can be *VERY EXPENSIVE* to compute, since it performs gradient ascent across the SDF
+         */
+        VoxelGrid::VoxelGrid<Eigen::Vector3d> ComputeLocalMaximaMap() const;
+
+        bool GradientIsEffectiveFlat(const Eigen::Vector3d& gradient) const;
+
+        VoxelGrid::GRID_INDEX GetNextFromGradient(const VoxelGrid::GRID_INDEX& index, const Eigen::Vector3d& gradient) const;
+    };
+}
+
+#endif // SDF_HPP

flightlib/third_party/sdf_tools/include/sdf_tools/sdf_builder.hpp

@@ -0,0 +1,95 @@
+#include <stdlib.h>
+#include <vector>
+#include <string>
+#include <Eigen/Geometry>
+#include <ros/ros.h>
+#include <moveit_msgs/GetPlanningScene.h>
+#include <urdf_model/model.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include "sdf_tools/sdf.hpp"
+#include "sdf_tools/SDF.h"
+
+#ifndef SDF_BUILDER_HPP
+#define SDF_BUILDER_HPP
+
+namespace sdf_tools
+{
+    static const uint8_t USE_CACHED = 0x00;
+    static const uint8_t USE_ONLY_OCTOMAP = 0x01;
+    static const uint8_t USE_ONLY_COLLISION_OBJECTS = 0x02;
+    static const uint8_t USE_FULL_PLANNING_SCENE = 0x03;
+
+    typedef struct
+    {
+        uint32_t location[3];
+        uint32_t closest_point[3];
+        double distance_square;
+        int32_t update_direction;
+    } bucket_cell;
+
+    typedef VoxelGrid::VoxelGrid<bucket_cell> DistanceField;
+
+    double ComputeDistanceSquared(int32_t x1, int32_t y1, int32_t z1, int32_t x2, int32_t y2, int32_t z2);
+
+    class SDF_Builder
+    {
+    protected:
+
+        bool initialized_;
+        bool has_cached_sdf_;
+        bool has_cached_collmap_;
+        bool has_planning_scene_;
+        Eigen::Isometry3d origin_transform_;
+        std::string frame_;
+        double x_size_;
+        double y_size_;
+        double z_size_;
+        double resolution_;
+        float OOB_value_;
+        SignedDistanceField cached_sdf_;
+        VoxelGrid::VoxelGrid<uint8_t> cached_collmap_;
+        std::shared_ptr<planning_scene::PlanningScene> planning_scene_ptr_;
+        ros::NodeHandle nh_;
+        ros::ServiceClient planning_scene_client_;
+
+        SignedDistanceField UpdateSDFFromPlanningScene();
+
+        VoxelGrid::VoxelGrid<uint8_t> UpdateCollisionMapFromPlanningScene();
+
+        bool BuildInternalPlanningScene();
+
+        DistanceField BuildDistanceField(std::vector<Eigen::Vector3i>& points);
+
+        std::vector<std::vector<std::vector<std::vector<int>>>> MakeNeighborhoods();
+
+        static int GetDirectionNumber(int dx, int dy, int dz);
+
+        std::string GenerateSDFComputeBotURDFString();
+
+        std::string GenerateSDFComputeBotSRDFString();
+
+    public:
+
+        SDF_Builder(ros::NodeHandle& nh, Eigen::Isometry3d origin_transform, std::string frame, double x_size, double y_size, double z_size, double resolution, float OOB_value, std::string planning_scene_service);
+
+        SDF_Builder(ros::NodeHandle& nh, std::string frame, double x_size, double y_size, double z_size, double resolution, float OOB_value, std::string planning_scene_service);
+
+        SDF_Builder();
+
+        void UpdatePlanningSceneFromMessage(moveit_msgs::PlanningScene& planning_scene);
+
+        SignedDistanceField UpdateSDF(uint8_t update_mode);
+
+        SignedDistanceField GetCachedSDF();
+
+        VoxelGrid::VoxelGrid<uint8_t> UpdateCollisionMap(uint8_t update_mode);
+
+        VoxelGrid::VoxelGrid<uint8_t> GetCachedCollisionMap();
+
+    };
+
+
+}
+
+#endif // SDF_BUILDER_HPP

flightlib/third_party/sdf_tools/include/sdf_tools/tagged_object_collision_map.hpp

@@ -0,0 +1,270 @@
+#include <stdlib.h>
+#include <vector>
+#include <string>
+#include <Eigen/Geometry>
+#include <Eigen/Sparse>
+#include <visualization_msgs/Marker.h>
+#include <arc_utilities/voxel_grid.hpp>
+#include <arc_utilities/arc_helpers.hpp>
+#include <arc_utilities/eigen_helpers.hpp>
+#include <sdf_tools/sdf.hpp>
+#include <sdf_tools/TaggedObjectCollisionMap.h>
+
+#ifndef TAGGED_OBJECT_COLLISION_MAP_HPP
+#define TAGGED_OBJECT_COLLISION_MAP_HPP
+
+#define ENABLE_UNORDERED_MAP_SIZE_HINTS
+
+namespace sdf_tools
+{
+    struct TAGGED_OBJECT_COLLISION_CELL
+    {
+        float occupancy;
+        uint32_t component;
+        uint32_t object_id;
+        uint32_t convex_segment;
+
+        TAGGED_OBJECT_COLLISION_CELL();
+
+        TAGGED_OBJECT_COLLISION_CELL(const float in_occupancy, const uint32_t in_object_id);
+
+        TAGGED_OBJECT_COLLISION_CELL(const float in_occupancy, const uint32_t in_object_id, const uint32_t in_component, const uint32_t in_convex_segment);
+
+        bool SharesConvexSegment(const TAGGED_OBJECT_COLLISION_CELL& other) const;
+
+        std::vector<uint32_t> GetListOfConvexSegments() const;
+
+        bool IsPartOfConvexSegment(const uint32_t segment) const;
+
+        void AddToConvexSegment(const uint32_t segment);
+
+        void RemoveFromConvexSegment(const uint32_t segment);
+    };
+
+    std::vector<uint8_t> TaggedObjectCollisionCellToBinary(const TAGGED_OBJECT_COLLISION_CELL& value);
+
+    TAGGED_OBJECT_COLLISION_CELL TaggedObjectCollisionCellFromBinary(const std::vector<uint8_t>& binary);
+
+    class TaggedObjectCollisionMapGrid
+    {
+    protected:
+
+        static std_msgs::ColorRGBA GenerateComponentColor(const uint32_t component, const float alpha=1.0f);
+
+        bool IsSurfaceIndex(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
+
+        struct bucket_cell
+        {
+            uint32_t location[3];
+            uint32_t closest_point[3];
+            double distance_square;
+            int32_t update_direction;
+        };
+
+        typedef VoxelGrid::VoxelGrid<bucket_cell> DistanceField;
+
+        DistanceField BuildDistanceField(const std::vector<VoxelGrid::GRID_INDEX>& points) const;
+
+        std::vector<std::vector<std::vector<std::vector<int>>>> MakeNeighborhoods() const;
+
+        int GetDirectionNumber(const int dx, const int dy, const int dz) const;
+
+        double ComputeDistanceSquared(const int32_t x1, const int32_t y1, const int32_t z1, const int32_t x2, const int32_t y2, const int32_t z2) const;
+
+        VoxelGrid::VoxelGrid<TAGGED_OBJECT_COLLISION_CELL> collision_field_;
+        uint32_t number_of_components_;
+        std::string frame_;
+        bool initialized_;
+        bool components_valid_;
+        bool convex_segments_valid_;
+
+        std::vector<uint8_t> PackBinaryRepresentation(const std::vector<TAGGED_OBJECT_COLLISION_CELL>& raw) const;
+
+        std::vector<TAGGED_OBJECT_COLLISION_CELL> UnpackBinaryRepresentation(const std::vector<uint8_t>& packed) const;
+
+        int64_t MarkConnectedComponent(const int64_t x_index, const int64_t y_index, const int64_t z_index, const uint32_t connected_component);
+
+        std::vector<VoxelGrid::GRID_INDEX> CheckIfConvex(const VoxelGrid::GRID_INDEX& candidate_index, std::unordered_map<VoxelGrid::GRID_INDEX, int8_t>& explored_indices, const VoxelGrid::VoxelGrid<std::vector<uint32_t>>& region_grid, const uint32_t current_convex_region) const;
+
+    public:
+
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        TaggedObjectCollisionMapGrid(const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const TAGGED_OBJECT_COLLISION_CELL& default_value, const TAGGED_OBJECT_COLLISION_CELL& OOB_value);
+
+        TaggedObjectCollisionMapGrid(const Eigen::Isometry3d& origin_transform, const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const TAGGED_OBJECT_COLLISION_CELL& default_value, const TAGGED_OBJECT_COLLISION_CELL& OOB_value);
+
+        TaggedObjectCollisionMapGrid(const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const TAGGED_OBJECT_COLLISION_CELL& OOB_value);
+
+        TaggedObjectCollisionMapGrid(const Eigen::Isometry3d& origin_transform, const std::string& frame, const double resolution, const double x_size, const double y_size, const double z_size, const TAGGED_OBJECT_COLLISION_CELL& OOB_value);
+
+        TaggedObjectCollisionMapGrid();
+
+        bool IsInitialized() const;
+
+        bool AreComponentsValid() const;
+
+        bool AreConvexSegmentsValid() const;
+
+        std::pair<bool, bool> CheckIfCandidateCorner3d(const Eigen::Vector3d& location) const;
+
+        std::pair<bool, bool> CheckIfCandidateCorner4d(const Eigen::Vector4d& location) const;
+
+        std::pair<bool, bool> CheckIfCandidateCorner(const double x, const double y, const double z) const;
+
+        std::pair<bool, bool> CheckIfCandidateCorner(const VoxelGrid::GRID_INDEX& index) const;
+
+        std::pair<bool, bool> CheckIfCandidateCorner(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
+
+        std::pair<const TAGGED_OBJECT_COLLISION_CELL&, bool> GetImmutable3d(const Eigen::Vector3d& location) const;
+
+        std::pair<const TAGGED_OBJECT_COLLISION_CELL&, bool> GetImmutable4d(const Eigen::Vector4d& location) const;
+
+        std::pair<const TAGGED_OBJECT_COLLISION_CELL&, bool> GetImmutable(const double x, const double y, const double z) const;
+
+        std::pair<const TAGGED_OBJECT_COLLISION_CELL&, bool> GetImmutable(const VoxelGrid::GRID_INDEX& index) const;
+
+        std::pair<const TAGGED_OBJECT_COLLISION_CELL&, bool> GetImmutable(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
+
+        std::pair<TAGGED_OBJECT_COLLISION_CELL&, bool> GetMutable3d(const Eigen::Vector3d& location);
+
+        std::pair<TAGGED_OBJECT_COLLISION_CELL&, bool> GetMutable4d(const Eigen::Vector4d& location);
+
+        std::pair<TAGGED_OBJECT_COLLISION_CELL&, bool> GetMutable(const double x, const double y, const double z);
+
+        std::pair<TAGGED_OBJECT_COLLISION_CELL&, bool> GetMutable(const VoxelGrid::GRID_INDEX& index);
+
+        std::pair<TAGGED_OBJECT_COLLISION_CELL&, bool> GetMutable(const int64_t x_index, const int64_t y_index, const int64_t z_index);
+
+        bool Set(const double x, const double y, const double z, const TAGGED_OBJECT_COLLISION_CELL& value);
+
+        bool Set3d(const Eigen::Vector3d& location, const TAGGED_OBJECT_COLLISION_CELL& value);
+
+        bool Set4d(const Eigen::Vector4d& location, const TAGGED_OBJECT_COLLISION_CELL& value);
+
+        bool Set(const int64_t x_index, const int64_t y_index, const int64_t z_index, const TAGGED_OBJECT_COLLISION_CELL& value);
+
+        bool Set(const VoxelGrid::GRID_INDEX& index, const TAGGED_OBJECT_COLLISION_CELL& value);
+
+        bool Set(const double x, const double y, const double z, TAGGED_OBJECT_COLLISION_CELL&& value);
+
+        bool Set3d(const Eigen::Vector3d& location, TAGGED_OBJECT_COLLISION_CELL&& value);
+
+        bool Set4d(const Eigen::Vector4d& location, TAGGED_OBJECT_COLLISION_CELL&& value);
+
+        bool Set(const int64_t x_index, const int64_t y_index, const int64_t z_index, TAGGED_OBJECT_COLLISION_CELL&& value);
+
+        bool Set(const VoxelGrid::GRID_INDEX& index, TAGGED_OBJECT_COLLISION_CELL&& value);
+
+        double GetXSize() const;
+
+        double GetYSize() const;
+
+        double GetZSize() const;
+
+        double GetResolution() const;
+
+        TAGGED_OBJECT_COLLISION_CELL GetDefaultValue() const;
+
+        TAGGED_OBJECT_COLLISION_CELL GetOOBValue() const;
+
+        int64_t GetNumXCells() const;
+
+        int64_t GetNumYCells() const;
+
+        int64_t GetNumZCells() const;
+
+        const Eigen::Isometry3d& GetOriginTransform() const;
+
+        const Eigen::Isometry3d& GetInverseOriginTransform() const;
+
+        std::string GetFrame() const;
+
+        std::pair<uint32_t, bool> GetNumConnectedComponents() const;
+
+        std::vector<int64_t> LocationToGridIndex3d(const Eigen::Vector3d& location) const;
+
+        std::vector<int64_t> LocationToGridIndex4d(const Eigen::Vector4d& location) const;
+
+        std::vector<int64_t> LocationToGridIndex(const double x, const double y, const double z) const;
+
+        std::vector<double> GridIndexToLocation(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;
+
+        bool SaveToFile(const std::string& filepath) const;
+
+        bool LoadFromFile(const std::string &filepath);
+
+        sdf_tools::TaggedObjectCollisionMap GetMessageRepresentation() const;
+
+        bool LoadFromMessageRepresentation(const sdf_tools::TaggedObjectCollisionMap& message);
+
+        TaggedObjectCollisionMapGrid Resample(const double new_resolution) const;
+
+        uint32_t UpdateConnectedComponents();
+
+        std::map<uint32_t, std::pair<int32_t, int32_t>> ComputeComponentTopology(const bool ignore_empty_components, const bool recompute_connected_components, const bool verbose);
+
+        std::map<uint32_t, std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>> ExtractComponentSurfaces(const bool ignore_empty_components) const;
+
+        /* Extracts the active indices from a surface map as a vector, which is useful in contexts where a 1-dimensional index into the surface is needed
+         */
+        std::vector<VoxelGrid::GRID_INDEX> ExtractStaticSurface(const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& raw_surface) const;
+
+        std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t> ConvertToDynamicSurface(const std::vector<VoxelGrid::GRID_INDEX>& static_surface) const;
+
+        std::unordered_map<VoxelGrid::GRID_INDEX, size_t> BuildSurfaceIndexMap(const std::vector<VoxelGrid::GRID_INDEX>& static_surface) const;
+
+        std::pair<int32_t, int32_t> ComputeHolesInSurface(const uint32_t component, const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface, const bool verbose) const;
+
+        int32_t ComputeConnectivityOfSurfaceVertices(const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface_vertex_connectivity) const;
+
+        std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> ExtractSignedDistanceField(const float oob_value, const std::vector<uint32_t>& objects_to_use) const;
+
+        VoxelGrid::VoxelGrid<std::vector<uint32_t>> ComputeConvexRegions(const double max_check_radius) const;
+
+        void GrowConvexRegion(const VoxelGrid::GRID_INDEX& start_index, VoxelGrid::VoxelGrid<std::vector<uint32_t>>& region_grid, const double max_check_radius, const uint32_t current_convex_region) const;
+
+        EigenHelpers::VectorVector3d GenerateRayPrimitiveVectors(const uint32_t number_of_rays, const double cone_angle) const;
+
+        std::pair<std::vector<size_t>, std::vector<size_t>> CastSingleRay(const std::unordered_map<VoxelGrid::GRID_INDEX, size_t>& surface_index_map, const VoxelGrid::GRID_INDEX& current_surface_index, const Eigen::Vector3d& ray_unit_vector) const;
+
+        std::pair<std::vector<size_t>, std::vector<size_t>> PerformRayCasting(const sdf_tools::SignedDistanceField& sdf, const std::unordered_map<VoxelGrid::GRID_INDEX, size_t>& surface_index_map, const VoxelGrid::GRID_INDEX& current_surface_index, const EigenHelpers::VectorVector3d& ray_primitive_vectors) const;
+
+        //std::pair<Eigen::MatrixXd, std::pair<Eigen::SparseMatrix<double>, Eigen::SparseMatrix<double>>> ComputeSparseLineOfSight(const std::vector<VoxelGrid::GRID_INDEX>& static_surface, const uint32_t number_of_rays, const double cone_angle) const
+        Eigen::MatrixXd ComputeSparseLineOfSight(const std::vector<VoxelGrid::GRID_INDEX>& static_surface, const uint32_t number_of_rays, const double cone_angle) const;
+
+        std::pair<Eigen::VectorXd, Eigen::MatrixXd> ExtractKLargestEigenvaluesAndEigenvectors(const Eigen::EigenSolver<Eigen::MatrixXd>::EigenvalueType& raw_eigenvalues, const Eigen::EigenSolver<Eigen::MatrixXd>::EigenvectorsType& raw_eigenvectors, const uint32_t num_values) const;
+
+        std::vector<uint32_t> PerformKMeansSpectralClustering(const Eigen::EigenSolver<Eigen::MatrixXd>::EigenvalueType& raw_eigenvalues, const Eigen::EigenSolver<Eigen::MatrixXd>::EigenvectorsType& raw_eigenvectors, const uint32_t num_clusters) const;
+
+        double ComputeConvexityMetric(const Eigen::MatrixXd& los_matrix, const std::vector<uint32_t>& cluster_labels) const;
+
+        std::vector<std::vector<size_t>> ClusterSurfaceFromLOSMatrix(const Eigen::MatrixXd& los_matrix, const uint32_t max_num_clusters) const;
+
+        std::vector<std::vector<VoxelGrid::GRID_INDEX>> ComputeWeaklyConvexSurfaceSegments(const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface, const uint32_t max_num_clusters) const;
+
+        std::map<uint32_t, uint32_t> UpdateConvexSegments();
+
+        std::map<uint32_t, sdf_tools::SignedDistanceField> MakeObjectSDFs(const std::vector<uint32_t>& object_ids) const;
+
+        std::map<uint32_t, sdf_tools::SignedDistanceField> MakeObjectSDFs() const;
+
+        visualization_msgs::Marker ExportForDisplay(const float alpha, const std::vector<uint32_t>& objects_to_draw=std::vector<uint32_t>()) const;
+
+        visualization_msgs::Marker ExportForDisplay(const std::map<uint32_t, std_msgs::ColorRGBA>& object_color_map=std::map<uint32_t, std_msgs::ColorRGBA>()) const;
+
+        visualization_msgs::Marker ExportContourOnlyForDisplay(const float alpha, const std::vector<uint32_t>& objects_to_draw=std::vector<uint32_t>()) const;
+
+        visualization_msgs::Marker ExportContourOnlyForDisplay(const std::map<uint32_t, std_msgs::ColorRGBA>& object_color_map=std::map<uint32_t, std_msgs::ColorRGBA>()) const;
+
+        visualization_msgs::Marker ExportForDisplayOccupancyOnly(const std_msgs::ColorRGBA& collision_color, const std_msgs::ColorRGBA& free_color, const std_msgs::ColorRGBA& unknown_color) const;
+
+        visualization_msgs::Marker ExportConnectedComponentsForDisplay(bool color_unknown_components) const;
+
+        visualization_msgs::Marker ExportConvexSegmentForDisplay(const uint32_t object_id, const uint32_t convex_segment) const;
+
+        visualization_msgs::Marker ExportSurfaceForDisplay(const std::unordered_map<VoxelGrid::GRID_INDEX, uint8_t>& surface, const std_msgs::ColorRGBA& surface_color) const;
+    };
+}
+
+#endif // TAGGED_OBJECT_COLLISION_MAP_HPP

flightlib/third_party/sdf_tools/msg/CollisionMap.msg

@@ -0,0 +1,10 @@
+std_msgs/Header header
+geometry_msgs/Transform origin_transform
+geometry_msgs/Vector3 dimensions
+float64 cell_size
+float32 OOB_occupancy_value
+uint32 OOB_component_value
+uint32 number_of_components
+bool components_valid
+bool initialized
+uint8[] data

flightlib/third_party/sdf_tools/msg/SDF.msg

@@ -0,0 +1,8 @@
+std_msgs/Header header
+geometry_msgs/Transform origin_transform
+geometry_msgs/Vector3 dimensions
+float64 sdf_cell_size
+float32 OOB_value
+bool initialized
+bool locked
+uint8[] data

flightlib/third_party/sdf_tools/msg/TaggedObjectCollisionMap.msg

@@ -0,0 +1,10 @@
+std_msgs/Header header
+geometry_msgs/Transform origin_transform
+geometry_msgs/Vector3 dimensions
+float64 cell_size
+uint32 number_of_components
+bool components_valid
+bool convex_segments_valid
+bool initialized
+uint8[] OOB_value
+uint8[] data

flightlib/third_party/sdf_tools/package.xml

@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<package>
+  <name>sdf_tools</name>
+  <version>0.0.1</version>
+  <description>Builds 2D signed distance fields from images, 3D signed distance fields from MoveIt PlanningScene/Octomap, provides a lightweight signed distance field library, message types for signed distance fields, and tools to compress signed distance fields for transport and storage.</description>
+  <maintainer email="calder.pg@gmail.com">Calder Phillips-Grafflin</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>image_transport</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>moveit_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>moveit_core</build_depend>
+  <build_depend>moveit_ros_planning</build_depend>
+  <build_depend>arc_utilities</build_depend>
+  <build_depend>message_generation</build_depend>
+
+  <run_depend>image_transport</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>moveit_msgs</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>cv_bridge</run_depend>
+  <run_depend>moveit_core</run_depend>
+  <run_depend>moveit_ros_planning</run_depend>
+  <run_depend>arc_utilities</run_depend>
+  <run_depend>message_runtime</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

flightlib/third_party/sdf_tools/src/image_2d_sdf_node.cpp

@@ -0,0 +1,327 @@
+#include <ros/ros.h>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv/cv.h>
+#include <sensor_msgs/image_encodings.h>
+
+#define snap(x) ((x)>=0?(int)((x)+0.5):(int)((x)-0.5))
+
+typedef struct sdf_cell {
+    double dx;
+    double dy;
+} sdf_cell_t;
+
+class ImageSDF
+{
+protected:
+
+    ros::NodeHandle nh_;
+    image_transport::ImageTransport it_;
+    image_transport::Subscriber binary_sub_;
+    image_transport::Publisher sdf_preview_pub_;
+    image_transport::Publisher sdf_raw_pub_;
+    std::vector< std::vector<double> > distance_field_;
+    double max_distance_;
+    double min_distance_;
+
+public:
+
+    ImageSDF(ros::NodeHandle &n, std::string binary_base_topic, std::string sdf_preview_topic, std::string sdf_raw_topic) : nh_(n), it_(n)
+    {
+        max_distance_ = 0.0;
+        min_distance_ = 0.0;
+        binary_sub_ = it_.subscribe(binary_base_topic, 1, &ImageSDF::camera_cb, this);
+        sdf_preview_pub_ = it_.advertise(sdf_preview_topic, 1, true);
+        sdf_raw_pub_ = it_.advertise(sdf_raw_topic, 1, true);
+        std::string transport_in = binary_sub_.getTransport();
+        ROS_INFO("Subscribed using %s for transport", transport_in.c_str());
+    }
+
+    void loop()
+    {
+        while (ros::ok())
+        {
+            ros::spinOnce();
+        }
+    }
+
+    void update_sdf(cv::Mat& image)
+    {
+        ROS_DEBUG("Making intermediate containers for SDF");
+        std::vector< std::vector<sdf_cell_t> > empty_cells;
+        std::vector< std::vector<sdf_cell_t> > filled_cells;
+        // Resize the fields
+        int width = image.cols;
+        int height = image.rows;
+        empty_cells.resize(height);
+        filled_cells.resize(height);
+        distance_field_.resize(height);
+        for (int i = 0; i < height; i++)
+        {
+            empty_cells[i].resize(width);
+            filled_cells[i].resize(width);
+            distance_field_[i].resize(width);
+        }
+        ROS_DEBUG("Marking filled/empty pixels for SDF");
+        // Go through the image and add filled/empty pixels to the corresponding fields
+        sdf_cell_t empty_cell;
+        empty_cell.dx = INFINITY;
+        empty_cell.dy = INFINITY;
+        sdf_cell_t filled_cell;
+        filled_cell.dx = 0.0;
+        filled_cell.dy = 0.0;
+        for (int i = 0; i < height; i++)
+        {
+            for (int j = 0; j < width; j++)
+            {
+                uint8_t pixel = image.at<uint8_t>(i,j);
+                if (pixel != 0)
+                {
+                    filled_cells[i][j] = filled_cell;
+                    empty_cells[i][j] = empty_cell;
+                }
+                else
+                {
+                    filled_cells[i][j] = empty_cell;
+                    empty_cells[i][j] = filled_cell;
+                }
+            }
+        }
+        ROS_DEBUG("Running 8SSEDT on intermediate containers");
+        // Run the 8SSEDT algorithm to compute the partial SDFs
+        update_partial_sdf(filled_cells);
+        update_partial_sdf(empty_cells);
+        ROS_DEBUG("Computing the final SDF");
+        // Combine the partial fields to form the SDF
+        max_distance_ = 0.0;
+        min_distance_ = 0.0;
+        for (int i = 0; i < height; i++)
+        {
+            for (int j = 0; j < width; j++)
+            {
+                double filled_distance = sqrt(pow(filled_cells[i][j].dx, 2) + pow(filled_cells[i][j].dy, 2));
+                double empty_distance = sqrt(pow(empty_cells[i][j].dx, 2) + pow(empty_cells[i][j].dy, 2));
+                double new_distance = filled_distance - empty_distance;
+                distance_field_[i][j] = new_distance;
+                if (new_distance > max_distance_)
+                {
+                    max_distance_ = new_distance;
+                }
+                if (new_distance < min_distance_)
+                {
+                    min_distance_ = new_distance;
+                }
+            }
+        }
+    }
+
+    bool update_partial_sdf(std::vector< std::vector<sdf_cell_t> >& partial_field)
+    {
+        if (partial_field.size() > 0)
+        {
+            // Pass 1.1.0
+            for (size_t y = 0; y < partial_field[0].size(); y++)
+            {
+                // Pass 1.1.1
+                for (size_t x = 0; x < partial_field.size(); x++)
+                {
+                    // Get value from grid
+                    sdf_cell_t cell = get(partial_field, x, y);
+                    // Do work
+                    compare(partial_field, cell, x, y, -1, 0);
+                    compare(partial_field, cell, x, y, 0, -1);
+                    compare(partial_field, cell, x, y, -1, -1);
+                    compare(partial_field, cell, x, y, 1, -1);
+                    // Store back into grid
+                    put(partial_field, cell, x, y);
+                }
+                // Pass 1.1.2
+                for (int x = (partial_field.size() - 1); x >= 0; x--)
+                {
+                    // Get value from grid
+                    sdf_cell_t cell = get(partial_field, x, y);
+                    // Do work
+                    compare(partial_field, cell, x, y, 1, 0);
+                    // Store back into grid
+                    put(partial_field, cell, x, y);
+                }
+            }
+            // Pass 1.2.0
+            for (int y = (partial_field[0].size() - 1); y >= 0; y--)
+            {
+                // Pass 1.1.1
+                for (int x = (partial_field.size() - 1); x >= 0; x--)
+                {
+                    // Get value from grid
+                    sdf_cell_t cell = get(partial_field, x, y);
+                    // Do work
+                    compare(partial_field, cell, x, y, 1, 0);
+                    compare(partial_field, cell, x, y, 0, 1);
+                    compare(partial_field, cell, x, y, -1, 1);
+                    compare(partial_field, cell, x, y, 1, 1);
+                    // Store back into grid
+                    put(partial_field, cell, x, y);
+                }
+                // Pass 1.1.2
+                for (size_t x = 0; x < partial_field.size(); x++)
+                {
+                    // Get value from grid
+                    sdf_cell_t cell = get(partial_field, x, y);
+                    // Do work
+                    compare(partial_field, cell, x, y, -1, 0);
+                    // Store back into grid
+                    put(partial_field, cell, x, y);
+                }
+            }
+            return true;
+        }
+        else
+        {
+            return false;
+        }
+    }
+
+    inline sdf_cell_t get(std::vector< std::vector<sdf_cell_t> >& partial_field, size_t x, size_t y)
+    {
+        if (x < partial_field.size())
+        {
+            if (y < partial_field[x].size())
+            {
+                return partial_field[x][y];
+            }
+        }
+        sdf_cell_t empty;
+        empty.dx = INFINITY;
+        empty.dy = INFINITY;
+        return empty;
+    }
+
+    inline void put(std::vector< std::vector<sdf_cell_t> >& partial_field, sdf_cell_t& cell, size_t x, size_t y)
+    {
+        if (x < partial_field.size())
+        {
+            if (y < partial_field[x].size())
+            {
+                partial_field[x][y] = cell;
+            }
+        }
+    }
+
+    inline double distance_squared(sdf_cell_t& cell)
+    {
+        return ((cell.dx * cell.dx) + (cell.dy * cell.dy));
+    }
+
+    inline void compare(std::vector< std::vector<sdf_cell_t> >& partial_field, sdf_cell_t& cell, int x, int y, int x_offset, int y_offset)
+    {
+        sdf_cell_t other = get(partial_field, x + x_offset, y + y_offset);
+        other.dx += x_offset;
+        other.dy += y_offset;
+        if (distance_squared(other) < distance_squared(cell))
+        {
+            cell = other;
+        }
+    }
+
+    void camera_cb(const sensor_msgs::ImageConstPtr& image)
+    {
+        ROS_DEBUG("Got new image to resize and SDF");
+        // Convert to OpenCV
+        cv_bridge::CvImagePtr cv_ptr;
+        try
+        {
+            cv_ptr = cv_bridge::toCvCopy(image);
+        }
+        catch (cv_bridge::Exception& e)
+        {
+            ROS_ERROR("cv_bridge exception: %s", e.what());
+            return;
+        }
+        // Make destination
+        cv::Mat binary; //(cv::Size(resized_width_, resized_height_), CV_8UC1);
+        binary = cv_ptr->image;
+        // Compute the SDF for the image
+        ROS_DEBUG("Attempting to compute SDF of image...");
+        update_sdf(binary);
+        ROS_DEBUG("...SDF compute finished");
+        // Publish the raw SDF
+        cv::Mat raw_sdf_image(cv::Size(binary.cols, binary.rows), CV_32FC2);
+        for (int i = 0; i < binary.rows; i++)
+        {
+            for (int j = 0; j < binary.cols; j++)
+            {
+                double current_distance = distance_field_[i][j];
+                if (current_distance >= 0.0)
+                {
+                    raw_sdf_image.at<cv::Vec2f>(i,j)[0] = fabs(current_distance);
+                    raw_sdf_image.at<cv::Vec2f>(i,j)[1] = 0.0;
+                }
+                else
+                {
+                    raw_sdf_image.at<cv::Vec2f>(i,j)[0] = 0.0;
+                    raw_sdf_image.at<cv::Vec2f>(i,j)[1] = fabs(current_distance);
+                }
+            }
+        }
+        // Convert back to ROS
+        sensor_msgs::Image sdf_raw_image;
+        cv_bridge::CvImage sdf_raw_converted(image->header, sensor_msgs::image_encodings::TYPE_32FC2, raw_sdf_image);
+        sdf_raw_converted.toImageMsg(sdf_raw_image);
+        // Republish
+        sdf_raw_pub_.publish(sdf_raw_image);
+        // Convert SDF to false-color image
+        cv::Mat false_color_sdf(cv::Size(binary.cols, binary.rows), CV_8UC3);
+        for (int i = 0; i < binary.rows; i++)
+        {
+            for (int j = 0; j < binary.cols; j++)
+            {
+                double current_distance = distance_field_[i][j];
+                uint8_t blue_channel = 0x00;
+                uint8_t red_channel = 0x00;
+                uint8_t green_channel = 0x00;
+                if (current_distance > 0.0)
+                {
+                    red_channel = (uint8_t)(64.0 + (64.0 * fabs(current_distance / max_distance_)));
+                }
+                else if (current_distance == 0.0 || current_distance == -0.0)
+                {
+                    green_channel = 0xff;
+                }
+                else
+                {
+                    blue_channel = (uint8_t)(64.0 + (64.0 * fabs(current_distance / min_distance_)));
+                }
+                false_color_sdf.at<cv::Vec3b>(i, j)[0] = blue_channel;
+                false_color_sdf.at<cv::Vec3b>(i, j)[1] = green_channel;
+                false_color_sdf.at<cv::Vec3b>(i, j)[2] = red_channel;
+            }
+        }
+        // Convert back to ROS
+        sensor_msgs::Image sdf_preview_image;
+        cv_bridge::CvImage sdf_preview_converted(image->header, sensor_msgs::image_encodings::BGR8, false_color_sdf);
+        sdf_preview_converted.toImageMsg(sdf_preview_image);
+        // Republish
+        sdf_preview_pub_.publish(sdf_preview_image);
+        ROS_DEBUG("Resize + SDF finished");
+    }
+};
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "image_sdf");
+    ROS_INFO("Starting SDF from image generator...");
+    ros::NodeHandle nh;
+    ros::NodeHandle nhp("~");
+    std::string binary_base_topic;
+    std::string sdf_preview_topic;
+    std::string sdf_raw_topic;
+    nhp.param(std::string("binary_base_topic"), binary_base_topic, std::string("camera/rgb/binary"));
+    nhp.param(std::string("sdf_preview_topic"), sdf_preview_topic, std::string("camera/rgb/sdf"));
+    nhp.param(std::string("sdf_raw_topic"), sdf_raw_topic, std::string("camera/rgb/sdf_raw"));
+    ImageSDF processor(nh, binary_base_topic, sdf_preview_topic, sdf_raw_topic);
+    ROS_INFO("...startup complete");
+    processor.loop();
+    return 0;
+}

flightlib/third_party/sdf_tools/src/sdf_generation_node.cpp

@@ -0,0 +1,107 @@
+#include <ros/ros.h>
+#include "sdf_tools/SDF.h"
+#include "sdf_tools/sdf_builder.hpp"
+#include <arc_utilities/eigen_helpers_conversions.hpp>
+#include <time.h>
+
+visualization_msgs::Marker ExportCollisionMapForDisplay(VoxelGrid::VoxelGrid<uint8_t>& collision_map, std::string frame, float alpha)
+{
+    // Assemble a visualization_markers::Marker representation of the SDF to display in RViz
+    visualization_msgs::Marker display_rep;
+    // Populate the header
+    display_rep.header.frame_id = frame;
+    // Populate the options
+    display_rep.ns = "collision_map_display";
+    display_rep.id = 1;
+    display_rep.type = visualization_msgs::Marker::CUBE_LIST;
+    display_rep.action = visualization_msgs::Marker::ADD;
+    display_rep.lifetime = ros::Duration(0.0);
+    display_rep.frame_locked = false;
+    const Eigen::Isometry3d base_transform = Eigen::Isometry3d::Identity();
+    display_rep.pose = EigenHelpersConversions::EigenIsometry3dToGeometryPose(base_transform);
+    display_rep.scale.x = collision_map.GetCellSizes()[0];
+    display_rep.scale.y = collision_map.GetCellSizes()[1];
+    display_rep.scale.z = collision_map.GetCellSizes()[2];
+    // Add all cells in collision
+    for (int64_t x_index = 0; x_index < collision_map.GetNumXCells(); x_index++)
+    {
+        for (int64_t y_index = 0; y_index < collision_map.GetNumYCells(); y_index++)
+        {
+            for (int64_t z_index = 0; z_index < collision_map.GetNumZCells(); z_index++)
+            {
+                // Check if the current cell is in collision
+                uint8_t status = collision_map.GetImmutable(x_index, y_index, z_index).first;
+                if (status == 1)
+                {
+                    // Convert cell indices into a real-world location
+                    std::vector<double> location = collision_map.GridIndexToLocation(x_index, y_index, z_index);
+                    geometry_msgs::Point new_point;
+                    new_point.x = location[0];
+                    new_point.y = location[1];
+                    new_point.z = location[2];
+                    display_rep.points.push_back(new_point);
+                    // Color it
+                    std_msgs::ColorRGBA new_color;
+                    new_color.a = alpha;
+                    new_color.b = 0.0;
+                    new_color.g = 0.0;
+                    new_color.r = 1.0;
+                    display_rep.colors.push_back(new_color);
+                }
+                else
+                {
+                    assert(status == 0);
+                }
+            }
+        }
+    }
+    return display_rep;
+}
+
+int main(int argc, char** argv)
+{
+    //test_voxel_grid();
+    ros::init(argc, argv, "planning_scene_SDF_generator");
+    ROS_INFO("Starting SDF from planning scene generator...");
+    ros::NodeHandle nh;
+    ros::NodeHandle nhp("~");
+    std::string frame;
+    double resolution;
+    double x_size;
+    double y_size;
+    double z_size;
+    nhp.param(std::string("sdf_origin_frame"), frame, std::string("base"));
+    nhp.param(std::string("resolution"), resolution, 0.05);
+    nhp.param(std::string("x_size"), x_size, 1.0);
+    nhp.param(std::string("y_size"), y_size, 2.0);
+    nhp.param(std::string("z_size"), z_size, 1.0);
+    Eigen::Translation3d origin_translation(0.1, -1.0, -0.3);
+    Eigen::Quaterniond origin_rotation;
+    origin_rotation.setIdentity();
+    Eigen::Isometry3d origin_transform = origin_translation * origin_rotation;
+    sdf_tools::SDF_Builder sdf_builder(nh, origin_transform, frame, x_size, y_size, z_size, resolution, INFINITY, "get_planning_scene");
+    ROS_INFO("...startup complete");
+    ////////////////////
+    ///// Display! /////
+    ////////////////////
+    ros::Publisher viz_pub = nh.advertise<visualization_msgs::Marker>("sdf_markers", 1, true);
+    ros::Rate spin_rate(10);
+    while (ros::ok())
+    {
+        /* Collision map visualization message publish */
+        std::cout << "Generating a new Collision Map..." << std::endl;
+        VoxelGrid::VoxelGrid<uint8_t> coll_map = sdf_builder.UpdateCollisionMap(sdf_tools::USE_FULL_PLANNING_SCENE);
+        std::cout << "...Collision Map with " << (coll_map.GetNumXCells() * coll_map.GetNumYCells() * coll_map.GetNumZCells()) << " cells generated - sending to RVIZ" << std::endl;
+        viz_pub.publish(ExportCollisionMapForDisplay(coll_map, "base", 1.0));
+        /* SDF visualization message publish */
+        clock_t st, et;
+        st = std::clock();
+        sdf_tools::SignedDistanceField sdf = sdf_builder.UpdateSDF(sdf_tools::USE_CACHED);
+        et = std::clock();
+        std::cout << "SDF with " << (sdf.GetNumXCells() * sdf.GetNumYCells() * sdf.GetNumZCells()) << " cells generated - took " << (((float)(et - st)) / CLOCKS_PER_SEC) << " seconds to compute" << std::endl;
+        viz_pub.publish(sdf.ExportForDisplay(0.1));
+        ros::spinOnce();
+        spin_rate.sleep();
+    }
+    return 0;
+}

flightlib/third_party/sdf_tools/src/sdf_tools/dynamic_spatial_hashed_collision_map.cpp

@@ -0,0 +1,200 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <vector>
+#include <string>
+#include <functional>
+#include <unordered_map>
+#include <sstream>
+#include <iostream>
+#include <stdexcept>
+#include <Eigen/Geometry>
+#include <visualization_msgs/Marker.h>
+#include <arc_utilities/eigen_helpers_conversions.hpp>
+#include <arc_utilities/voxel_grid.hpp>
+#include <arc_utilities/dynamic_spatial_hashed_voxel_grid.hpp>
+#include <sdf_tools/collision_map.hpp>
+#include <sdf_tools/dynamic_spatial_hashed_collision_map.hpp>
+
+namespace sdf_tools
+{
+    DynamicSpatialHashedCollisionMapGrid::DynamicSpatialHashedCollisionMapGrid(std::string frame, double resolution, int64_t chunk_x_size, int64_t chunk_y_size, int64_t chunk_z_size, COLLISION_CELL OOB_value)
+    {
+        frame_ = frame;
+        VoxelGrid::DynamicSpatialHashedVoxelGrid<COLLISION_CELL> new_field(resolution, chunk_x_size, chunk_y_size, chunk_z_size, OOB_value);
+        collision_field_ = new_field;
+        number_of_components_ = 0;
+        components_valid_ = false;
+    }
+
+    DynamicSpatialHashedCollisionMapGrid::DynamicSpatialHashedCollisionMapGrid(Eigen::Isometry3d origin_transform, std::string frame, double resolution, int64_t chunk_x_size, int64_t chunk_y_size, int64_t chunk_z_size, COLLISION_CELL OOB_value)
+    {
+        frame_ = frame;
+        VoxelGrid::DynamicSpatialHashedVoxelGrid<COLLISION_CELL> new_field(origin_transform, resolution, chunk_x_size, chunk_y_size, chunk_z_size, OOB_value);
+        collision_field_ = new_field;
+        number_of_components_ = 0;
+        components_valid_ = false;
+    }
+
+    DynamicSpatialHashedCollisionMapGrid::DynamicSpatialHashedCollisionMapGrid() : number_of_components_(0), initialized_(false), components_valid_(false) {}
+
+    bool DynamicSpatialHashedCollisionMapGrid::IsInitialized() const
+    {
+        return initialized_;
+    }
+
+    bool DynamicSpatialHashedCollisionMapGrid::AreComponentsValid() const
+    {
+        return components_valid_;
+    }
+
+    std::pair<COLLISION_CELL, VoxelGrid::FOUND_STATUS> DynamicSpatialHashedCollisionMapGrid::Get(const double x, const double y, const double z) const
+    {
+        return collision_field_.GetImmutable(x, y, z);
+    }
+
+    std::pair<COLLISION_CELL, VoxelGrid::FOUND_STATUS> DynamicSpatialHashedCollisionMapGrid::Get(const Eigen::Vector3d& location) const
+    {
+        return collision_field_.GetImmutable(location);
+    }
+
+    VoxelGrid::SET_STATUS DynamicSpatialHashedCollisionMapGrid::SetCell(const double x, const double y, const double z, COLLISION_CELL value)
+    {
+        return collision_field_.SetCellValue(x, y, z, value);
+    }
+
+    VoxelGrid::SET_STATUS DynamicSpatialHashedCollisionMapGrid::SetCell(const Eigen::Vector3d& location, COLLISION_CELL value)
+    {
+        return collision_field_.SetCellValue(location, value);
+    }
+
+    VoxelGrid::SET_STATUS DynamicSpatialHashedCollisionMapGrid::SetChunk(const double x, const double y, const double z, COLLISION_CELL value)
+    {
+        return collision_field_.SetChunkValue(x, y, z, value);
+    }
+
+    VoxelGrid::SET_STATUS DynamicSpatialHashedCollisionMapGrid::SetChunk(const Eigen::Vector3d& location, COLLISION_CELL value)
+    {
+        return collision_field_.SetChunkValue(location, value);
+    }
+
+    Eigen::Isometry3d DynamicSpatialHashedCollisionMapGrid::GetOriginTransform() const
+    {
+        return collision_field_.GetOriginTransform();
+    }
+
+    std::vector<visualization_msgs::Marker> DynamicSpatialHashedCollisionMapGrid::ExportForDisplay(const std_msgs::ColorRGBA& collision_color, const std_msgs::ColorRGBA& free_color, const std_msgs::ColorRGBA& unknown_color) const
+    {
+        // Assemble a visualization_markers::Marker representation of the SDF to display in RViz
+        // We make one marker for the individual cells, and another for the chunks, but we return 0, 1, or 2
+        // markers depending on if any chenks or cells can be drawn.
+        // First, the chunks
+        visualization_msgs::Marker chunks_display_rep;
+        // Populate the header
+        chunks_display_rep.header.frame_id = frame_;
+        // Populate the options
+        chunks_display_rep.ns = "dynamic_spatial_hashed_collision_map_chunks_display";
+        chunks_display_rep.id = 1;
+        chunks_display_rep.type = visualization_msgs::Marker::CUBE_LIST;
+        chunks_display_rep.action = visualization_msgs::Marker::ADD;
+        chunks_display_rep.lifetime = ros::Duration(0.0);
+        chunks_display_rep.frame_locked = false;
+        const Eigen::Isometry3d base_transform = Eigen::Isometry3d::Identity();
+        chunks_display_rep.pose = EigenHelpersConversions::EigenIsometry3dToGeometryPose(base_transform);
+        std::vector<double> chunk_sizes = collision_field_.GetChunkSizes();
+        chunks_display_rep.scale.x = chunk_sizes[0];
+        chunks_display_rep.scale.y = chunk_sizes[1];
+        chunks_display_rep.scale.z = chunk_sizes[2];
+        // Second, the cells
+        visualization_msgs::Marker cells_display_rep;
+        // Populate the header
+        cells_display_rep.header.frame_id = frame_;
+        // Populate the options
+        cells_display_rep.ns = "dynamic_spatial_hashed_collision_map_cells_display";
+        cells_display_rep.id = 1;
+        cells_display_rep.type = visualization_msgs::Marker::CUBE_LIST;
+        cells_display_rep.action = visualization_msgs::Marker::ADD;
+        cells_display_rep.lifetime = ros::Duration(0.0);
+        cells_display_rep.frame_locked = false;
+        cells_display_rep.pose = EigenHelpersConversions::EigenIsometry3dToGeometryPose(base_transform);
+        std::vector<double> cell_sizes = collision_field_.GetCellSizes();
+        cells_display_rep.scale.x = cell_sizes[0];
+        cells_display_rep.scale.y = cell_sizes[1];
+        cells_display_rep.scale.z = cell_sizes[2];
+        // Now, go through the chunks and add everything to the message
+        const Eigen::Isometry3d& grid_transform = GetOriginTransform();
+        const std::unordered_map<VoxelGrid::CHUNK_REGION, VoxelGrid::DynamicSpatialHashedVoxelGridChunk<COLLISION_CELL>>& raw_chunks = collision_field_.GetInternalChunks();
+        std::unordered_map<VoxelGrid::CHUNK_REGION, VoxelGrid::DynamicSpatialHashedVoxelGridChunk<COLLISION_CELL>>::const_iterator raw_chunks_itr;
+        for (raw_chunks_itr = raw_chunks.begin(); raw_chunks_itr != raw_chunks.end(); ++raw_chunks_itr)
+        {
+            const VoxelGrid::DynamicSpatialHashedVoxelGridChunk<COLLISION_CELL>& current_chunk = raw_chunks_itr->second;
+            if (current_chunk.IsChunkInitialized())
+            {
+                const COLLISION_CELL& current_cell = current_chunk.GetChunkImmutable();
+                std::vector<double> cell_location_in_grid = current_chunk.GetIndexLocationInGrid(0, 0, 0);
+                Eigen::Vector3d grid_location(cell_location_in_grid[0], cell_location_in_grid[1], cell_location_in_grid[2]);
+                Eigen::Vector3d location = grid_transform * grid_location;
+                geometry_msgs::Point new_point;
+                new_point.x = location.x();
+                new_point.y = location.y();
+                new_point.z = location.z();
+                chunks_display_rep.points.push_back(new_point);
+                if (current_cell.occupancy > 0.5)
+                {
+                    chunks_display_rep.colors.push_back(collision_color);
+                }
+                else if (current_cell.occupancy < 0.5)
+                {
+                    chunks_display_rep.colors.push_back(free_color);
+                }
+                else
+                {
+                    chunks_display_rep.colors.push_back(unknown_color);
+                }
+            }
+            else if (current_chunk.IsCellInitialized())
+            {
+                for (int64_t x_index = 0; x_index < current_chunk.GetNumXCells(); x_index++)
+                {
+                    for (int64_t y_index = 0; y_index < current_chunk.GetNumYCells(); y_index++)
+                    {
+                        for (int64_t z_index = 0; z_index < current_chunk.GetNumZCells(); z_index++)
+                        {
+                            const COLLISION_CELL& current_cell = current_chunk.GetImmutableByIndex(x_index, y_index, z_index).first;
+                            std::vector<double> cell_location_in_grid = current_chunk.GetIndexLocationInGrid(x_index, y_index, z_index);
+                            Eigen::Vector3d grid_location(cell_location_in_grid[0], cell_location_in_grid[1], cell_location_in_grid[2]);
+                            Eigen::Vector3d location = grid_transform * grid_location;
+                            geometry_msgs::Point new_point;
+                            new_point.x = location.x();
+                            new_point.y = location.y();
+                            new_point.z = location.z();
+                            cells_display_rep.points.push_back(new_point);
+                            if (current_cell.occupancy > 0.5)
+                            {
+                                cells_display_rep.colors.push_back(collision_color);
+                            }
+                            else if (current_cell.occupancy < 0.5)
+                            {
+                                cells_display_rep.colors.push_back(free_color);
+                            }
+                            else
+                            {
+                                cells_display_rep.colors.push_back(unknown_color);
+                            }
+                        }
+                    }
+                }
+            }
+        }
+        // Assemble the data to return
+        std::vector<visualization_msgs::Marker> display_markers;
+        if (chunks_display_rep.points.size() > 0)
+        {
+            display_markers.push_back(chunks_display_rep);
+        }
+        if (cells_display_rep.points.size() > 0)
+        {
+            display_markers.push_back(cells_display_rep);
+        }
+        return display_markers;
+    }
+}

flightlib/third_party/sdf_tools/src/sdf_tools/sdf_builder.cpp

@@ -0,0 +1,584 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <vector>
+#include <string>
+#include <sstream>
+#include <iostream>
+#include <fstream>
+#include <stdexcept>
+#include <zlib.h>
+#include <octomap_msgs/conversions.h>
+#include "sdf_tools/sdf.hpp"
+#include "sdf_tools/sdf_builder.hpp"
+#include "sdf_tools/SDF.h"
+
+namespace sdf_tools
+{
+    double ComputeDistanceSquared(int32_t x1, int32_t y1, int32_t z1, int32_t x2, int32_t y2, int32_t z2)
+    {
+        int32_t dx = x1 - x2;
+        int32_t dy = y1 - y2;
+        int32_t dz = z1 - z2;
+        return double((dx * dx) + (dy * dy) + (dz * dz));
+    }
+
+    SDF_Builder::SDF_Builder(ros::NodeHandle& nh, Eigen::Isometry3d origin_transform, std::string frame, double x_size, double y_size, double z_size, double resolution, float OOB_value, std::string planning_scene_service) : nh_(nh)
+    {
+        origin_transform_ = origin_transform;
+        frame_ = frame;
+        x_size_ = x_size;
+        y_size_ = y_size;
+        z_size_ = z_size;
+        resolution_ = resolution;
+        OOB_value_ = OOB_value;
+        if (!BuildInternalPlanningScene())
+        {
+            throw std::invalid_argument("Unable to construct internal planning scene");
+        }
+        initialized_ = true;
+        has_cached_sdf_ = false;
+        has_cached_collmap_ = false;
+        has_planning_scene_ = false;
+        planning_scene_client_ = nh.serviceClient<moveit_msgs::GetPlanningScene>(planning_scene_service);
+    }
+
+    SDF_Builder::SDF_Builder(ros::NodeHandle& nh, std::string frame, double x_size, double y_size, double z_size, double resolution, float OOB_value, std::string planning_scene_service) : nh_(nh)
+    {
+        Eigen::Translation3d origin_translation(-x_size * 0.5, -y_size * 0.5, -z_size * 0.5);
+        Eigen::Quaterniond origin_rotation;
+        origin_rotation.setIdentity();
+        origin_transform_ = origin_translation * origin_rotation;
+        frame_ = frame;
+        x_size_ = x_size;
+        y_size_ = y_size;
+        z_size_ = z_size;
+        resolution_ = resolution;
+        OOB_value_ = OOB_value;
+        if (!BuildInternalPlanningScene())
+        {
+            throw std::invalid_argument("Unable to construct internal planning scene");
+        }
+        initialized_ = true;
+        has_cached_sdf_ = false;
+        has_cached_collmap_ = false;
+        has_planning_scene_ = false;
+        planning_scene_client_ = nh.serviceClient<moveit_msgs::GetPlanningScene>(planning_scene_service);
+    }
+
+    SDF_Builder::SDF_Builder()
+    {
+        initialized_ = false;
+        has_cached_sdf_ = false;
+        has_cached_collmap_ = false;
+        has_planning_scene_ = false;
+    }
+
+    bool SDF_Builder::BuildInternalPlanningScene()
+    {
+        /* Builds a planning scene from XML string urdf and srdf descriptions */
+        // Make the URDF model
+        boost::shared_ptr<urdf::Model> urdf_model(new urdf::Model());
+        urdf_model->initString(GenerateSDFComputeBotURDFString());
+        // Make the SRDF model
+        boost::shared_ptr<srdf::Model> srdf_model(new srdf::Model());
+        srdf_model->initString(*urdf_model, GenerateSDFComputeBotSRDFString());
+        // Make the planning scene
+        planning_scene_ptr_.reset();
+        planning_scene_ptr_ = std::shared_ptr<planning_scene::PlanningScene>(new planning_scene::PlanningScene(urdf_model, srdf_model));
+        return true;
+    }
+
+    std::string SDF_Builder::GenerateSDFComputeBotURDFString()
+    {
+        // Figure out the minimum+maximum X,Y,Z values (for safety, we pad them out to make sure)
+        double min_x_limit = origin_transform_.translation().x() - fabs(2 * x_size_);
+        double max_x_limit = origin_transform_.translation().x() + fabs(2 * x_size_);
+        double min_y_limit = origin_transform_.translation().y() - fabs(2 * y_size_);
+        double max_y_limit = origin_transform_.translation().y() + fabs(2 * y_size_);
+        double min_z_limit = origin_transform_.translation().z() - fabs(2 * z_size_);
+        double max_z_limit = origin_transform_.translation().z() + fabs(2 * z_size_);
+        // Make the limits into strings
+        std::ostringstream mnxls_strm;
+        mnxls_strm << min_x_limit;
+        std::string min_x_limit_str = mnxls_strm.str();
+        std::ostringstream mxxls_strm;
+        mxxls_strm << max_x_limit;
+        std::string max_x_limit_str = mxxls_strm.str();
+        std::ostringstream mnyls_strm;
+        mnyls_strm << min_y_limit;
+        std::string min_y_limit_str = mnyls_strm.str();
+        std::ostringstream mxyls_strm;
+        mxyls_strm << max_y_limit;
+        std::string max_y_limit_str = mxyls_strm.str();
+        std::ostringstream mnzls_strm;
+        mnzls_strm << min_z_limit;
+        std::string min_z_limit_str = mnzls_strm.str();
+        std::ostringstream mxzls_strm;
+        mxzls_strm << max_z_limit;
+        std::string max_z_limit_str = mxzls_strm.str();
+        // Figure out the cell resolution
+        std::ostringstream crs_strm;
+        crs_strm << resolution_;
+        std::string cell_resolution_str = crs_strm.str();
+        // Make the URDF xml string
+        std::string urdf_string;
+        urdf_string = "<?xml version=\"1.0\" ?>\n<robot name=\"sdf_compute_bot\">\n<link name=\"" + frame_ + "\">\n</link>\n<joint name=\"virtual_x\" type=\"prismatic\">\n<parent link=\"" + frame_ + "\"/>\n<child link=\"x_stage\"/>\n<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n<axis xyz=\"1 0 0\"/>\n<limit effort=\"10.0\" lower=\"" + min_x_limit_str + "\" upper=\"10.0\" velocity=\"" + max_x_limit_str + "\"/>\n</joint>\n<link name=\"x_stage\">\n</link>\n<joint name=\"virtual_y\" type=\"prismatic\">\n<parent link=\"x_stage\"/>\n<child link=\"y_stage\"/>\n<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n<axis xyz=\"0 1 0\"/>\n<limit effort=\"10.0\" lower=\"" + min_y_limit_str + "\" upper=\"" + max_y_limit_str + "\" velocity=\"10.0\"/>\n</joint>\n<link name=\"y_stage\">\n</link>\n<joint name=\"virtual_z\" type=\"prismatic\">\n<parent link=\"y_stage\"/>\n<child link=\"sdf_bot\"/>\n<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n<axis xyz=\"0 0 1\"/>\n<limit effort=\"10.0\" lower=\"" + min_z_limit_str + "\" upper=\"" + max_z_limit_str + "\" velocity=\"10.0\"/>\n</joint>\n<link name=\"sdf_bot\">\n<visual>\n<geometry>\n<box size=\"" + cell_resolution_str + " " + cell_resolution_str + " " + cell_resolution_str + "\"/>\n</geometry>\n<origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>\n<material name=\"\">\n<color rgba=\"0.3 0.3 0.3 1.0\"/>\n</material>\n</visual>\n</link>\n</robot>";
+        return urdf_string;
+    }
+
+    std::string SDF_Builder::GenerateSDFComputeBotSRDFString()
+    {
+        std::string srdf_string;
+        srdf_string = "<?xml version=\"1.0\" ?>\n<robot name=\"sdf_compute_bot\">\n<group name=\"platform\">\n<joint name=\"virtual_x\" />\n<joint name=\"virtual_y\" />\n<joint name=\"virtual_z\" />\n</group>\n<disable_collisions link1=\"" + frame_ + "\" link2=\"x_stage\" reason=\"Adjacent\" />\n<disable_collisions link1=\"" + frame_ + "\" link2=\"y_stage\" reason=\"Adjacent\" />\n<disable_collisions link1=\"" + frame_ + "\" link2=\"sdf_bot\" reason=\"Adjacent\" />\n<disable_collisions link1=\"x_stage\" link2=\"y_stage\" reason=\"Adjacent\" />\n<disable_collisions link1=\"x_stage\" link2=\"sdf_bot\" reason=\"Adjacent\" />\n<disable_collisions link1=\"y_stage\" link2=\"sdf_bot\" reason=\"Adjacent\" />\n</robot>";
+        return srdf_string;
+    }
+
+    SignedDistanceField SDF_Builder::UpdateSDF(uint8_t update_mode)
+    {
+        if (!initialized_)
+        {
+            throw std::invalid_argument("SDF Builder has not been initialized");
+        }
+        if (update_mode == USE_CACHED)
+        {
+            if (has_planning_scene_)
+            {
+                // Build the SDF
+                return UpdateSDFFromPlanningScene();
+            }
+            else
+            {
+                ROS_ERROR("No planning scene available");
+                throw std::invalid_argument("No planning scene available");
+            }
+        }
+        else
+        {
+            if (update_mode == USE_ONLY_COLLISION_OBJECTS)
+            {
+                // Update the planning scene
+                moveit_msgs::GetPlanningSceneRequest ps_req;
+                ps_req.components.components = moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY;
+                moveit_msgs::GetPlanningSceneResponse ps_res;
+                planning_scene_client_.call(ps_req, ps_res);
+                moveit_msgs::PlanningScene& planning_scene_state = ps_res.scene;
+                planning_scene_ptr_->usePlanningSceneMsg(planning_scene_state);
+                has_planning_scene_ = true;
+                // Build the SDF
+                return UpdateSDFFromPlanningScene();
+            }
+            else if (update_mode == USE_ONLY_OCTOMAP)
+            {
+                // Update the planning scene
+                moveit_msgs::GetPlanningSceneRequest ps_req;
+                ps_req.components.components = moveit_msgs::PlanningSceneComponents::OCTOMAP;
+                moveit_msgs::GetPlanningSceneResponse ps_res;
+                planning_scene_client_.call(ps_req, ps_res);
+                moveit_msgs::PlanningScene& planning_scene_state = ps_res.scene;
+                planning_scene_ptr_->usePlanningSceneMsg(planning_scene_state);
+                has_planning_scene_ = true;
+                // Build the SDF
+                return UpdateSDFFromPlanningScene();
+            }
+            else if (update_mode == USE_FULL_PLANNING_SCENE)
+            {
+                // Update the planning scene
+                moveit_msgs::GetPlanningSceneRequest ps_req;
+                ps_req.components.components = moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY | moveit_msgs::PlanningSceneComponents::OCTOMAP;
+                moveit_msgs::GetPlanningSceneResponse ps_res;
+                planning_scene_client_.call(ps_req, ps_res);
+                moveit_msgs::PlanningScene& planning_scene_state = ps_res.scene;
+                planning_scene_ptr_->usePlanningSceneMsg(planning_scene_state);
+                has_planning_scene_ = true;
+                // Build the SDF
+                return UpdateSDFFromPlanningScene();
+            }
+            else
+            {
+                ROS_ERROR("Invalid update mode (mode not recognized)");
+                throw std::invalid_argument("Invalid update mode (mode not recognized)");
+            }
+        }
+    }
+
+    SignedDistanceField SDF_Builder::GetCachedSDF()
+    {
+        if (has_cached_sdf_)
+        {
+            return cached_sdf_;
+        }
+        else
+        {
+            ROS_ERROR("No cached SDF available");
+            throw std::invalid_argument("No cached SDF available");
+        }
+    }
+
+    VoxelGrid::VoxelGrid<uint8_t> SDF_Builder::UpdateCollisionMap(uint8_t update_mode)
+    {
+        if (!initialized_)
+        {
+            throw std::invalid_argument("SDF Builder has not been initialized");
+        }
+        if (update_mode == USE_CACHED)
+        {
+            if (has_planning_scene_)
+            {
+                // Build the collision map
+                return UpdateCollisionMapFromPlanningScene();
+            }
+            else
+            {
+                ROS_ERROR("No planning scene available");
+                throw std::invalid_argument("No planning scene available");
+            }
+        }
+        else
+        {
+            if (update_mode == USE_ONLY_COLLISION_OBJECTS)
+            {
+                // Update the planning scene
+                moveit_msgs::GetPlanningSceneRequest ps_req;
+                ps_req.components.components = moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY;
+                moveit_msgs::GetPlanningSceneResponse ps_res;
+                planning_scene_client_.call(ps_req, ps_res);
+                moveit_msgs::PlanningScene& planning_scene_state = ps_res.scene;
+                planning_scene_ptr_->usePlanningSceneMsg(planning_scene_state);
+                has_planning_scene_ = true;
+                // Build the collision map
+                return UpdateCollisionMapFromPlanningScene();
+            }
+            else if (update_mode == USE_ONLY_OCTOMAP)
+            {
+                // Update the planning scene
+                moveit_msgs::GetPlanningSceneRequest ps_req;
+                ps_req.components.components = moveit_msgs::PlanningSceneComponents::OCTOMAP;
+                moveit_msgs::GetPlanningSceneResponse ps_res;
+                planning_scene_client_.call(ps_req, ps_res);
+                moveit_msgs::PlanningScene& planning_scene_state = ps_res.scene;
+                planning_scene_ptr_->usePlanningSceneMsg(planning_scene_state);
+                has_planning_scene_ = true;
+                // Build the collision map
+                return UpdateCollisionMapFromPlanningScene();
+            }
+            else if (update_mode == USE_FULL_PLANNING_SCENE)
+            {
+                // Update the planning scene
+                moveit_msgs::GetPlanningSceneRequest ps_req;
+                ps_req.components.components = moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES | moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY | moveit_msgs::PlanningSceneComponents::OCTOMAP;
+                moveit_msgs::GetPlanningSceneResponse ps_res;
+                planning_scene_client_.call(ps_req, ps_res);
+                moveit_msgs::PlanningScene& planning_scene_state = ps_res.scene;
+                planning_scene_ptr_->usePlanningSceneMsg(planning_scene_state);
+                has_planning_scene_ = true;
+                // Build the collision map
+                return UpdateCollisionMapFromPlanningScene();
+            }
+            else
+            {
+                ROS_ERROR("Invalid update mode (mode not recognized)");
+                throw std::invalid_argument("Invalid update mode (mode not recognized)");
+            }
+        }
+    }
+
+    VoxelGrid::VoxelGrid<uint8_t> SDF_Builder::GetCachedCollisionMap()
+    {
+        if (has_cached_collmap_)
+        {
+            return cached_collmap_;
+        }
+        else
+        {
+            ROS_ERROR("No cached Collision Map available");
+            throw std::invalid_argument("No cached Collision Map available");
+        }
+    }
+
+    VoxelGrid::VoxelGrid<uint8_t> SDF_Builder::UpdateCollisionMapFromPlanningScene()
+    {
+        // Make a collision field for debugging
+        VoxelGrid::VoxelGrid<uint8_t> collision_field(origin_transform_, resolution_, x_size_, y_size_, z_size_, 0);
+        // Loop through the planning scene to populate the voxel grids
+        std::string x_joint("virtual_x");
+        std::string y_joint("virtual_y");
+        std::string z_joint("virtual_z");
+        collision_detection::CollisionRequest col_req;
+        collision_detection::CollisionResult col_res;
+        robot_state::RobotState& sdf_compute_bot_state = planning_scene_ptr_->getCurrentStateNonConst();
+        for (int64_t x_index = 0; x_index < collision_field.GetNumXCells(); x_index++)
+        {
+            for (int64_t y_index = 0; y_index < collision_field.GetNumYCells(); y_index++)
+            {
+                for (int64_t z_index = 0; z_index < collision_field.GetNumZCells(); z_index++)
+                {
+                    // Convert SDF indices into a real-world location
+                    std::vector<double> location = collision_field.GridIndexToLocation(x_index, y_index, z_index);
+                    double x = location[0];
+                    double y = location[1];
+                    double z = location[2];
+                    // Set them
+                    sdf_compute_bot_state.setJointPositions(x_joint, &x);
+                    sdf_compute_bot_state.setJointPositions(y_joint, &y);
+                    sdf_compute_bot_state.setJointPositions(z_joint, &z);
+                    col_res.clear();
+                    planning_scene_ptr_->checkCollision(col_req, col_res);
+                    if (col_res.collision)
+                    {
+                        // Mark as filled
+                        //std::cout << "Collision" << std::endl;
+                        uint8_t status = 1;
+                        collision_field.SetValue(x_index, y_index, z_index, status);
+                    }
+                    else
+                    {
+                        // Mark as free space
+                        //std::cout << "No collision" << std::endl;
+                        uint8_t status = 0;
+                        collision_field.SetValue(x_index, y_index, z_index, status);
+                    }
+                }
+            }
+        }
+        // Export the collision map
+        cached_collmap_ = collision_field;
+        has_cached_collmap_ = true;
+        return collision_field;
+    }
+
+    SignedDistanceField SDF_Builder::UpdateSDFFromPlanningScene()
+    {
+        // Make the SDF
+        SignedDistanceField new_sdf(origin_transform_, frame_, resolution_, x_size_, y_size_, z_size_, OOB_value_);
+        // Loop through the planning scene to populate the voxel grids
+        std::vector<Eigen::Vector3i> filled;
+        std::vector<Eigen::Vector3i> free;
+        std::string x_joint("virtual_x");
+        std::string y_joint("virtual_y");
+        std::string z_joint("virtual_z");
+        collision_detection::CollisionRequest col_req;
+        collision_detection::CollisionResult col_res;
+        robot_state::RobotState& sdf_compute_bot_state = planning_scene_ptr_->getCurrentStateNonConst();
+        for (uint32_t x_index = 0; x_index < new_sdf.GetNumXCells(); x_index++)
+        {
+            for (uint32_t y_index = 0; y_index < new_sdf.GetNumYCells(); y_index++)
+            {
+                for (uint32_t z_index = 0; z_index < new_sdf.GetNumZCells(); z_index++)
+                {
+                    // Convert SDF indices into a real-world location
+                    std::vector<double> location = new_sdf.GridIndexToLocation(x_index, y_index, z_index);
+                    double x = location[0];
+                    double y = location[1];
+                    double z = location[2];
+                    sdf_compute_bot_state.setJointPositions(x_joint, &x);
+                    sdf_compute_bot_state.setJointPositions(y_joint, &y);
+                    sdf_compute_bot_state.setJointPositions(z_joint, &z);
+                    col_res.clear();
+                    planning_scene_ptr_->checkCollision(col_req, col_res);
+                    if (col_res.collision)
+                    {
+                        // Mark as filled
+                        filled.push_back(Eigen::Vector3i(x_index, y_index, z_index));
+                    }
+                    else
+                    {
+                        // Mark as free space
+                        free.push_back(Eigen::Vector3i(x_index, y_index, z_index));
+                    }
+                }
+            }
+        }
+        // Make two distance fields (one for distance to filled voxels, one for distance to free voxels
+        DistanceField filled_distance_field = BuildDistanceField(filled);
+        DistanceField free_distance_field = BuildDistanceField(free);
+        // Generate the SDF
+        for (int64_t x_index = 0; x_index < filled_distance_field.GetNumXCells(); x_index++)
+        {
+            for (int64_t y_index = 0; y_index < filled_distance_field.GetNumYCells(); y_index++)
+            {
+                for (int64_t z_index = 0; z_index < filled_distance_field.GetNumZCells(); z_index++)
+                {
+                    double distance1 = sqrt(filled_distance_field.GetImmutable(x_index, y_index, z_index).first.distance_square) * resolution_;
+                    double distance2 = sqrt(free_distance_field.GetImmutable(x_index, y_index, z_index).first.distance_square) * resolution_;
+                    new_sdf.Set(x_index, y_index, z_index, (distance1 - distance2));
+                }
+            }
+        }
+        // Export the SDF
+        cached_sdf_ = new_sdf;
+        has_cached_sdf_ = true;
+        return new_sdf;
+    }
+
+    std::vector<std::vector<std::vector<std::vector<int>>>> SDF_Builder::MakeNeighborhoods()
+    {
+        std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods;
+        neighborhoods.resize(2);
+        for (size_t n = 0; n < neighborhoods.size(); n++)
+        {
+            neighborhoods[n].resize(27);
+            // Loop through the source directions
+            for (int dx = -1; dx <= 1; dx++)
+            {
+                for (int dy = -1; dy <= 1; dy++)
+                {
+                    for (int dz = -1; dz <= 1; dz++)
+                    {
+                        int direction_number = GetDirectionNumber(dx, dy, dz);
+                        // Loop through the target directions
+                        for (int tdx = -1; tdx <= 1; tdx++)
+                        {
+                            for (int tdy = -1; tdy <= 1; tdy++)
+                            {
+                                for (int tdz = -1; tdz <= 1; tdz++)
+                                {
+                                    if (tdx == 0 && tdy == 0 && tdz == 0)
+                                    {
+                                        continue;
+                                    }
+                                    if (n >= 1)
+                                    {
+                                        if ((abs(tdx) + abs(tdy) + abs(tdz)) != 1)
+                                        {
+                                            continue;
+                                        }
+                                        if ((dx * tdx) < 0 || (dy * tdy) < 0 || (dz * tdz) < 0)
+                                        {
+                                            continue;
+                                        }
+                                    }
+                                    std::vector<int> new_point;
+                                    new_point.resize(3);
+                                    new_point[0] = tdx;
+                                    new_point[1] = tdy;
+                                    new_point[2] = tdz;
+                                    neighborhoods[n][direction_number].push_back(new_point);
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+        return neighborhoods;
+    }
+
+    int SDF_Builder::GetDirectionNumber(int dx, int dy, int dz)
+    {
+        return ((dx + 1) * 9) + ((dy + 1) * 3) + (dz + 1);
+    }
+
+    DistanceField SDF_Builder::BuildDistanceField(std::vector<Eigen::Vector3i>& points)
+    {
+        // Make the DistanceField container
+        bucket_cell default_cell;
+        default_cell.distance_square = INFINITY;
+        DistanceField distance_field(origin_transform_, resolution_, x_size_, y_size_, z_size_, default_cell);
+        // Compute maximum distance square
+        long max_distance_square = (distance_field.GetNumXCells() * distance_field.GetNumXCells()) + (distance_field.GetNumYCells() * distance_field.GetNumYCells()) + (distance_field.GetNumZCells() * distance_field.GetNumZCells());
+        // Make bucket queue
+        std::vector<std::vector<bucket_cell>> bucket_queue(max_distance_square + 1);
+        bucket_queue[0].reserve(points.size());
+        // Set initial update direction
+        int initial_update_direction = GetDirectionNumber(0, 0, 0);
+        // Mark all points with distance zero and add to the bucket queue
+        for (size_t index = 0; index < points.size(); index++)
+        {
+            std::pair<bucket_cell&, bool> query = distance_field.GetMutable((int64_t)points[index].x(), (int64_t)points[index].y(), (int64_t)points[index].z());
+            if (query.second)
+            {
+                query.first.location[0] = points[index].x();
+                query.first.location[1] = points[index].y();
+                query.first.location[2] = points[index].z();
+                query.first.closest_point[0] = points[index].x();
+                query.first.closest_point[1] = points[index].y();
+                query.first.closest_point[2] = points[index].z();
+                query.first.distance_square = 0.0;
+                query.first.update_direction = initial_update_direction;
+                bucket_queue[0].push_back(query.first);
+            }
+            // If the point is outside the bounds of the SDF, skip
+            else
+            {
+                continue;
+            }
+        }
+        // Process the bucket queue
+        std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods = MakeNeighborhoods();
+        for (size_t bq_idx = 0; bq_idx < bucket_queue.size(); bq_idx++)
+        {
+            std::vector<bucket_cell>::iterator queue_itr = bucket_queue[bq_idx].begin();
+            while (queue_itr != bucket_queue[bq_idx].end())
+            {
+                // Get the current location
+                bucket_cell& cur_cell = *queue_itr;
+                double x = cur_cell.location[0];
+                double y = cur_cell.location[1];
+                double z = cur_cell.location[2];
+                // Pick the update direction
+                int D = bq_idx;
+                if (D > 1)
+                {
+                    D = 1;
+                }
+                // Make sure the update direction is valid
+                if (cur_cell.update_direction < 0 || cur_cell.update_direction > 26)
+                {
+                    ++queue_itr;
+                    continue;
+                }
+                // Get the current neighborhood list
+                std::vector<std::vector<int>>& neighborhood = neighborhoods[D][cur_cell.update_direction];
+                // Update the distance from the neighboring cells
+                for (size_t nh_idx = 0; nh_idx < neighborhood.size(); nh_idx++)
+                {
+                    // Get the direction to check
+                    int dx = neighborhood[nh_idx][0];
+                    int dy = neighborhood[nh_idx][1];
+                    int dz = neighborhood[nh_idx][2];
+                    int nx = x + dx;
+                    int ny = y + dy;
+                    int nz = z + dz;
+                    std::pair<bucket_cell&, bool> neighbor_query = distance_field.GetMutable((int64_t)nx, (int64_t)ny, (int64_t)nz);
+                    if (!neighbor_query.second)
+                    {
+                        // "Neighbor" is outside the bounds of the SDF
+                        continue;
+                    }
+                    // Update the neighbor's distance based on the current
+                    int new_distance_square = ComputeDistanceSquared(nx, ny, nz, cur_cell.closest_point[0], cur_cell.closest_point[1], cur_cell.closest_point[2]);
+                    if (new_distance_square > max_distance_square)
+                    {
+                        // Skip these cases
+                        continue;
+                    }
+                    if (new_distance_square < neighbor_query.first.distance_square)
+                    {
+                        // If the distance is better, time to update the neighbor
+                        neighbor_query.first.distance_square = new_distance_square;
+                        neighbor_query.first.closest_point[0] = cur_cell.closest_point[0];
+                        neighbor_query.first.closest_point[1] = cur_cell.closest_point[1];
+                        neighbor_query.first.closest_point[2] = cur_cell.closest_point[2];
+                        neighbor_query.first.location[0] = nx;
+                        neighbor_query.first.location[1] = ny;
+                        neighbor_query.first.location[2] = nz;
+                        neighbor_query.first.update_direction = GetDirectionNumber(dx, dy, dz);
+                        // Add the neighbor into the bucket queue
+                        bucket_queue[new_distance_square].push_back(neighbor_query.first);
+                    }
+                }
+                // Increment the queue iterator
+                ++queue_itr;
+            }
+            // Clear the current queue now that we're done with it
+            bucket_queue[bq_idx].clear();
+        }
+        return distance_field;
+    }
+
+    void SDF_Builder::UpdatePlanningSceneFromMessage(moveit_msgs::PlanningScene& planning_scene)
+    {
+        planning_scene_ptr_->usePlanningSceneMsg(planning_scene);
+        has_planning_scene_ = true;
+    }
+}

flightlib/third_party/sdf_tools/src/sdf_tools_tutorial.cpp

@@ -0,0 +1,155 @@
+#include <ros/ros.h>
+#include <arc_utilities/voxel_grid.hpp>
+#include <arc_utilities/pretty_print.hpp>
+#include "sdf_tools/collision_map.hpp"
+#include "sdf_tools/CollisionMap.h"
+#include "sdf_tools/sdf.hpp"
+#include "sdf_tools/SDF.h"
+
+int main(int argc, char** argv)
+{
+    // Make a ROS node, which we'll use to publish copies of the data in the CollisionMap and SDF
+    // and Rviz markers that allow us to visualize them.
+    ros::init(argc, argv, "sdf_tools_tutorial");
+    // Get a handle to the current node
+    ros::NodeHandle nh;
+    // Make a publisher for visualization messages
+    ros::Publisher visualization_pub = nh.advertise<visualization_msgs::Marker>("sdf_tools_tutorial_visualization", 1, true);
+    // Make a publisher for serialized CollisionMaps
+    ros::Publisher collision_map_pub = nh.advertise<sdf_tools::CollisionMap>("collision_map_pub", 1, true);
+    // Make a publisher for serialized SDFs
+    ros::Publisher sdf_pub = nh.advertise<sdf_tools::SDF>("sdf_pub", 1, true);
+    // In preparation, we want to set a couple common paramters
+    double resolution = 0.25;
+    double x_size = 10.0;
+    double y_size = 10.0;
+    double z_size = 10.0;
+    // Let's center the grid around the origin
+    Eigen::Translation3d origin_translation(-5.0, -5.0, -5.0);
+    Eigen::Quaterniond origin_rotation(1.0, 0.0, 0.0, 0.0);
+    Eigen::Isometry3d origin_transform = origin_translation * origin_rotation;
+    std::string frame = "tutorial_frame";
+    ///////////////////////////////////
+    //// Let's make a CollisionMap ////
+    ///////////////////////////////////
+    // We pick a reasonable out-of-bounds value
+    sdf_tools::COLLISION_CELL oob_cell;
+    oob_cell.occupancy = 0.0;
+    oob_cell.component = 0; // This should ALWAYS be zero, unless you know exactly what you're doing
+    // Instead, we could initialize it like this - the component value is automatically set to 0
+    sdf_tools::COLLISION_CELL oob_cell_2(0.0);
+    // First, let's make the container
+    sdf_tools::CollisionMapGrid collision_map(origin_transform, frame, resolution, x_size, y_size, z_size, oob_cell);
+    // Let's set some values
+    // This is how you should iterate through the 3D grid's cells
+    for (int64_t x_index = 0; x_index < collision_map.GetNumXCells(); x_index++)
+    {
+        for (int64_t y_index = 0; y_index < collision_map.GetNumYCells(); y_index++)
+        {
+            for (int64_t z_index = 0; z_index < collision_map.GetNumZCells(); z_index++)
+            {
+                // Let's make the bottom corner (low x, low y, low z) an object
+                if ((x_index < (collision_map.GetNumXCells() / 2)) && (y_index < (collision_map.GetNumYCells() / 2)) && (z_index < (collision_map.GetNumZCells() / 2)))
+                {
+                    sdf_tools::COLLISION_CELL obstacle_cell(1.0); // Occupancy values > 0.5 are obstacles
+                    collision_map.Set(x_index, y_index, z_index, obstacle_cell);
+                }
+            }
+        }
+    }
+    // We can also set by location
+    sdf_tools::COLLISION_CELL obstacle_cell(1.0); // Occupancy values > 0.5 are obstacles
+    collision_map.Set(0.0, 0.0, 0.0, obstacle_cell);
+    // Let's get some values
+    // We can query by index
+    int64_t x_index = 10;
+    int64_t y_index = 10;
+    int64_t z_index = 10;
+    std::pair<sdf_tools::COLLISION_CELL, bool> index_query = collision_map.Get(x_index, y_index, z_index);
+    std::cout << "Index query result - stored value " << index_query.first.occupancy << " (occupancy) " << index_query.first.component << " (component) was it in the grid? - " << index_query.second << std::endl;
+    // Or we can query by location
+    double x_location = 0.0;
+    double y_location = 0.0;
+    double z_location = 0.0;
+    std::pair<sdf_tools::COLLISION_CELL, bool> location_query = collision_map.Get(x_location, y_location, z_location);
+    std::cout << "Location query result - stored value " << location_query.first.occupancy << " (occupancy) " << location_query.first.component << " (component) was it in the grid? - " << location_query.second << std::endl;
+    // Let's compute connected components
+    uint32_t num_connected_components = collision_map.UpdateConnectedComponents();
+    std::cout << " There are " << num_connected_components << " connected components in the grid" << std::endl;
+    // Let's display the results to Rviz
+    // First, the CollisionMap itself
+    // We need to provide colors to use
+    std_msgs::ColorRGBA collision_color;
+    collision_color.r = 1.0;
+    collision_color.g = 0.0;
+    collision_color.b = 0.0;
+    collision_color.a = 0.5;
+    std_msgs::ColorRGBA free_color;
+    free_color.r = 0.0;
+    free_color.g = 1.0;
+    free_color.b = 0.0;
+    free_color.a = 0.5;
+    std_msgs::ColorRGBA unknown_color;
+    unknown_color.r = 1.0;
+    unknown_color.g = 1.0;
+    unknown_color.b = 0.0;
+    unknown_color.a = 0.5;
+    visualization_msgs::Marker collision_map_marker = collision_map.ExportForDisplay(collision_color, free_color, unknown_color);
+    // To be safe, you'll need to set these yourself. The namespace (ns) value should distinguish between different things being displayed
+    // while the id value lets you have multiple versions of the same message at once. Always set this to 1 if you only want one copy.
+    collision_map_marker.ns = "collision_map";
+    collision_map_marker.id = 1;
+    // Send it off for display
+    visualization_pub.publish(collision_map_marker);
+    // Now, let's draw the connected components
+    visualization_msgs::Marker connected_components_marker = collision_map.ExportConnectedComponentsForDisplay(false); // Generally, you don't want a special color for unknown [P(occupancy) = 0.5] components
+    connected_components_marker.ns = "connected_components";
+    connected_components_marker.id = 1;
+    visualization_pub.publish(connected_components_marker);
+    // Let's export the CollisionMap - this is how you can transfer it to another ROS node
+    collision_map_pub.publish(collision_map.GetMessageRepresentation());
+    // You can also save it to a file
+    std::string collision_map_filename = "collision_map.cmg";
+    collision_map.SaveToFile(collision_map_filename);
+    // And load it back in
+    bool loaded = collision_map.LoadFromFile(collision_map_filename);
+    if (loaded)
+    {
+        std::cout << "Reloaded CollisionMap from file" << std::endl;
+    }
+    else
+    {
+        std::cerr << "Whoa, something broke!" << std::endl;
+    }
+    ///////////////////////////
+    //// Let's make an SDF ////
+    ///////////////////////////
+    // We pick a reasonable out-of-bounds value
+    float oob_value = INFINITY;
+    // We start by extracting the SDF from the CollisionMap
+    std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> sdf_with_extrema = collision_map.ExtractSignedDistanceField(oob_value);
+    sdf_tools::SignedDistanceField& sdf = sdf_with_extrema.first;
+    std::pair<double, double> sdf_extrema = sdf_with_extrema.second;
+    std::cout << "Maximum distance in the SDF: " << sdf_extrema.first << ", minimum distance in the SDF: " << sdf_extrema.second << std::endl;
+    // We lock the SDF to prevent unintended changes that would invalidate it
+    sdf.Lock();
+    // Let's get some values
+    std::pair<float, bool> index_sdf_query = sdf.GetSafe(x_index, y_index, z_index);
+    std::cout << "Index query result - stored distance " << index_sdf_query.first << " was it in the grid? - " << index_sdf_query.second << std::endl;
+    std::pair<float, bool> location_sdf_query = sdf.GetSafe(x_location, y_location, z_location);
+    std::cout << "Location query result - stored distance " << location_sdf_query.first << " was it in the grid? - " << location_sdf_query.second << std::endl;
+    // Let's get some gradients
+    std::vector<double> index_gradient_query = sdf.GetGradient(x_index, y_index, z_index, true); // Usually, you want to enable 'edge gradients' i.e. gradients for cells on the edge of the grid that don't have 6 neighbors
+    std::cout << "Index gradient query result - gradient " << PrettyPrint::PrettyPrint(index_gradient_query) << std::endl;
+    std::vector<double> location_gradient_query = sdf.GetGradient(x_location, y_location, z_location, true); // Usually, you want to enable 'edge gradients' i.e. gradients for cells on the edge of the grid that don't have 6 neighbors
+    std::cout << "Location gradient query result - gradient " << PrettyPrint::PrettyPrint(location_gradient_query) << std::endl;
+    // Let's display the results to Rviz
+    visualization_msgs::Marker sdf_marker = sdf.ExportForDisplay(0.5); // Set the alpha for display
+    sdf_marker.ns = "sdf";
+    sdf_marker.id = 1;
+    visualization_pub.publish(sdf_marker);
+    // Let's export the SDF
+    sdf_pub.publish(sdf.GetMessageRepresentation());
+    std::cout << "...done" << std::endl;
+    return 0;
+}

flightlib/third_party/sdf_tools/src/test_voxel_grid.cpp

@@ -0,0 +1,345 @@
+#include "arc_utilities/voxel_grid.hpp"
+#include "arc_utilities/pretty_print.hpp"
+#include "sdf_tools/collision_map.hpp"
+#include "arc_utilities/dynamic_spatial_hashed_voxel_grid.hpp"
+#include "sdf_tools/dynamic_spatial_hashed_collision_map.hpp"
+#include "sdf_tools/sdf.hpp"
+#include "ros/ros.h"
+#include "visualization_msgs/MarkerArray.h"
+#include <chrono>
+#include <random>
+
+void test_voxel_grid_indices()
+{
+    VoxelGrid::VoxelGrid<int> test_grid(1.0, 20.0, 20.0, 20.0, 0);
+    // Load with special values
+    int check_val = 1;
+    std::vector<int> check_vals;
+    for (int64_t x_index = 0; x_index < test_grid.GetNumXCells(); x_index++)
+    {
+        for (int64_t y_index = 0; y_index < test_grid.GetNumYCells(); y_index++)
+        {
+            for (int64_t z_index = 0; z_index < test_grid.GetNumZCells(); z_index++)
+            {
+                test_grid.SetValue(x_index, y_index, z_index, check_val);
+                check_vals.push_back(check_val);
+                check_val++;
+            }
+        }
+    }
+    // Check the values
+    int check_index = 0;
+    bool pass = true;
+    for (int64_t x_index = 0; x_index < test_grid.GetNumXCells(); x_index++)
+    {
+        for (int64_t y_index = 0; y_index < test_grid.GetNumYCells(); y_index++)
+        {
+            for (int64_t z_index = 0; z_index < test_grid.GetNumZCells(); z_index++)
+            {
+                int ref_val = test_grid.GetImmutable(x_index, y_index, z_index).first;
+                //std::cout << "Value in grid: " << ref_val << " Value should be: " << check_vals[check_index] << std::endl;
+                if (ref_val == check_vals[check_index])
+                {
+                    //std::cout << "Check pass" << std::endl;
+                }
+                else
+                {
+                    std::cout << "Check fail" << std::endl;
+                    pass = false;
+                }
+                check_index++;
+            }
+        }
+    }
+    if (pass)
+    {
+        std::cout << "VG-I - All checks pass" << std::endl;
+    }
+    else
+    {
+        std::cout << "*** VG-I - Checks failed ***" << std::endl;
+    }
+}
+
+void test_voxel_grid_locations()
+{
+    VoxelGrid::VoxelGrid<int> test_grid(1.0, 20.0, 20.0, 20.0, 0);
+    // Load with special values
+    int check_val = 1;
+    std::vector<int> check_vals;
+    for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0)
+    {
+        for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0)
+        {
+            for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0)
+            {
+                test_grid.SetValue(x_pos, y_pos, z_pos, check_val);
+                check_vals.push_back(check_val);
+                check_val++;
+            }
+        }
+    }
+    // Check the values
+    int check_index = 0;
+    bool pass = true;
+    for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0)
+    {
+        for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0)
+        {
+            for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0)
+            {
+                int ref_val = test_grid.GetImmutable(x_pos, y_pos, z_pos).first;
+                //std::cout << "Value in grid: " << ref_val << " Value should be: " << check_vals[check_index] << std::endl;
+                if (ref_val == check_vals[check_index])
+                {
+                    //std::cout << "Value check pass" << std::endl;
+                }
+                else
+                {
+                    std::cout << "Value check fail" << std::endl;
+                    pass = false;
+                }
+                check_index++;
+                std::vector<double> query_point = {x_pos, y_pos, z_pos};
+                //std::cout << "Query point - " << PrettyPrint::PrettyPrint(query_point) << std::endl;
+                std::vector<int64_t> query_index = test_grid.LocationToGridIndex(x_pos, y_pos, z_pos);
+                //std::cout << "Query index - " << PrettyPrint::PrettyPrint(query_index) << std::endl;
+                std::vector<double> query_location = test_grid.GridIndexToLocation(query_index[0], query_index[1], query_index[2]);
+                //std::cout << "Query location - " << PrettyPrint::PrettyPrint(query_location) << std::endl;
+                std::vector<int64_t> found_query_index = test_grid.LocationToGridIndex(query_location[0], query_location[1], query_location[2]);
+                //std::cout << "Found query index - " << PrettyPrint::PrettyPrint(found_query_index) << std::endl;
+                if (query_point[0] == query_location[0] && query_point[1] == query_location[1] && query_point[2] == query_location[2])
+                {
+                    //std::cout << "Position check pass" << std::endl;
+                }
+                else
+                {
+                    std::cout << "Position check fail" << std::endl;
+                    pass = false;
+                }
+                if (query_index[0] == found_query_index[0] && query_index[1] == found_query_index[1] && query_index[2] == found_query_index[2])
+                {
+                    //std::cout << "Position index check pass" << std::endl;
+                }
+                else
+                {
+                    std::cout << "Position index check fail" << std::endl;
+                    pass = false;
+                }
+            }
+        }
+    }
+    if (pass)
+    {
+        std::cout << "VG-L - All checks pass" << std::endl;
+    }
+    else
+    {
+        std::cout << "*** VG-L - Checks failed ***" << std::endl;
+    }
+}
+
+void test_voxel_grid_serialization()
+{
+    VoxelGrid::VoxelGrid<int> test_grid(1.0, 20.0, 20.0, 20.0, 0);
+    // Load with special values
+    int check_val = 1;
+    std::vector<int> check_vals;
+    for (int64_t x_index = 0; x_index < test_grid.GetNumXCells(); x_index++)
+    {
+        for (int64_t y_index = 0; y_index < test_grid.GetNumYCells(); y_index++)
+        {
+            for (int64_t z_index = 0; z_index < test_grid.GetNumZCells(); z_index++)
+            {
+                test_grid.SetValue(x_index, y_index, z_index, check_val);
+                check_vals.push_back(check_val);
+                check_val++;
+            }
+        }
+    }
+    std::vector<uint8_t> buffer;
+    VoxelGrid::VoxelGrid<int>::Serialize(test_grid, buffer, arc_utilities::SerializeFixedSizePOD<int>);
+    const VoxelGrid::VoxelGrid<int> read_grid = VoxelGrid::VoxelGrid<int>::Deserialize(buffer, 0, arc_utilities::DeserializeFixedSizePOD<int>).first;
+    // Check the values
+    int check_index = 0;
+    bool pass = true;
+    for (int64_t x_index = 0; x_index < read_grid.GetNumXCells(); x_index++)
+    {
+        for (int64_t y_index = 0; y_index < read_grid.GetNumYCells(); y_index++)
+        {
+            for (int64_t z_index = 0; z_index < read_grid.GetNumZCells(); z_index++)
+            {
+                int ref_val = read_grid.GetImmutable(x_index, y_index, z_index).first;
+                //std::cout << "Value in grid: " << ref_val << " Value should be: " << check_vals[check_index] << std::endl;
+                if (ref_val == check_vals[check_index])
+                {
+                    //std::cout << "Check pass" << std::endl;
+                }
+                else
+                {
+                    std::cout << "Check fail" << std::endl;
+                    pass = false;
+                }
+                check_index++;
+            }
+        }
+    }
+    if (pass)
+    {
+        std::cout << "VG-I de/serialize - All checks pass" << std::endl;
+    }
+    else
+    {
+        std::cout << "*** VG-I de/serialize - Checks failed ***" << std::endl;
+    }
+}
+
+void test_dsh_voxel_grid_locations()
+{
+    VoxelGrid::DynamicSpatialHashedVoxelGrid<int> test_grid(1.0, 4, 4, 4, 0);
+    // Load with special values
+    int check_val = 1;
+    std::vector<int> check_vals;
+    for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0)
+    {
+        for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0)
+        {
+            for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0)
+            {
+                test_grid.SetCellValue(x_pos, y_pos, z_pos, check_val);
+                check_vals.push_back(check_val);
+                check_val++;
+            }
+        }
+    }
+    // Check the values
+    int check_index = 0;
+    bool pass = true;
+    for (double x_pos = -9.5; x_pos <= 9.5; x_pos += 1.0)
+    {
+        for (double y_pos = -9.5; y_pos <= 9.5; y_pos += 1.0)
+        {
+            for (double z_pos = -9.5; z_pos <= 9.5; z_pos += 1.0)
+            {
+                int ref_val = test_grid.GetImmutable(x_pos, y_pos, z_pos).first;
+                //std::cout << "Value in grid: " << ref_val << " Value should be: " << check_vals[check_index] << std::endl;
+                if (ref_val == check_vals[check_index])
+                {
+                    //std::cout << "Value check pass" << std::endl;
+                }
+                else
+                {
+                    std::cout << "Value check fail" << std::endl;
+                    pass = false;
+                }
+                check_index++;
+            }
+        }
+    }
+    if (pass)
+    {
+        std::cout << "DSHVG - All checks pass" << std::endl;
+    }
+    else
+    {
+        std::cout << "*** DSHVG - Checks failed ***" << std::endl;
+    }
+}
+
+
+void test_float_binary_conversion(float test_val)
+{
+    std::cout << "Initial value " << test_val << std::endl;
+    std::vector<uint8_t> binary_value = sdf_tools::FloatToBinary(test_val);
+    float final_val = sdf_tools::FloatFromBinary(binary_value);
+    std::cout << "Final value " << final_val << std::endl;
+}
+
+Eigen::Vector3d get_random_location(std::default_random_engine& generator, const double min_x, const double min_y, const double min_z, const double max_x, const double max_y, const double max_z)
+{
+    std::uniform_real_distribution<double> x_distribution(min_x, max_x);
+    std::uniform_real_distribution<double> y_distribution(min_y, max_y);
+    std::uniform_real_distribution<double> z_distribution(min_z, max_z);
+    double rand_x = x_distribution(generator);
+    double rand_y = y_distribution(generator);
+    double rand_z = z_distribution(generator);
+    return Eigen::Vector3d(rand_x, rand_y, rand_z);
+}
+
+bool get_random_bool(std::default_random_engine& generator)
+{
+    std::uniform_int_distribution<int> distribution(0,1);
+    int rand_int = distribution(generator);
+    if (rand_int)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+visualization_msgs::MarkerArray test_dsh_collision_map(std::default_random_engine& generator)
+{
+    sdf_tools::COLLISION_CELL default_cell(0.0);
+    sdf_tools::COLLISION_CELL filled_cell(1.0);
+    sdf_tools::DynamicSpatialHashedCollisionMapGrid test_col_map("test_voxel_grid", 1.0, 5, 5, 5, default_cell);
+    // Add a bunch of random data
+    for (int idx = 0; idx < 500; idx++)
+    {
+        // Get a random location in +-10m
+        Eigen::Vector3d random_location = get_random_location(generator, -20.0, -20.0, -20.0, 20.0, 20.0, 20.0);
+        // Get a random bool to choose between cell/chunk
+        bool use_cell = get_random_bool(generator);
+        // Update the col map
+        if (use_cell)
+        {
+            test_col_map.SetCell(random_location, filled_cell);
+        }
+        else
+        {
+            test_col_map.SetChunk(random_location, filled_cell);
+        }
+    }
+    // Get the Rviz markers
+    std_msgs::ColorRGBA filled_color;
+    filled_color.a = 1.0;
+    filled_color.b = 0.0;
+    filled_color.g = 0.0;
+    filled_color.r = 1.0;
+    std_msgs::ColorRGBA free_color;
+    free_color.a = 0.1;
+    free_color.b = 0.0;
+    free_color.g = 1.0;
+    free_color.r = 0.0;
+    std_msgs::ColorRGBA unknown_color;
+    unknown_color.a = 0.5;
+    unknown_color.b = 1.0;
+    unknown_color.g = 0.0;
+    unknown_color.r = 0.0;
+    std::vector<visualization_msgs::Marker> display_markers = test_col_map.ExportForDisplay(filled_color, free_color, unknown_color);
+    visualization_msgs::MarkerArray display_rep;
+    display_rep.markers = display_markers;
+    return display_rep;
+}
+
+
+int main(int argc, char** argv)
+{
+    // construct a trivial random generator engine from a time-based seed:
+    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
+    std::default_random_engine generator(seed);
+    ros::init(argc, argv, "test_voxel_grid");
+    ros::NodeHandle nh;
+    ros::Publisher display_pub = nh.advertise<visualization_msgs::MarkerArray>("display_test_voxel_grid", 1, true);
+    test_voxel_grid_indices();
+    test_voxel_grid_locations();
+    test_voxel_grid_serialization();
+    test_dsh_voxel_grid_locations();
+    test_float_binary_conversion(5280.0);
+    visualization_msgs::MarkerArray display_rep = test_dsh_collision_map(generator);
+    display_pub.publish(display_rep);
+    ros::spin();
+    return 0;
+}

flightlib/third_party/sdf_tools/srv/ComputeSDF.srv

@@ -0,0 +1,4 @@
+bool request_new
+---
+bool is_valid
+sdf_tools/SDF sdf