Commit ed516141 authored by JasonChmn's avatar JasonChmn Committed by Pierre Fernbach
Browse files

Add serialization for bezier/cubic hermite/ all piecewise curves => Test OK

parent e5d08ed3
......@@ -14,6 +14,8 @@
#include "MathDefs.h"
#include "serialization/archive.hpp"
#include <math.h>
#include <vector>
#include <stdexcept>
......@@ -40,7 +42,10 @@ inline unsigned int bin(const unsigned int n, const unsigned int k)
/// \brief Computes a Bernstein polynome.
///
template <typename Numeric = double>
struct Bern{
struct Bern : public serialization::Serializable< Bern<Numeric> > {
Bern(){}
Bern(const unsigned int m, const unsigned int i)
:m_minus_i(m - i)
,i_(i)
......@@ -57,6 +62,16 @@ Numeric operator()(const Numeric u) const
Numeric m_minus_i;
Numeric i_;
Numeric bin_m_i_;
// Serialization of the class
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive& ar, const unsigned int version){
ar & boost::serialization::make_nvp("m_minus_i", m_minus_i);
ar & boost::serialization::make_nvp("i", i_);
ar & boost::serialization::make_nvp("bin_m_i", bin_m_i_);
}
};
/// \brief Computes all Bernstein polynomes for a certain degree.
......
......@@ -16,6 +16,9 @@
#include "MathDefs.h"
#include "serialization/archive.hpp"
#include "serialization/eigen-matrix.hpp"
#include <vector>
#include <stdexcept>
......@@ -30,7 +33,8 @@ namespace curves
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
, typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>
struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>,
public serialization::Serializable< bezier_curve<Time, Numeric, Dim, Safe, Point> >
{
typedef Point point_t;
typedef Time time_t;
......@@ -43,6 +47,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>
/* Constructors - destructors */
public:
bezier_curve(){}
/// \brief Constructor.
/// Given the first and last point of a control points set, create the bezier curve.
/// \param PointsBegin : an iterator pointing to the first element of a control point container.
......@@ -401,13 +407,27 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point>
/*const*/ t_point_t control_points_;
static const double MARGIN;
public:
static bezier_curve_t zero(const time_t T=1.)
{
std::vector<point_t> ts;
ts.push_back(point_t::Zero(Dim));
return bezier_curve_t(ts.begin(), ts.end(),0.,T);
}
// Serialization of the class
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive& ar, const unsigned int version){
ar & boost::serialization::make_nvp("T_min", T_min_);
ar & boost::serialization::make_nvp("T_max", T_max_);
ar & boost::serialization::make_nvp("mult_T", mult_T_);
ar & boost::serialization::make_nvp("size", size_);
ar & boost::serialization::make_nvp("degree", degree_);
ar & boost::serialization::make_nvp("bernstein", bernstein_);
ar & boost::serialization::make_nvp("control_points", control_points_);
}
};
template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point>
......
......@@ -18,6 +18,11 @@
#include <iostream>
#include "serialization/archive.hpp"
#include "serialization/eigen-matrix.hpp"
#include <boost/serialization/utility.hpp>
namespace curves
{
/// \class CubicHermiteSpline.
......@@ -32,7 +37,8 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
, typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1>
, typename Tangent= Eigen::Matrix<Numeric, Eigen::Dynamic, 1>
>
struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>
struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>,
public serialization::Serializable< cubic_hermite_spline<Time, Numeric, Dim, Safe, Point, Tangent> >
{
typedef std::pair<Point, Tangent> pair_point_tangent_t;
typedef std::vector< pair_point_tangent_t ,Eigen::aligned_allocator<Point> > t_pair_point_tangent_t;
......@@ -40,6 +46,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>
typedef Numeric num_t;
public:
cubic_hermite_spline(){}
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
/// \param wayPointsEns : an iterator pointing to the last element of a pair(position, derivative) container.
......@@ -66,7 +75,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>
cubic_hermite_spline(const cubic_hermite_spline& other)
: control_points_(other.control_points_), time_control_points_(other.time_control_points_),
duration_splines_(other.duration_splines_), t_min_(other.t_min_), t_max_(other.t_max_),
duration_splines_(other.duration_splines_), T_min_(other.T_min_), T_max_(other.T_max_),
size_(other.size_), degree_(other.degree_)
{}
......@@ -82,7 +91,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>
///
virtual Point operator()(const Time t) const
{
if(Safe &! (t_min_ <= t && t <= t_max_))
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::invalid_argument("can't evaluate cubic hermite spline, out of range");
}
......@@ -114,8 +123,8 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>
void setTime(const vector_time_t & time_control_points)
{
time_control_points_ = time_control_points;
t_min_ = time_control_points_.front();
t_max_ = time_control_points_.back();
T_min_ = time_control_points_.front();
T_max_ = time_control_points_.back();
if (time_control_points.size() != size())
{
throw std::length_error("size of time control points should be equal to number of control points");
......@@ -361,16 +370,30 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point>
/// duration of each subspline is : ( T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} )<br>
/// It contains \f$N-1\f$ durations.
vector_time_t duration_splines_;
/// Starting time of cubic hermite spline : t_min_ is equal to first time of control points.
/*const*/ Time t_min_;
/// Ending time of cubic hermite spline : t_max_ is equal to last time of control points.
/*const*/ Time t_max_;
/// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
/*const*/ Time T_min_;
/// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
/*const*/ Time T_max_;
/// Number of control points (pairs).
std::size_t size_;
/// Degree (Cubic so degree 3)
std::size_t degree_;
/*Attributes*/
// Serialization of the class
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive& ar, const unsigned int version){
ar & boost::serialization::make_nvp("control_points", control_points_);
ar & boost::serialization::make_nvp("time_control_points", time_control_points_);
ar & boost::serialization::make_nvp("duration_splines", duration_splines_);
ar & boost::serialization::make_nvp("T_min", T_min_);
ar & boost::serialization::make_nvp("T_max", T_max_);
ar & boost::serialization::make_nvp("size", size_);
ar & boost::serialization::make_nvp("degree", degree_);
}
};
} // namespace curve
......
......@@ -11,6 +11,8 @@
#include "curve_abc.h"
#include "curve_conversion.h"
#include "serialization/archive.hpp"
#include "serialization/eigen-matrix.hpp"
namespace curves
{
......
......@@ -50,7 +50,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point>,
/* Constructors - destructors */
public:
polynomial() {}
polynomial(){}
/// \brief Constructor.
/// \param coefficients : a reference to an Eigen matrix where each column is a coefficient,
......
......@@ -9,6 +9,7 @@ SET_TARGET_PROPERTIES(${PY_NAME} PROPERTIES PREFIX "")
PKG_CONFIG_USE_DEPENDENCY(${PY_NAME} eigenpy)
TARGET_LINK_LIBRARIES(${PY_NAME} ${Boost_LIBRARIES})
MESSAGE(WARNING "Boost libraries are : ${Boost_LIBRARIES}")
IF(APPLE)
# We need to change the extension for python bindings
......
......@@ -9,6 +9,7 @@
#include "python_definitions.h"
#include "python_variables.h"
#include "archive_python_binding.h"
#include <vector>
......@@ -287,6 +288,7 @@ BOOST_PYTHON_MODULE(curves)
.def("compute_derivate", &bezier6_t::compute_derivate)
.def("compute_primitive", &bezier6_t::compute_primitive)
.def("waypoints", &wayPointsToList<bezier6_t,6>)
.def(SerializableVisitor<bezier6_t>())
.def_readonly("degree", &bezier6_t::degree_)
.def_readonly("nbWaypoints", &bezier6_t::size_)
;
......@@ -306,6 +308,7 @@ BOOST_PYTHON_MODULE(curves)
.def("compute_derivate", &bezier3_t::compute_derivate)
.def("compute_primitive", &bezier3_t::compute_primitive)
.def("waypoints", &wayPointsToList<bezier3_t,3>)
.def(SerializableVisitor<bezier3_t>())
.def_readonly("degree", &bezier3_t::degree_)
.def_readonly("nbWaypoints", &bezier3_t::size_)
;
......@@ -344,13 +347,13 @@ BOOST_PYTHON_MODULE(curves)
/** BEGIN polynomial curve function**/
class_<polynomial_t>("polynomial", init<const polynomial_t::coeff_t, const real, const real >())
class_<polynomial_t>("polynomial", init<const polynomial_t::coeff_t, const real, const real >())
.def("__init__", make_constructor(&wrapSplineConstructor))
.def("min", &polynomial_t::min)
.def("max", &polynomial_t::max)
.def("__call__", &polynomial_t::operator())
.def("derivate", &polynomial_t::derivate)
.def("serialize", &polynomial_t::saveAsText)
.def(SerializableVisitor<polynomial_t>())
;
/** END polynomial function**/
......@@ -364,6 +367,7 @@ BOOST_PYTHON_MODULE(curves)
.def("derivate", &piecewise_polynomial_curve_t::derivate)
.def("add_curve", &piecewise_polynomial_curve_t::add_curve)
.def("is_continuous", &piecewise_polynomial_curve_t::is_continuous)
//.def(SerializableVisitor<piecewise_polynomial_curve_t>())
;
class_<piecewise_bezier3_curve_t>
("piecewise_bezier3_curve", no_init)
......@@ -374,6 +378,7 @@ BOOST_PYTHON_MODULE(curves)
.def("derivate", &piecewise_bezier3_curve_t::derivate)
.def("add_curve", &piecewise_bezier3_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier3_curve_t::is_continuous)
//.def(SerializableVisitor<piecewise_bezier3_curve_t>())
;
class_<piecewise_bezier6_curve_t>
("piecewise_bezier6_curve", no_init)
......@@ -384,6 +389,7 @@ BOOST_PYTHON_MODULE(curves)
.def("derivate", &piecewise_bezier6_curve_t::derivate)
.def("add_curve", &piecewise_bezier6_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier6_curve_t::is_continuous)
//.def(SerializableVisitor<piecewise_bezier6_curve_t>())
;
class_<piecewise_cubic_hermite_curve_t>
("piecewise_cubic_hermite_curve", no_init)
......@@ -394,6 +400,7 @@ BOOST_PYTHON_MODULE(curves)
.def("derivate", &piecewise_cubic_hermite_curve_t::derivate)
.def("add_curve", &piecewise_cubic_hermite_curve_t::add_curve)
.def("is_continuous", &piecewise_cubic_hermite_curve_t::is_continuous)
//.def(SerializableVisitor<piecewise_cubic_hermite_curve_t>())
;
/** END piecewise curve function **/
......@@ -421,6 +428,7 @@ BOOST_PYTHON_MODULE(curves)
.def("max", &cubic_hermite_spline_t::max)
.def("__call__", &cubic_hermite_spline_t::operator())
.def("derivate", &cubic_hermite_spline_t::derivate)
.def(SerializableVisitor<cubic_hermite_spline_t>())
;
/** END cubic_hermite_spline **/
......
......@@ -28,11 +28,13 @@ class TestCurves(unittest.TestCase):
while t < 2.:
self.assertTrue(norm(a(t) - matrix([1., 2., 3.]).T) < __EPS)
t += 0.1
waypoints_0 = matrix([[0,0,0], [0,0,0]]).transpose()
waypoints6_0 = matrix([[0,0,0,0,0,0], [0,0,0,0,0,0]]).transpose()
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
waypoints6 = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).transpose()
time_waypoints = matrix([0., 1.]).transpose()
# Create bezier6 and bezier3
a = bezier6(waypoints6)
a6 = bezier6(waypoints6)
a = bezier3(waypoints, 0., 3.)
# Test : Degree, min, max, derivate
#self.print_str(("test 1")
......@@ -80,6 +82,18 @@ class TestCurves(unittest.TestCase):
a = bezier3(waypoints, c)
self.assertTrue(norm(a.derivate(0, 1) - c.init_vel) < 1e-10)
self.assertTrue(norm(a.derivate(1, 2) - c.end_acc) < 1e-10)
# Test serialization : Bezier 6
a6.saveAsText("serialization_bezier6.test")
#waypoints = matrix([[0,0,0,], [0,0,0,]]).transpose()
b6 = bezier6(waypoints6_0)
b6.loadFromText("serialization_bezier6.test")
self.assertTrue((a6(0.4) == b6(0.4)).all())
# Test serialization : bezier 3
a.saveAsText("serialization_bezier3.test")
#waypoints = matrix([[0,0,0,], [0,0,0,]]).transpose()
b = bezier3(waypoints_0)
b.loadFromText("serialization_bezier3.test")
self.assertTrue((a(0.4) == b(0.4)).all())
return
def test_polynomial(self):
......@@ -94,11 +108,11 @@ class TestCurves(unittest.TestCase):
self.assertTrue((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2)
# Test serialization
#a.saveAsText("serialiation_polynomial.test")
a.saveAsText("serialiation_polynomial.test")
#waypoints = matrix([[0,0,0,], [0,0,0,]]).transpose()
#b = polynomial(waypoints)
#b.loadFromText("serialiation_polynomial.test")
#self.assertTrue((a(0.4) == b(0.4)).all())
b = polynomial(waypoints)
b.loadFromText("serialiation_polynomial.test")
self.assertTrue((a(0.4) == b(0.4)).all())
return
def test_piecewise_polynomial_curve(self):
......@@ -118,9 +132,7 @@ class TestCurves(unittest.TestCase):
ppc.is_continuous(0)
ppc.is_continuous(1)
# Test serialization
#serialize_piecewise_polynomial_curve("serialize_ppc.test",ppc);
#ppc2 = deserialize_piecewise_polynomial_curve("serialize_ppc.test");
#self.assertTrue((ppc(0.4) == ppc2(0.4)).all())
return
def test_piecewise_bezier3_curve(self):
......
......@@ -1439,8 +1439,10 @@ void piecewiseCurveConversionFromDiscretePointsTest(bool& error)
void serializationCurvesTest(bool& error)
{
std::string errMsg1("in serializationPiecewisePolynomialCurveTest, Error While serializing Polynomials : ");
std::string errMsg2("in serializationPiecewisePolynomialCurveTest, Error While serializing Piecewise curves : ");
std::string errMsg1("in serializationCurveTest, Error While serializing Polynomial : ");
std::string errMsg2("in serializationCurveTest, Error While serializing Bezier : ");
std::string errMsg3("in serializationCurveTest, Error While serializing Cubic Hermite : ");
std::string errMsg4("in serializationCurveTest, Error While serializing Piecewise curves : ");
point_t a(1,1,1); // in [0,1[
point_t b(2,1,1); // in [1,2[
point_t c(3,1,1); // in [2,3]
......@@ -1452,36 +1454,52 @@ void serializationCurvesTest(bool& error)
polynomial_t pol1(vec1.begin(), vec1.end(), 0, 1);
polynomial_t pol2(vec2.begin(), vec2.end(), 1, 2);
polynomial_t pol3(vec3.begin(), vec3.end(), 2, 3);
piecewise_polynomial_curve_t pc(pol1);
piecewise_polynomial_curve_t pc_test = pc;
pc.add_curve(pol2);
pc.add_curve(pol3);
piecewise_polynomial_curve_t ppc(pol1);
ppc.add_curve(pol2);
ppc.add_curve(pol3);
std::string fileName("fileTest.test");
// Test serialization on Polynomial
std::cout<<"Serialize test : Polynomial => ";
pol1.saveAsText(fileName);
// Test deserialization
polynomial_t pol_test;
pol_test.loadFromText(fileName);
CompareCurves<polynomial_t, polynomial_t>(pol1, pol_test, errMsg1, error);
std::cout<<"OK"<<std::endl;
// Test serialization on Piecewise curves
std::cout<<"Serialize test : Piecewise polynomial curve => ";
pc.saveAsText(fileName);
// Test deserialization
piecewise_polynomial_curve_t ppc_deserialized;
ppc_deserialized.loadFromText(fileName);
CompareCurves<piecewise_polynomial_curve_t,piecewise_polynomial_curve_t>(pc, ppc_deserialized, errMsg2, error);
std::cout<<"OK"<<std::endl;
// Test serialization on Bezier
bezier_curve_t bc = bezier_from_curve<bezier_curve_t, polynomial_t>(pol1);
bc.saveAsText(fileName);
bezier_curve_t bc_test;
bc_test.loadFromText(fileName);
CompareCurves<polynomial_t, bezier_curve_t>(pol1, bc_test, errMsg2, error);
// Test serialization on Cubic Hermite
cubic_hermite_spline_t chs = hermite_from_curve<cubic_hermite_spline_t, polynomial_t>(pol1);
chs.saveAsText(fileName);
cubic_hermite_spline_t chs_test;
chs_test.loadFromText(fileName);
CompareCurves<polynomial_t, cubic_hermite_spline_t>(pol1, chs_test, errMsg3, error);
// Test serialization on Piecewise Polynomial curve
ppc.saveAsText(fileName);
piecewise_polynomial_curve_t ppc_test;
ppc_test.loadFromText(fileName);
CompareCurves<piecewise_polynomial_curve_t,piecewise_polynomial_curve_t>(ppc, ppc_test, errMsg4, error);
// Test serialization on Piecewise Bezier curve
piecewise_bezier_curve_t pbc = ppc.convert_piecewise_curve_to_bezier<bezier_curve_t>();
pbc.saveAsText(fileName);
piecewise_bezier_curve_t pbc_test;
pbc_test.loadFromText(fileName);
CompareCurves<piecewise_polynomial_curve_t,piecewise_bezier_curve_t>(ppc, pbc_test, errMsg4, error);
// Test serialization on Piecewise Cubic Hermite curve
piecewise_cubic_hermite_curve_t pchc = ppc.convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>();
pchc.saveAsText(fileName);
piecewise_cubic_hermite_curve_t pchc_test;
pchc_test.loadFromText(fileName);
CompareCurves<piecewise_polynomial_curve_t,piecewise_cubic_hermite_curve_t>(ppc, pchc_test, errMsg4, error);
}
int main(int /*argc*/, char** /*argv[]*/)
{
std::cout << "performing tests... \n";
bool error = false;
/*
CubicFunctionTest(error);
ExactCubicNoErrorTest(error);
ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
......@@ -1506,7 +1524,6 @@ int main(int /*argc*/, char** /*argv[]*/)
toPolynomialConversionTest(error);
cubicConversionTest(error);
curveAbcDimDynamicTest(error);
*/
serializationCurvesTest(error);
if(error)
......
Supports Markdown
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