diff --git a/VirtualRobot/Util/json/eigen_conversion.cpp b/VirtualRobot/Util/json/eigen_conversion.cpp
index afe96e578637bb4ca86e7879e44517265c5e7dfa..7486c73a184e06542533a363289ac4e9a6f10504 100644
--- a/VirtualRobot/Util/json/eigen_conversion.cpp
+++ b/VirtualRobot/Util/json/eigen_conversion.cpp
@@ -3,46 +3,40 @@
namespace Eigen
{
- template <>
- void to_json<Vector3f>(nlohmann::json& j, const MatrixBase<Vector3f>& vector)
- {
- j["x"] = vector.x();
- j["y"] = vector.y();
- j["z"] = vector.z();
- }
-
template <>
void from_json<Vector3f>(const nlohmann::json& j, MatrixBase<Vector3f>& vector)
{
- vector.x() = j.at("x").get<float>();
- vector.y() = j.at("y").get<float>();
- vector.z() = j.at("z").get<float>();
- }
-
-
- template <>
- void to_json<Matrix4f>(nlohmann::json& j, const MatrixBase<Matrix4f>& matrix)
- {
- // check lower row for [ 0 0 0 1 ]
- if (matrix.row(3).isApprox(Eigen::Vector4f(0, 0, 0, 1).transpose(), 1e-9f))
+ if (j.is_object())
{
- // must construct a Vector3f to trigger specialized version of to_json()
- j["pos"] = Eigen::Vector3f(math::Helpers::Position(matrix));
- j["ori"] = math::Helpers::Orientation(matrix);
+ vector.x() = j.at("x").get<float>();
+ vector.y() = j.at("y").get<float>();
+ vector.z() = j.at("z").get<float>();
}
- else
+ else // j.is_array()
{
- jsonbase::to_json(j, matrix);
+ jsonbase::from_json(j, vector);
}
}
+
template <>
void from_json<Matrix4f>(const nlohmann::json& j, MatrixBase<Matrix4f>& matrix)
{
- if (j.count("pos") > 0 && j.count("ori") > 0)
+ if (j.is_object())
{
- matrix = math::Helpers::Pose(j.at("pos").get<Eigen::Vector3f>(),
- j.at("ori").get<Eigen::Matrix3f>());
+ const auto& j_ori = j.at("ori");
+ Eigen::Matrix3f ori;
+
+ if (j_ori.is_object())
+ {
+ ori = j_ori.get<Eigen::Quaternionf>().toRotationMatrix();
+ }
+ else
+ {
+ ori = j_ori.get<Eigen::Matrix3f>();
+ }
+
+ matrix = math::Helpers::Pose(j.at("pos").get<Eigen::Vector3f>(), ori);
}
else
{
diff --git a/VirtualRobot/Util/json/eigen_conversion.h b/VirtualRobot/Util/json/eigen_conversion.h
index be04d1cf353ecfe2d1ba55a274ae0c24183909a6..4af202990214a068b6e8945ea87d863ebefb7274 100644
--- a/VirtualRobot/Util/json/eigen_conversion.h
+++ b/VirtualRobot/Util/json/eigen_conversion.h
@@ -10,8 +10,9 @@
#include "json.hpp"
+
/**
- * Provide to_json() and from_json() overloads for nlohmann::json,
+ * Provide `to_json()` and `from_json()` overloads for `nlohmann::json`,
* which allows simple syntax like:
*
* @code
@@ -30,7 +31,7 @@ namespace Eigen
{
- // MatrixBase (non-specialized)
+ // MatrixBase (non-specialized).
/// Writes the matrix as list of rows.
template <typename Derived>
@@ -43,44 +44,30 @@ namespace Eigen
// Specialization for Vector3f (implemented in .cpp)
- /// Writes vector with x, y, z keys.
- template <>
- void to_json<Vector3f>(nlohmann::json& j, const MatrixBase<Vector3f>& vector);
-
- /// Reads vector from x, y, z keys.
+ /// If `j` is an object, reads vector from `x, y, z` keys. Otherwise, reads it as matrix.
template <>
void from_json<Vector3f>(const nlohmann::json& j, MatrixBase<Vector3f>& vector);
- // Specialization for Matrix4f as Transformation matrix (implemented in .cpp)
-
- /**
- * @brief Write a 4x4 matrix, with optimization for transformation matrices.
- *
- * If the matrix is a transformation matrix, writes it as position
- * (Vector3f) and orientation (Matrix3f).
- * Otherwise, uses the non-specialized method (list of rows).
- */
- template <>
- void to_json<Matrix4f>(nlohmann::json& j, const MatrixBase<Matrix4f>& vector);
+ // Specialization for Matrix4f as transformation matrix (implemented in .cpp).
/**
- * @brief Reads a 4x4 matrix as written by to_json<Matrix4f>().
+ * @brief Reads a 4x4 matrix from list of rows or `pos` and `ori` keys.
*
- * Reads the matrix as transformation matrix from position and orientation
- * or from list of rows, depending on j's contents.
+ * If `j` is an object, reads `matrix` as transformation matrix from `pos` and `ori` keys.
+ * Otherweise, reads it from list of rows.
*/
template <>
- void from_json<Matrix4f>(const nlohmann::json& j, MatrixBase<Matrix4f>& vector);
+ void from_json<Matrix4f>(const nlohmann::json& j, MatrixBase<Matrix4f>& matrix);
// Quaternion
- /// Writes the quaternion with qw, qx, qy, qz keys.
+ /// Writes the quaternion with `qw, qx, qy, qz` keys.
template <typename Derived>
void to_json(nlohmann::json& j, const QuaternionBase<Derived>& quat);
- /// Reads the quaternion from qw, qx, qy, qz keys.
+ /// Reads the quaternion from `qw, qx, qy, qz` keys.
template <typename Derived>
void from_json(const nlohmann::json& j, QuaternionBase<Derived>& quat);
diff --git a/VirtualRobot/tests/VirtualRobotJsonEigenConversionTest.cpp b/VirtualRobot/tests/VirtualRobotJsonEigenConversionTest.cpp
index 9026027ef2e5111c2a22112ba8f3507686877de9..f362e6f5b89d18857c2605e5d2957314134ebc30 100644
--- a/VirtualRobot/tests/VirtualRobotJsonEigenConversionTest.cpp
+++ b/VirtualRobot/tests/VirtualRobotJsonEigenConversionTest.cpp
@@ -34,7 +34,8 @@ using namespace Eigen;
BOOST_AUTO_TEST_SUITE(VirtualRobotJsonEigenConversionTest)
-BOOST_AUTO_TEST_CASE(test_matrix4f_non_transform)
+
+BOOST_AUTO_TEST_CASE(test_Matrix4f_non_transform)
{
Matrix4f in = in.Identity(), out = out.Zero();
@@ -54,7 +55,7 @@ BOOST_AUTO_TEST_CASE(test_matrix4f_non_transform)
BOOST_CHECK_EQUAL(in, out);
}
-BOOST_AUTO_TEST_CASE(test_matrix4f_transform)
+BOOST_AUTO_TEST_CASE(test_Matrix4f_transform)
{
Matrix4f in = math::Helpers::Pose(Vector3f { 3, 2, 3 },
AngleAxisf( 1.2f, Eigen::Vector3f(1,2,3).normalized()));
@@ -72,7 +73,7 @@ BOOST_AUTO_TEST_CASE(test_matrix4f_transform)
}
-BOOST_AUTO_TEST_CASE(test_vector3f)
+BOOST_AUTO_TEST_CASE(test_Vector3f)
{
Vector3f in = in.Identity(), out = out.Zero();
@@ -93,7 +94,25 @@ BOOST_AUTO_TEST_CASE(test_vector3f)
}
-BOOST_AUTO_TEST_CASE(test_quaternionf)
+BOOST_AUTO_TEST_CASE(test_Vector3f_from_xyz)
+{
+ Eigen::Vector3f in(0, -1, 2.5), out;
+ nlohmann::json j;
+ j["x"] = in.x();
+ j["y"] = in.y();
+ j["z"] = in.z();
+
+ BOOST_TEST_MESSAGE("JSON: \n" << j.dump(2));
+
+ out = j;
+ BOOST_CHECK_EQUAL(in, out);
+
+ out = j.get<Eigen::Vector3f>();
+ BOOST_CHECK_EQUAL(in, out);
+}
+
+
+BOOST_AUTO_TEST_CASE(test_Quaternionf)
{
Quaternionf in { AngleAxisf(static_cast<float>(M_PI), Vector3f(1, 1, 1).normalized()) };
Quaternionf out = out.Identity();
@@ -109,4 +128,38 @@ BOOST_AUTO_TEST_CASE(test_quaternionf)
}
+BOOST_AUTO_TEST_CASE(test_Matrix4f_transform_from_pos_ori)
+{
+ Eigen::Vector3f pos(0, -1, 2.5);
+ Eigen::Quaternionf ori(AngleAxisf(static_cast<float>(M_PI_2), Eigen::Vector3f::UnitY()));
+ Matrix4f in = math::Helpers::Pose(pos, ori);
+ Matrix4f out = out.Zero();
+
+ // ori = Quaternion
+ nlohmann::json j;
+ j["pos"] = pos;
+ j["ori"] = ori;
+ BOOST_TEST_MESSAGE("JSON (ori = quat): \n" << j.dump(2));
+
+ out = j;
+ BOOST_CHECK_EQUAL(in, out);
+
+ out = j.get<Matrix4f>();
+ BOOST_CHECK_EQUAL(in, out);
+
+ // ori = Matrix
+ j = nlohmann::json();
+ j["pos"] = pos;
+ j["ori"] = ori.toRotationMatrix();
+
+ BOOST_TEST_MESSAGE("JSON (ori = mat): \n" << j.dump(2));
+
+ out = j;
+ BOOST_CHECK_EQUAL(in, out);
+
+ out = j.get<Matrix4f>();
+ BOOST_CHECK_EQUAL(in, out);
+}
+
+
BOOST_AUTO_TEST_SUITE_END()