Merge branch 'shapes-json' into 'master'

Add JSON serialization for AABB and OB

See merge request Simox/simox!48

SimoxUtility/CMakeLists.txt

@@ -68,6 +68,7 @@ SET(SOURCES
     json/eigen_conversion.cpp
     json/io.cpp
     json/converters.cpp
+    json/util.cpp
 
     math/pose/align_box_orientation.cpp
     math/pose/check_rotation_matrix.cpp
@@ -80,6 +81,7 @@ SET(SOURCES
     math/SoftMinMax.cpp
 
     shapes/AxisAlignedBoundingBox.cpp
+    shapes/json_conversions.cpp
 )
 
 SET(INCLUDES
@@ -121,6 +123,7 @@ SET(INCLUDES
     json/eigen_conversion.h
     json/io.h
     json/json.hpp
+    json/util.h
 
     math/scale_value.h
 
@@ -234,6 +237,7 @@ SET(INCLUDES
     shapes/OrientedBoxBase.h
     shapes/OrientedBox.h
     shapes/XYConstrainedOrientedBox.h
+    shapes/json_conversions.h
 
     backport/span/gcc_header.h
 )

SimoxUtility/json.h

@@ -5,3 +5,4 @@
 #include "json/converters.h"
 #include "json/eigen_conversion.h"
 #include "json/io.h"
+#include "json/util.h"

SimoxUtility/json/util.cpp

+#include "util.h"
+
+#include <sstream>
+
+
+namespace simox
+{
+    nlohmann::json::const_reference json::at_any_key(const nlohmann::json& j, const std::vector<std::string>& keys)
+    {
+        for (const std::string& key : keys)
+        {
+            if (j.count(key))
+            {
+                return j.at(key);
+            }
+        }
+
+        std::stringstream ss;
+        ss << "None of these keys found in JSON document: \n";
+        for (const auto& k : keys)
+        {
+            ss << "- '" << k << "'\n";
+        }
+        throw std::out_of_range(ss.str());
+    }
+}
+

SimoxUtility/json/util.h

+#pragma once
+
+#include <optional>
+
+#include "json.hpp"
+
+
+namespace simox::json
+{
+
+    /**
+     * @brief Get the value at the first key in `keys` contained in `j`.
+     * @param j The JSON object.
+     * @param keys The keys.
+     * @return The value at the first contained key.
+     *
+     * @throw std::out_of_range If none of the keys is found in `j`.
+     */
+    nlohmann::json::const_reference at_any_key(const nlohmann::json& j, const std::vector<std::string>& keys);
+
+    template <class T>
+    T get_at_any_key(const nlohmann::json& j, const std::vector<std::string>& keys)
+    {
+        return at_any_key(j, keys).get<T>();
+    }
+
+}

SimoxUtility/shapes.h

@@ -9,3 +9,4 @@
 #include "shapes/OrientedEllipsis.h"
 #include "shapes/OrientedEllipsisBase.h"
 #include "shapes/XYConstrainedOrientedBox.h"
+#include "shapes/json_conversions.h"

SimoxUtility/shapes/AxisAlignedBoundingBox.cpp

 #include "AxisAlignedBoundingBox.h"
 
+#include <SimoxUtility/error/SimoxError.h>
+
 
 namespace simox
 {
@@ -35,6 +37,23 @@ namespace simox
     {
     }
 
+
+    AxisAlignedBoundingBox AxisAlignedBoundingBox::from_points(const std::vector<Eigen::Vector3f>& points)
+    {
+        if (points.empty())
+        {
+            throwSimoxError("Points must not be empty in `AxisAlignedBoundingBox::from_points().`");
+        }
+
+        AxisAlignedBoundingBox aabb(points.front(), points.front());
+        for (const auto& p : points)
+        {
+            aabb.expand_to(p);
+        }
+        return aabb;
+    }
+
+
     Eigen::Matrix32f AxisAlignedBoundingBox::limits() const
     {
         return _limits;
@@ -295,6 +314,11 @@ namespace simox
         return { min().cwiseMin(point), max().cwiseMax(point) };
     }
 
+    void AxisAlignedBoundingBox::throwSimoxError(const std::string& msg)
+    {
+        throw simox::error::SimoxError(msg);
+    }
+
 }
 
 std::ostream& simox::operator<<(std::ostream& os, const AxisAlignedBoundingBox rhs)

SimoxUtility/shapes/AxisAlignedBoundingBox.h

@@ -29,6 +29,19 @@ namespace simox
         /// Construct an AABB with limits in a 3x2 matrix.
         AxisAlignedBoundingBox(const Eigen::Matrix32f& limits);
 
+        /**
+         * @brief Compute the AABB of the given points.
+         * @throws `simox::error::SimoxError` If `points` is empty.
+         */
+        static AxisAlignedBoundingBox from_points(const std::vector<Eigen::Vector3f>& points);
+        /**
+         * @brief Compute the AABB of the given points.
+         * `PointT` must have x, y, z member variables.
+         * @throws `simox::error::SimoxError` If `points` is empty.
+         */
+        template <class PointT>
+        static AxisAlignedBoundingBox from_points(const std::vector<PointT>& points);
+
 
         Eigen::Matrix32f limits() const;
         Eigen::Matrix32f& limits();
@@ -97,23 +110,41 @@ namespace simox
         template <class PointT>
         bool is_inside(const PointT& p) const;
 
-        /// Expand `*this` (in-place) to include `point`.
+        /**
+         * @brief Expand `*this` (in-place) to include `point`.
+         * Note that `*this` should be suitable initialized beforehand.
+         * @see from_points()
+         */
         template <class PointT>
-        void expand_to(const PointT& p)
+        void expand_to(const PointT& point)
         {
-            expand_to(Eigen::Vector3f(p.x, p.y, p.z));
+            expand_to(Eigen::Vector3f(point.x, point.y, point.z));
         }
         void expand_to(const Eigen::Vector3f& point);
 
-        /// Return this AABB expanded to include `point`.
+        /**
+         * @brief Return this AABB expanded to include `point`.
+         * Note that `*this` should be suitable initialized beforehand.
+         * @see from_points()
+         */
         template <class PointT>
-        void expanded_to(const PointT& p) const
+        void expanded_to(const PointT& point) const
         {
-            expanded_to(Eigen::Vector3f(p.x, p.y, p.z));
+            expanded_to(Eigen::Vector3f(point.x, point.y, point.z));
         }
         AxisAlignedBoundingBox expanded_to(const Eigen::Vector3f& point) const;
 
 
+    private:
+
+        /**
+         * @brief Throw a simox::error::SimoxError() with the given message.
+         * This avoids exposing the include in the header.
+         */
+        [[noreturn]]
+        static void throwSimoxError(const std::string& msg);
+
+
     private:
 
         /**
@@ -136,6 +167,16 @@ namespace simox
 
 namespace aabb
 {
+    template <class PointT>
+    AxisAlignedBoundingBox from_points(const std::vector<PointT>& points)
+    {
+        return AxisAlignedBoundingBox::from_points<PointT>(points);
+    }
+    inline AxisAlignedBoundingBox from_points(const std::vector<Eigen::Vector3f>& points)
+    {
+        return AxisAlignedBoundingBox::from_points(points);
+    }
+
     /// Return the distance between center of `lhs`'s and `rhs`'s centers.
     float central_distance(const AxisAlignedBoundingBox& lhs, const AxisAlignedBoundingBox& rhs);
 
@@ -158,6 +199,23 @@ namespace aabb
     }
 }
 
+    template <class PointT>
+    AxisAlignedBoundingBox AxisAlignedBoundingBox::from_points(const std::vector<PointT>& points)
+    {
+        if (points.empty())
+        {
+            throwSimoxError("Points must not be empty in `AxisAlignedBoundingBox::from_points()`.");
+        }
+
+        const PointT& front = points.front();
+        AxisAlignedBoundingBox aabb(front.x, front.x, front.y, front.y, front.z, front.z);
+        for (const auto& p : points)
+        {
+            aabb.expand_to(p);
+        }
+        return aabb;
+    }
+
     template <class PointT>
     bool AxisAlignedBoundingBox::is_inside(const PointT& p) const
     {

SimoxUtility/shapes/OrientedBox.h

@@ -120,6 +120,26 @@ namespace simox
             this->_t.template block<3, 1>(0, 3) = corner;
         }
 
+
+        OrientedBox(const vector_t& center, const rotation_t& orientation, const vector_t& extents) :
+            OrientedBox(center - orientation * extents / 2,
+                        orientation.col(0) * extents(0),
+                        orientation.col(1) * extents(1),
+                        orientation.col(2) * extents(2))
+        {
+        }
+
+        OrientedBox(const transform_t& center_pose, const vector_t& extents) :
+            OrientedBox(center_pose.template block<3, 1>(0, 3).eval(), center_pose.template block<3, 3>(0, 0).eval(), extents)
+        {
+        }
+
+        OrientedBox(const vector_t& center, const Eigen::Quaternion<FloatT>& ori, const vector_t& extents) :
+            OrientedBox(center, ori.toRotationMatrix(), extents)
+        {
+        }
+
+
         OrientedBox(const base& b) : base(b) {}
         template<class T>
         OrientedBox<T> cast() const
@@ -133,6 +153,7 @@ namespace simox
             };
         }
 
+
         OrientedBox transformed(const rotation_t& t) const
         {
             return

SimoxUtility/shapes/json_conversions.cpp

+#include "json_conversions.h"
+
+#include <SimoxUtility/json.h>
+#include <SimoxUtility/json/util.h>
+
+#include "AxisAlignedBoundingBox.h"
+#include "OrientedBox.h"
+
+
+void simox::to_json(nlohmann::json& j, const simox::AxisAlignedBoundingBox& aabb)
+{
+    j["center"] = aabb.center();
+    j["extents"] = aabb.extents();
+    j["min"] = aabb.min();
+    j["max"] = aabb.max();
+}
+
+void simox::from_json(const nlohmann::json& j, simox::AxisAlignedBoundingBox& aabb)
+{
+    if (j.count("min") && j.count("max"))
+    {
+        auto min = j.at("min").get<Eigen::Vector3f>();
+        auto max = j.at("max").get<Eigen::Vector3f>();
+
+        aabb.set_min(min);
+        aabb.set_max(max);
+    }
+    else
+    {
+        Eigen::Vector3f center = simox::json::get_at_any_key<Eigen::Vector3f>(j, { "center", "pos", "position"});
+        Eigen::Vector3f extents = simox::json::get_at_any_key<Eigen::Vector3f>(j, { "extents", "dimensions" });
+
+        aabb.set_center(center);
+        aabb.set_extents(extents);
+    }
+}
+
+
+namespace simox::json
+{
+    template <typename Float>
+    void to_json(nlohmann::json& j, const OrientedBox<Float>& ob)
+    {
+        j["position"] = ob.center();
+        j["orientation"] = Eigen::Quaternion<Float>(ob.rotation());
+        j["extents"] = ob.dimensions();
+    }
+
+    template <typename Float>
+    void from_json(const nlohmann::json& j, OrientedBox<Float>& ob)
+    {
+        Eigen::Vector3d pos = simox::json::get_at_any_key<Eigen::Vector3d>(j, { "pos", "position" });
+        Eigen::Quaterniond quat = simox::json::get_at_any_key<Eigen::Quaterniond>(j, { "ori", "orientation" });
+        Eigen::Matrix3d ori = quat.toRotationMatrix();
+        Eigen::Vector3d extents = simox::json::get_at_any_key<Eigen::Vector3d>(j, { "extents", "dimensions" });
+
+        Eigen::Vector3d corner = pos - ori * extents / 2;
+
+        ob = OrientedBox<double>(corner,
+                                 ori.col(0) * extents(0),
+                                 ori.col(1) * extents(1),
+                                 ori.col(2) * extents(2)).cast<Float>();
+    }
+}
+
+void simox::to_json(nlohmann::json& j, const OrientedBox<float>& ob)
+{
+    simox::json::to_json<float>(j, ob);
+}
+void simox::from_json(const nlohmann::json& j, OrientedBox<float>& ob)
+{
+    simox::json::from_json<float>(j, ob);
+}
+
+void simox::to_json(nlohmann::json& j, const OrientedBox<double>& ob)
+{
+    simox::json::to_json<double>(j, ob);
+}
+void simox::from_json(const nlohmann::json& j, OrientedBox<double>& ob)
+{
+    simox::json::from_json<double>(j, ob);
+}
+
+
+

SimoxUtility/shapes/json_conversions.h

+#pragma once
+
+#include <SimoxUtility/json/json.hpp>
+
+
+namespace simox
+{
+    class AxisAlignedBoundingBox;
+    template<class FloatT>
+    class OrientedBox;
+
+
+    void to_json(nlohmann::json& j, const AxisAlignedBoundingBox& aabb);
+    void from_json(const nlohmann::json& j, AxisAlignedBoundingBox& aabb);
+
+    void to_json(nlohmann::json& j, const OrientedBox<float>& ob);
+    void from_json(const nlohmann::json& j, OrientedBox<float>& ob);
+
+    void to_json(nlohmann::json& j, const OrientedBox<double>& ob);
+    void from_json(const nlohmann::json& j, OrientedBox<double>& ob);
+
+}
+

SimoxUtility/tests/json/EigenConversionTest.cpp

@@ -160,4 +160,14 @@ BOOST_AUTO_TEST_CASE(test_Matrix4f_transform_from_pos_ori)
 }
 
 
+BOOST_AUTO_TEST_CASE(test_from_json_Vector3d)
+{
+    nlohmann::json j = nlohmann::json::parse("[ 100.0, -200.0, 0.0 ]");
+    Eigen::Vector3d v = j.get<Eigen::Vector3d>();
+    BOOST_CHECK_EQUAL(v.x(), 100);
+    BOOST_CHECK_EQUAL(v.y(), -200);
+    BOOST_CHECK_EQUAL(v.z(), 0);
+}
+
+
 BOOST_AUTO_TEST_SUITE_END()

SimoxUtility/tests/shapes/AABB_center_extents.json

+{
+  "center": [
+    45.0,
+    90.0,
+    135.0
+  ],
+  "extents": [
+    110.0,
+    220.0,
+    330.0
+  ]
+}

SimoxUtility/tests/shapes/AABB_min_max.json

+{
+  "min": [
+    -10.0,
+    -20.0,
+    -30.0
+  ],
+  "max": [
+    100.0,
+    200.0,
+    300.0
+  ]
+}

SimoxUtility/tests/shapes/AxisAlignedBoundingBoxTest.cpp

+/**
+* @package    SimoxUtility
+* @author     Raphael Grimm
+* @copyright  2019 Raphael Grimm
+*/
+
+#define BOOST_TEST_MODULE SimoxUtility/shapes/OrientedBoxTest
+
+#include <random>
+#include <iostream>
+
+#include <boost/test/included/unit_test.hpp>
+
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+
+
+namespace
+{
+    struct PointT
+    {
+        float x = 0, y = 0, z = 0;
+    };
+}
+
+
+BOOST_AUTO_TEST_CASE(test_AABB_from_points_eigen)
+{
+    std::vector<Eigen::Vector3f> points{ { 0, 0, 0 } , { -2, 0, 1 }, { 2, 0, -1 } };
+
+    simox::AxisAlignedBoundingBox aabb = simox::aabb::from_points(points);
+
+    BOOST_CHECK_EQUAL(aabb.min(), Eigen::Vector3f(-2, 0, -1));
+    BOOST_CHECK_EQUAL(aabb.max(), Eigen::Vector3f( 2, 0,  1));
+}
+
+BOOST_AUTO_TEST_CASE(test_AABB_from_points_custom)
+{
+    std::vector<PointT> points{ { 0, 0, 0 } , { -2, 0, 1 }, { 2, 0, -1 } };
+
+    simox::AxisAlignedBoundingBox aabb = simox::aabb::from_points(points);
+
+    BOOST_CHECK_EQUAL(aabb.min(), Eigen::Vector3f(-2, 0, -1));
+    BOOST_CHECK_EQUAL(aabb.max(), Eigen::Vector3f( 2, 0,  1));
+}

SimoxUtility/tests/shapes/CMakeLists.txt

 
+ADD_SU_TEST( AxisAlignedBoundingBoxTest )
+ADD_SU_TEST( JsonConversionsTest )
 ADD_SU_TEST( OrientedBoxTest )
 ADD_SU_TEST( XYConstrainedOrientedBoxTest )
+
+
+SET(TEST_FILES
+    AABB_center_extents.json
+    AABB_min_max.json
+    OrientedBox_pos_ori_dimensions.json
+    OrientedBox_position_orientation_extents.json
+)
+
+foreach (FILENAME IN LISTS TEST_FILES)
+    configure_file("${FILENAME}" "${Simox_TEST_DIR}/${FILENAME}" COPYONLY)
+endforeach()

SimoxUtility/tests/shapes/JsonConversionsTest.cpp

+/**
+* @package    SimoxUtility
+* @author     Raphael Grimm
+* @copyright  2019 Raphael Grimm
+*/
+
+#define BOOST_TEST_MODULE SimoxUtility/shapes/OrientedBoxTest
+
+#include <random>
+#include <iostream>
+
+#include <boost/test/included/unit_test.hpp>
+
+#include <filesystem>
+
+#include <SimoxUtility/shapes/json_conversions.h>
+#include <SimoxUtility/shapes/OrientedBox.h>
+#include <SimoxUtility/shapes/AxisAlignedBoundingBox.h>
+#include <SimoxUtility/json/io.h>
+
+
+#include <SimoxUtility/json/util.h>
+#include <SimoxUtility/json/eigen_conversion.h>
+
+
+namespace
+{
+    static const std::vector<std::string> JSON_AABB_FILENAMES = {
+        "AABB_center_extents.json",
+        "AABB_min_max.json",
+    };
+    static const std::vector<std::string> JSON_ORIENTED_BOX_FILENAMES = {
+        "OrientedBox_position_orientation_extents.json",
+        "OrientedBox_pos_ori_dimensions.json",
+    };
+
+    const float PRECF = 1e-3f;
+    const double PRECD = 1e-3;
+}
+
+#define BOOST_CHECK_EQUAL_VECTOR(lhs, rhs, prec) \
+    BOOST_TEST_INFO((lhs).transpose() << " == " << (rhs).transpose()); \
+    BOOST_CHECK_LE(((lhs) - (rhs)).norm(), prec);
+
+
+namespace
+{
+    template <typename Float>
+    void test_read_OrientedBox_type(const Float prec)
+    {
+        namespace fs = std::filesystem;
+
+        for (const std::string& filename : JSON_ORIENTED_BOX_FILENAMES)
+        {
+            BOOST_TEST_CONTEXT("File " << filename)
+            {
+                BOOST_REQUIRE(fs::is_regular_file(filename));
+
+                const nlohmann::json j = nlohmann::read_json(filename);
+
+                const simox::OrientedBox<float> ob = j.get<simox::OrientedBox<float>>();
+                BOOST_CHECK_EQUAL_VECTOR(ob.center(), Eigen::Vector3f(10, -20, 0), prec);
+                BOOST_CHECK_EQUAL_VECTOR(ob.rotation(), Eigen::Quaternionf(0.707f, 0, 0.707f, 0).toRotationMatrix(), prec);
+                BOOST_CHECK_EQUAL_VECTOR(ob.dimensions(), Eigen::Vector3f(100, 200, 300), prec * Float(1e2));
+            }
+        }
+    }
+}
+
+
+BOOST_AUTO_TEST_CASE(test_read_AABB)
+{
+    namespace fs = std::filesystem;
+    float prec = 1e-4f;
+
+    for (const std::string& filename : JSON_AABB_FILENAMES)
+    {
+        BOOST_TEST_CONTEXT("File " << filename)
+        {
+            BOOST_REQUIRE(fs::is_regular_file(filename));
+
+            const nlohmann::json j = nlohmann::read_json(filename);
+
+            const simox::AxisAlignedBoundingBox aabb = j.get<simox::AxisAlignedBoundingBox>();
+            BOOST_CHECK_EQUAL_VECTOR(aabb.center(), Eigen::Vector3f(45, 90, 135), prec);
+            BOOST_CHECK_EQUAL_VECTOR(aabb.extents(), Eigen::Vector3f(110, 220, 330), prec);
+            BOOST_CHECK_EQUAL_VECTOR(aabb.min(), Eigen::Vector3f(-10, -20, -30), prec);
+            BOOST_CHECK_EQUAL_VECTOR(aabb.max(), Eigen::Vector3f(100, 200, 300), prec);
+        }
+    }
+}
+
+
+BOOST_AUTO_TEST_CASE(test_read_OrientedBox_float)
+{
+    test_read_OrientedBox_type<float>(PRECF);
+}
+
+BOOST_AUTO_TEST_CASE(test_read_OrientedBox_double)
+{
+    test_read_OrientedBox_type<double>(PRECD);
+}

SimoxUtility/tests/shapes/OrientedBoxTest.cpp

@@ -12,6 +12,8 @@
 #include <boost/test/included/unit_test.hpp>
 
 #include <SimoxUtility/shapes/OrientedBox.h>
+#include <SimoxUtility/math/pose/pose.h>
+
 
 BOOST_AUTO_TEST_CASE(test_OrientedBox)
 {
@@ -82,3 +84,63 @@ BOOST_AUTO_TEST_CASE(test_OrientedBox)
               Eigen::Vector3d::Random().cwiseAbs() * 10);
     }
 }
+
+
+#define BOOST_CHECK_EQUAL_VECTOR(lhs, rhs, prec) \
+    BOOST_TEST_INFO((lhs).transpose() << " == " << (rhs).transpose()); \
+    BOOST_CHECK_LE(((lhs) - (rhs)).norm(), prec);
+
+
+namespace
+{
+    struct FromPosOriExtents
+    {
+        const double prec = 1e-4;
+
+        const Eigen::Vector3f pos {10, -20, 0};
+        const Eigen::Quaternionf quat = Eigen::Quaternionf(0.707f, 0, 0.707f, 0).normalized();
+        const Eigen::Matrix3f ori = quat.toRotationMatrix();
+        const Eigen::Vector3f extents{100, 200, 300};
+
+        void test_oriented_box(const simox::OrientedBox<float>& ob)
+        {
+            BOOST_CHECK_EQUAL_VECTOR(ob.center(), pos, prec);
+            BOOST_CHECK_EQUAL_VECTOR(ob.rotation(), ori, prec);
+            BOOST_CHECK_EQUAL_VECTOR(ob.dimensions(), extents, prec);
+        }
+    };
+}
+
+
+BOOST_FIXTURE_TEST_SUITE(TestFromPosOriExtents, FromPosOriExtents)
+
+BOOST_AUTO_TEST_CASE(test_OrientedBox_from_pos_ori_extents_by_corner_extent_vectors)
+{
+    const Eigen::Vector3f corner = pos - ori * extents / 2;
+    const simox::OrientedBox<float> ob(corner,
+                                        ori.col(0) * extents(0),
+                                        ori.col(1) * extents(1),
+                                        ori.col(2) * extents(2));
+    test_oriented_box(ob);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_OrientedBox_from_pos_quat_extents)
+{
+    simox::OrientedBox<float> ob(pos, quat, extents);
+    test_oriented_box(ob);
+}
+
+BOOST_AUTO_TEST_CASE(test_OrientedBox_from_pos_orimat_extents)
+{
+    simox::OrientedBox<float> ob(pos, ori, extents);
+    test_oriented_box(ob);
+}
+
+BOOST_AUTO_TEST_CASE(test_OrientedBox_from_pose_ori_extents)
+{
+    simox::OrientedBox<float> ob(simox::math::pose(pos, quat), extents);
+    test_oriented_box(ob);
+}
+
+BOOST_AUTO_TEST_SUITE_END()

SimoxUtility/tests/shapes/OrientedBox_pos_ori_dimensions.json

+{
+  "dimensions": [
+    100.0,
+    200.0,
+    300.0
+  ],
+  "ori": {
+    "qw": 0.707,
+    "qx": 0.0,
+    "qy": 0.707,
+    "qz": 0.0
+  },
+  "pos": [
+    10.0,
+    -20.0,
+    0.0
+  ]
+}

SimoxUtility/tests/shapes/OrientedBox_position_orientation_extents.json

+{
+  "extents": [
+    100.0,
+    200.0,
+    300.0
+  ],
+  "orientation": {
+    "qw": 0.707,
+    "qx": 0.0,
+    "qy": 0.707,
+    "qz": 0.0
+  },
+  "position": [
+    10.0,
+    -20.0,
+    0.0
+  ]
+}