Unverified Commit 474d0706 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #997 from jcarpent/topic/fcl

Add serialization of pinocchio::Tensor
parents 9f365394 1f381757
......@@ -20,9 +20,11 @@
struct array
{
EIGEN_DEVICE_FUNC
EIGEN_STRONG_INLINE T& operator[] (size_t index) { return values[index]; }
EIGEN_STRONG_INLINE T& operator[] (size_t index)
{ return values[index]; }
EIGEN_DEVICE_FUNC
EIGEN_STRONG_INLINE const T& operator[] (size_t index) const { return values[index]; }
EIGEN_STRONG_INLINE const T& operator[] (size_t index) const
{ return values[index]; }
EIGEN_DEVICE_FUNC
EIGEN_STRONG_INLINE T& front() { return values[0]; }
......@@ -39,6 +41,23 @@
T values[n];
};
template<class T, std::size_t n>
EIGEN_DEVICE_FUNC bool operator==(const array<T,n> & lhs, const array<T,n> & rhs)
{
for (std::size_t i = 0; i < n; ++i) {
if (lhs[i] != rhs[i]) {
return false;
}
}
return true;
}
template<class T, std::size_t n>
EIGEN_DEVICE_FUNC bool operator!=(const array<T,n> & lhs, const array<T,n> & rhs)
{
return !(lhs == rhs);
}
} // namespace Eigen
#else
#include <array>
......@@ -66,10 +85,16 @@ namespace pinocchio
NumIndices = NumIndices_
};
typedef IndexType Index;
typedef Eigen::array<Index,NumIndices_> Dimensions;
inline Tensor& base() { return *this; }
inline const Tensor& base() const { return *this; }
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
Dimensions& dimensions() { return m_dimensions; }
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
const Dimensions& dimensions() const { return m_dimensions; }
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rank() const
{
return NumIndices;
......@@ -116,6 +141,17 @@ namespace pinocchio
return *this;
}
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor()
: m_storage()
{
}
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Tensor(const Tensor & other)
: m_storage(other.m_storage)
, m_dimensions(other.m_dimensions)
{
}
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit Tensor(Index dim1)
: m_storage(dim1)
{
......@@ -217,12 +253,22 @@ namespace pinocchio
#endif
}
EIGEN_DEVICE_FUNC bool operator==(const Tensor & other) const
{
return m_storage == other.m_storage;
}
EIGEN_DEVICE_FUNC bool operator!=(const Tensor & other) const
{
return m_storage != other.m_storage;
}
protected:
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> StorageType;
StorageType m_storage;
Index m_dimensions[NumIndices];
Dimensions m_dimensions;
};
......
......@@ -25,11 +25,12 @@
THE SOFTWARE.
*/
#ifndef __pinocchio_serialization_eigen_matrix_hpp__
#define __pinocchio_serialization_eigen_matrix_hpp__
#include <Eigen/Dense>
#include "pinocchio/math/tensor.hpp"
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>
......@@ -63,6 +64,74 @@ namespace boost
{
split_free(ar,m,version);
}
template <class Archive, typename _IndexType, std::size_t _NumIndices>
void save(Archive & ar, const Eigen::array<_IndexType,_NumIndices> & a, const unsigned int /*version*/)
{
ar & make_nvp("array",make_array(&a.front(),_NumIndices));
}
template <class Archive, typename _IndexType, std::size_t _NumIndices>
void load(Archive & ar, Eigen::array<_IndexType,_NumIndices> & a, const unsigned int /*version*/)
{
ar >> make_nvp("array",make_array(&a.front(),_NumIndices));
}
template <class Archive, typename _IndexType, std::size_t _NumIndices>
void serialize(Archive & ar, Eigen::array<_IndexType,_NumIndices> & a, const unsigned int version)
{
split_free(ar,a,version);
}
#ifdef PINOCCHIO_WITH_EIGEN_TENSOR_MODULE
template <class Archive, typename _IndexType, int _NumIndices>
void save(Archive & ar, const Eigen::DSizes<_IndexType,_NumIndices> & ds, const unsigned int version)
{
save(ar,static_cast<const Eigen::array<_IndexType,_NumIndices> &>(ds),version);
}
template <class Archive, typename _IndexType, int _NumIndices>
void load(Archive & ar, Eigen::DSizes<_IndexType,_NumIndices> & ds, const unsigned int version)
{
load(ar,static_cast<Eigen::array<_IndexType,_NumIndices> &>(ds),version);
}
template <class Archive, typename _IndexType, int _NumIndices>
void serialize(Archive & ar, Eigen::DSizes<_IndexType,_NumIndices> & ds, const unsigned int version)
{
split_free(ar,static_cast<Eigen::array<_IndexType,_NumIndices> &>(ds),version);
}
#endif
template <class Archive, typename _Scalar, int _NumIndices, int _Options, typename _IndexType>
void save(Archive & ar, const ::pinocchio::Tensor<_Scalar,_NumIndices,_Options,_IndexType> & t, const unsigned int /*version*/)
{
typedef ::pinocchio::Tensor<_Scalar,_NumIndices,_Options,_IndexType> Tensor;
const typename Tensor::Dimensions & dimensions = t.dimensions();
ar & BOOST_SERIALIZATION_NVP(dimensions);
ar & make_nvp("data",make_array(t.data(), (size_t)t.size()));
}
template <class Archive, typename _Scalar, int _NumIndices, int _Options, typename _IndexType>
void load(Archive & ar, ::pinocchio::Tensor<_Scalar,_NumIndices,_Options,_IndexType> & t, const unsigned int /*version*/)
{
typedef ::pinocchio::Tensor<_Scalar,_NumIndices,_Options,_IndexType> Tensor;
typename Tensor::Dimensions dimensions;
ar >> BOOST_SERIALIZATION_NVP(dimensions);
t.resize(dimensions);
ar >> make_nvp("data",make_array(t.data(), (size_t)t.size()));
}
template <class Archive, typename _Scalar, int _NumIndices, int _Options, typename _IndexType>
void serialize(Archive & ar, ::pinocchio::Tensor<_Scalar,_NumIndices,_Options,_IndexType> & t, const unsigned int version)
{
split_free(ar,t,version);
}
}
}
......
......@@ -3,6 +3,8 @@
//
#include "pinocchio/serialization/archive.hpp"
#include "pinocchio/serialization/eigen.hpp"
#include "pinocchio/serialization/spatial.hpp"
#include "pinocchio/serialization/frame.hpp"
......@@ -19,6 +21,38 @@
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
template<typename T1, typename T2 = T1>
struct call_equality_op
{
static bool run(const T1 & v1, const T2 & v2)
{
return v1 == v2;
}
};
template<typename T>
bool run_call_equality_op(const T & v1, const T & v2)
{
return call_equality_op<T,T>::run(v1,v2);
}
// Bug fix in Eigen::Tensor
#ifdef PINOCCHIO_WITH_EIGEN_TENSOR_MODULE
template<typename Scalar, int NumIndices, int Options, typename IndexType>
struct call_equality_op< pinocchio::Tensor<Scalar,NumIndices,Options,IndexType> >
{
typedef pinocchio::Tensor<Scalar,NumIndices,Options,IndexType> T;
static bool run(const T & v1, const T & v2)
{
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,Options> VectorXd;
Eigen::Map<const VectorXd> map1(v1.data(),v1.size(),1);
Eigen::Map<const VectorXd> map2(v2.data(),v2.size(),1);
return map1 == map2;
}
};
#endif
template<typename T>
void generic_test(const T & object,
const std::string & filename,
......@@ -35,7 +69,7 @@ void generic_test(const T & object,
loadFromText(object_loaded,txt_filename);
// Check
BOOST_CHECK(object_loaded == object);
BOOST_CHECK(run_call_equality_op(object_loaded,object));
}
// Load and save as string stream
......@@ -48,7 +82,7 @@ void generic_test(const T & object,
loadFromStringStream(object_loaded,is);
// Check
BOOST_CHECK(object_loaded == object);
BOOST_CHECK(run_call_equality_op(object_loaded,object));
}
// Load and save as string
......@@ -60,7 +94,7 @@ void generic_test(const T & object,
loadFromString(object_loaded,str_in);
// Check
BOOST_CHECK(object_loaded == object);
BOOST_CHECK(run_call_equality_op(object_loaded,object));
}
// Load and save as XML
......@@ -72,7 +106,7 @@ void generic_test(const T & object,
loadFromXML(object_loaded,xml_filename,tag_name);
// Check
BOOST_CHECK(object_loaded == object);
BOOST_CHECK(run_call_equality_op(object_loaded,object));
}
// Load and save as binary
......@@ -84,10 +118,39 @@ void generic_test(const T & object,
loadFromBinary(object_loaded,bin_filename);
// Check
BOOST_CHECK(object_loaded == object);
BOOST_CHECK(run_call_equality_op(object_loaded,object));
}
}
BOOST_AUTO_TEST_CASE(test_eigen_serialization)
{
using namespace pinocchio;
const Eigen::DenseIndex num_cols = 10;
const Eigen::DenseIndex num_rows = 20;
const Eigen::DenseIndex array_size = 3;
Eigen::MatrixXd Mat = Eigen::MatrixXd::Random(num_rows,num_cols);
generic_test(Mat,TEST_SERIALIZATION_FOLDER"/eigen_matrix","matrix");
Eigen::VectorXd Vec = Eigen::VectorXd::Random(num_rows*num_cols);
generic_test(Vec,TEST_SERIALIZATION_FOLDER"/eigen_vector","vector");
Eigen::array<Eigen::DenseIndex,array_size> array = { 1, 2, 3 };
generic_test(array,TEST_SERIALIZATION_FOLDER"/eigen_array","array");
const Eigen::DenseIndex tensor_size = 3;
const Eigen::DenseIndex x_dim = 10, y_dim = 20, z_dim = 30;
typedef pinocchio::Tensor<double,tensor_size> Tensor3x;
Tensor3x tensor(x_dim,y_dim,z_dim);
Eigen::Map<Eigen::VectorXd>(tensor.data(),tensor.size(),1).setRandom();
generic_test(tensor,TEST_SERIALIZATION_FOLDER"/eigen_tensor","tensor");
}
BOOST_AUTO_TEST_CASE(test_spatial_serialization)
{
using namespace pinocchio;
......@@ -292,7 +355,6 @@ BOOST_AUTO_TEST_CASE(test_throw_extension)
std::invalid_argument);
}
}
BOOST_AUTO_TEST_SUITE_END()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment