Commit bda51529 authored by stonneau's avatar stonneau

first commit for spline library and tests.

"Unit tests" not seriously done yet (just string outputs) , should use mockpp or something
parents
cmake_minimum_required(VERSION 2.6)
project(spline)
set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/build/")
set(LIBRARY_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/lib/")
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${PROJECT_SOURCE_DIR}/bin/")
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
add_subdirectory (src/spline)
add_subdirectory (src/tests/spline_test)
# - Try to find Eigen3 lib
#
# This module supports requiring a minimum version, e.g. you can do
# find_package(Eigen3 3.1.2)
# to require version 3.1.2 or newer of Eigen3.
#
# Once done this will define
#
# EIGEN3_FOUND - system has eigen lib with correct version
# EIGEN3_INCLUDE_DIR - the eigen include directory
# EIGEN3_VERSION - eigen version
# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
if(NOT Eigen3_FIND_VERSION)
if(NOT Eigen3_FIND_VERSION_MAJOR)
set(Eigen3_FIND_VERSION_MAJOR 2)
endif(NOT Eigen3_FIND_VERSION_MAJOR)
if(NOT Eigen3_FIND_VERSION_MINOR)
set(Eigen3_FIND_VERSION_MINOR 91)
endif(NOT Eigen3_FIND_VERSION_MINOR)
if(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION_PATCH 0)
endif(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
endif(NOT Eigen3_FIND_VERSION)
macro(_eigen3_check_version)
file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK FALSE)
else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK TRUE)
endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
if(NOT EIGEN3_VERSION_OK)
message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
"but at least version ${Eigen3_FIND_VERSION} is required")
endif(NOT EIGEN3_VERSION_OK)
endmacro(_eigen3_check_version)
if (EIGEN3_INCLUDE_DIR)
# in cache already
_eigen3_check_version()
set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
else (EIGEN3_INCLUDE_DIR)
find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
PATHS
${CMAKE_INSTALL_PREFIX}/include
${KDE4_INCLUDE_DIR}
PATH_SUFFIXES eigen3 eigen
)
if(EIGEN3_INCLUDE_DIR)
_eigen3_check_version()
endif(EIGEN3_INCLUDE_DIR)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
mark_as_advanced(EIGEN3_INCLUDE_DIR)
endif(EIGEN3_INCLUDE_DIR)
/**
* \file ExactCubic.h
* \brief Definition of a cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the CubicFunction class.
* It allows the creation and evaluation of natural 3D
* smooth cubic splines
*/
#ifndef _CLASS_CUBICFUNCTIONIMP
#define _CLASS_CUBICFUNCTIONIMP
#include "Exports.h"
#include "MathDefs.h"
namespace spline
{
class SplineVisitor;
/// \class CubicFunction
/// \brief Represents a cubic spline defined on the interval
/// [tBegin, tEnd]. It follows the equation
/// x(t) = a + b(t - tBegin) + c(t - tBegin)^2 + d(t - tBegin)^3
///
class CubicFunction
{
/* Constructors - destructors */
public:
///\brief Constructor
SPLINE_API CubicFunction(const Vector3& /*a*/, const Vector3& /*b*/, const Vector3& /*c*/, const Vector3& /*d*/, const Real /*tBegin*/, const Real /*tEnd*/);
///\brief Destructor
SPLINE_API ~CubicFunction();
private:
CubicFunction(const CubicFunction&);
CubicFunction& operator=(const CubicFunction&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param result : a reference to the Point set to the x(t)
SPLINE_API bool Evaluate(const Real /*t*/, Vector3& /*result*/) const;
/*Operations*/
/*Helpers*/
public:
/// \brief Given a timestep dt, returns a set of values for the exact spline
/// separated by this timestep
/// \param visitor : an object called for each timestep in the spline interval.
/// \param dt : the timestep
/// \param result : a reference to the Point set to the x(t)
SPLINE_API void Accept(SplineVisitor& /*visitor*/, Real /*dt*/) const;
/*Helpers*/
/*Attributes*/
private:
const Vector3 a_, b_, c_ ,d_;
const Real tBegin_, tEnd_;
/*Attributes*/
}; //class CubicFunction
}
#endif //_CLASS_CUBICFUNCTIONIMP
\ No newline at end of file
/**
* \file ExactCubic.h
* \brief class allowing to create an Exact cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the ExactCubic class.
* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of
* cubic splines fulfulling those 4 restrictions :
* - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
* - x_i(t_i+1) = x_i+1* ;
* - its derivative is continous at t_i+1
* - its acceleration is continous at t_i+1
* more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
* By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
*/
#ifndef _CLASS_EXACTCUBIC
#define _CLASS_EXACTCUBIC
#include "Exports.h"
#include "MathDefs.h"
#include <memory>
namespace spline
{
/** Definition for a waypoint */
typedef std::pair<const Real, const Vector3> Waypoint;
typedef std::deque<Waypoint> T_Waypoint;
typedef T_Waypoint::iterator IT_Waypoint;
typedef T_Waypoint::const_iterator CIT_Waypoint;
class SplineVisitor;
struct CubicPImpl; // private implementation
/// \class ExactCubic
/// \brief Represents a set of cubic splines defining a continuous function
/// crossing each of the waypoint given in its initialization
///
struct ExactCubic
{
/* Constructors - destructors */
public:
///\brief Constructor
///\param waypoints : a list comprising at least 2 waypoints in ascending time order
SPLINE_API ExactCubic(const T_Waypoint& /*waypoints*/);
///\brief Destructor
SPLINE_API ~ExactCubic();
private:
ExactCubic(const ExactCubic&);
ExactCubic& operator=(const ExactCubic&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param result : a reference to the Point set to the x(t)
/// \param return : true if evaluation is successful, false if t is out of range
SPLINE_API bool Evaluate(const Real /*t*/, Vector3& /*result*/) const;
/*Operations*/
/*Helpers*/
public:
/// \brief Given a timestep dt, returns a set of values for the exact spline
/// separated by this timestep
/// \param visitor : an object called for each timestep in the spline interval.
/// \param dt : the timestep
/// \param result : a reference to the Point set to the x(t)
SPLINE_API void Accept(SplineVisitor& /*visitor*/, Real /*dt*/) const;
/*Helpers*/
/*Attributes*/
private:
std::auto_ptr<CubicPImpl> pImpl_;
/*Attributes*/
};
}
#endif //_CLASS_EXACTCUBIC
\ No newline at end of file
/**
* \file Exports.h
* \brief Exports definition for the spline dll (windows)
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/
#if defined(WIN32) || defined(_WIN32) || defined(__WIN32) && !defined(__CYGWIN__)
#ifdef SPLINE_DLLEXPORT
#define SPLINE_API __declspec(dllexport)
#else
#define SPLINE_API __declspec(dllimport)
#endif
#else
#define SPLINE_API
#endif
/**
* \file Math.h
* \brief Linear algebra and other maths definitions. Based on Eigen 3 or more
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains math definitions used
* used throughout the library.
* Preprocessors definition are used to use eitheir float
* or double values, and 3 dimensional vectors for
* the Point structure.
*/
#ifndef _SPLINEMATH
#define _SPLINEMATH
#include "Exports.h"
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <deque>
#include <utility>
namespace spline{
#if (USEFLOAT)
typedef float Real;
typedef Eigen::Vector3f Vector3;
typedef Eigen::Vector2f Vector2;
typedef Eigen::VectorXf VectorX;
typedef Eigen::MatrixXf MatrixX;
typedef Eigen::Matrix4f Matrix4;
typedef Eigen::Matrix3f Matrix3;
#else
typedef double Real;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Vector2d Vector2;
typedef Eigen::VectorXd VectorX;
typedef Eigen::MatrixXd MatrixX;
typedef Eigen::Matrix4d Matrix4;
typedef Eigen::Matrix3d Matrix3;
#endif
//REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
template<typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat)
{
Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
VectorX m_sigma = svd.singularValues();
Real pinvtoler= 1.e-6; // choose your tolerance widely!
MatrixX m_sigma_inv = MatrixX::Zero(pinvmat.cols(),pinvmat.rows());
for (long i=0; i<m_sigma.rows(); ++i)
{
if (m_sigma(i) > pinvtoler)
m_sigma_inv(i,i)=1.0/m_sigma(i);
}
pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
}
} // namespace spline
#endif //_SPLINEMATH
/**
* \file SplineVisitor.h
* \brief Visitor for the Spline classes
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/
#ifndef _CLASS_SPLINE_VISITOR
#define _CLASS_SPLINE_VISITOR
namespace spline
{
/// \class SplineVisitor
/// \brief Represents a generic visitor for any Spline in the library.
///
class SPLINE_API SplineVisitor
{
/* Constructors - destructors */
public:
///\brief Constructor
SplineVisitor(){}
///\brief Destructor
~SplineVisitor(){}
private:
SplineVisitor(const SplineVisitor&);
SplineVisitor& operator=(const SplineVisitor&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param time : the time associated to the given value
/// \param value : the value x(time)
virtual void Visit(const Real /*time*/, const Vector3& /*value*/) = 0;
/*Operations*/
};
}
#endif //_CLASS_SPLINE_VISITOR
\ No newline at end of file
cmake_minimum_required(VERSION 2.6)
add_definitions(-DSPLINE_DLLEXPORT)
file(
GLOB_RECURSE
source_files
./*
./API/*
)
add_library(
spline
SHARED
${source_files}
)
#include "API/CubicFunction.h"
#include "API/SplineVisitor.h"
using namespace spline;
CubicFunction::CubicFunction(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Real tBegin, const Real tEnd)
: a_(a), b_(b), c_(c), d_(d), tBegin_(tBegin), tEnd_(tEnd)
{
// NOTHING
}
CubicFunction::~CubicFunction()
{
// NOTHING
}
bool CubicFunction::Evaluate(const Real t, Vector3& result) const
{
if(tBegin_ <= t && t <= tEnd_)
{
Real dt = (t - tBegin_);
result = a_ + b_ * dt + c_ * dt * dt + d_ * dt * dt * dt;
return true;
}
else // t out of bounds
{
return false;
}
}
void CubicFunction::Accept(SplineVisitor& visitor, Real dt) const
{
for(Real ti = tBegin_; ti <= tEnd_; ti = ti + dt)
{
Vector3 res; Evaluate(ti, res);
visitor.Visit(ti, res);
}
}
#include "API/ExactCubic.h"
#include "API/CubicFunction.h"
#include "API/SplineVisitor.h"
#include <vector>
namespace spline
{
typedef std::pair<Real, CubicFunction*> SubSpline;
typedef std::vector<SubSpline> T_SubSpline;
typedef T_SubSpline::iterator IT_SubSpline;
typedef T_SubSpline::const_iterator CIT_SubSpline;
struct CubicPImpl{
CubicPImpl(Real maxTime)
: maxTime_(maxTime)
{
// NOTHING
}
~CubicPImpl()
{
for(IT_SubSpline it = subSplines_.begin(); it != subSplines_.end(); ++ it)
{
delete it->second;
}
}
T_SubSpline subSplines_;
Real maxTime_; // max bound on which spline is defined.
};
} // namespace spline
using namespace spline;
ExactCubic::ExactCubic(const T_Waypoint& waypoints)
{
size_t size = waypoints.size();
assert(size > 2);
pImpl_.reset(new CubicPImpl(waypoints[size-1].first)); // setting max boundary
// refer to the paper to understand all this.
MatrixX h1 = MatrixX::Zero(size, size);
MatrixX h2 = MatrixX::Zero(size, size);
MatrixX h3 = MatrixX::Zero(size, size);
MatrixX h4 = MatrixX::Zero(size, size);
MatrixX h5 = MatrixX::Zero(size, size);
MatrixX h6 = MatrixX::Zero(size, size);
MatrixX a = MatrixX::Zero(size, 3);
MatrixX b = MatrixX::Zero(size, 3);
MatrixX c = MatrixX::Zero(size, 3);
MatrixX d = MatrixX::Zero(size, 3);
MatrixX x = MatrixX::Zero(size, 3);
Real dTi, dTi_1, dTi_sqr, dTi_1sqr, dTi_cube;
Real t_previous = waypoints[0].first;
// I think using numbers instead of iterators will be clearer on this case
for(int i=0; i< size - 1; ++i)
{
dTi = waypoints[i + 1].first - waypoints[i].first;
dTi_sqr = dTi * dTi;
dTi_cube = dTi_sqr * dTi;
assert(dTi > 0); //make sure of the ascendant order
// filling matrices values
h3(i,i) = -3 / dTi_sqr;
h3(i,i+1) = 3 / dTi_sqr;
h4(i,i) = -2 / dTi;
h4(i,i+1) = -1 / dTi;
h5(i,i) = 2 / dTi_cube;
h5(i,i+1) = -2 / dTi_cube;
h6(i,i) = 1 / dTi_sqr;
h6(i,i+1) = 1 / dTi_sqr;
// we stop one step earlier for matrices h1 and h2
if(i + 2 < size)
{
// this can be optimized but let's focus on clarity as long as not needed
dTi_1 = waypoints[i + 2].first - waypoints[i + 1].first;
dTi_1sqr = dTi_1 * dTi_1;
h1(i+1, i) = 2 / dTi;
h1(i+1, i+1) = 4 / dTi + 4 / dTi_1;
h1(i+1, i+2) = 2 / dTi_1;
h2(i+1, i) = - 6 / dTi_sqr;
h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
h2(i+1, i+2) = 6 / dTi_1sqr;
}
//computing x = [x0* x1* ... xT*]^T
x.row(i) = waypoints[i].second.transpose();
}
// adding last x
x.row(size-1) = waypoints[size-1] .second.transpose();
// now we just have to apply the linear relations:
a = x;
// should I pseudo inverse this?
PseudoInverse(h1);
b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
c = h3 * x + h4 * b;
d = h5 * x + h6 * b;
// Now each line i of the a, b, c and d matrices correponds to the coefficient of spline i
// Let's create those, and initialiaze our pImpl_
for(int i=0; i< size - 1; ++i) // last spline never evaluated, that's ok because xi(ti_1) = x_i_1*
{
CubicFunction* subSpline = new CubicFunction(a.row(i), b.row(i), c.row(i), d.row(i), waypoints[i].first, waypoints[i+1].first);
pImpl_->subSplines_.push_back(std::make_pair(waypoints[i].first, subSpline));
}
}
ExactCubic::~ExactCubic()
{
// NOTHING
}
bool ExactCubic::Evaluate(const Real t, Vector3& value) const
{
CIT_SubSpline it = pImpl_->subSplines_.begin();
CIT_SubSpline it2 = it;
++it2;
for(; it2!= pImpl_->subSplines_.end(); ++it2)
{
// t out of min bound
if(t < it->first)
{
return false;
}
else if(t <= it2->first)
{
return it->second->Evaluate(t, value);
}
++it;
}
// handling max Bound
if(t <= pImpl_->maxTime_)
{
return it->second->Evaluate(t, value);
}
else
{
return false; // t out of max bound
}
}
void ExactCubic::Accept(SplineVisitor& visitor, Real dt) const
{
for(Real ti = pImpl_->subSplines_[0].first; ti <= pImpl_->maxTime_; ti = ti + dt)
{
Vector3 res; Evaluate(ti, res);
visitor.Visit(ti, res);
}
}
cmake_minimum_required(VERSION 2.6)
include_directories("${PROJECT_SOURCE_DIR}/src/spline/API")
add_executable(
spline_tests Main.cpp
)
target_link_libraries (spline_tests spline)
#include "CubicFunction.h"
#include "ExactCubic.h"
#include "SplineVisitor.h"
#include <string>
#include <iostream>
#include <cmath>
using namespace std;
namespace spline
{
bool QuasiEqual(const Real a, const Real b, const float margin)
{
if ((a <= 0 && b <= 0) || (a >= 0 && b>= 0))
{
return (abs(a-b)) <= margin;
}
else
{
return abs(a) + abs(b) <= margin;
}
}
const float margin = 0.01f;
bool operator ==(const Vector3& a, const Vector3& b)
{
return QuasiEqual(a.x(), b.x(), margin) && QuasiEqual(a.y(), b.y(), margin) && QuasiEqual(a.z(), b.z(), margin);
}
} // namespace spline
using namespace spline;
ostream& operator<<(ostream& os, const Vector3& pt)
{
os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
return os;
}
void ComparePoints(const Vector3& pt1, const Vector3& pt2, const std::string& errmsg, bool& error)
{
if(!(pt1 == pt2))
{
error = true;
std::cout << errmsg << pt1 << " ; " << pt2 << "\n";
}
}
/*Cubic Function tests*/
void CubicFunctionTest(bool& error)
{
std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
Vector3 a(1,2,3);
Vector3 b(2,3,4);
Vector3 c(3,4,5);
Vector3 d(3,6,7);
CubicFunction cf(a, b, c, d, 0, 1);
Vector3 res1;
cf.Evaluate(0, res1);
Vector3 x0(1,2,3);
ComparePoints(x0, res1, errMsg + "(0) ", error);
Vector3 x1(9,15,19);
cf.Evaluate(1, res1);
ComparePoints(x1, res1, errMsg + "(1) ", error);
Vector3 x2(3.125,5.25,7.125);
cf.Evaluate(0.5, res1);
ComparePoints(x2, res1, errMsg + "(0.5) ", error);
CubicFunction cf2(a, b, c, d, 0.5, 1);
cf2.Evaluate(0.5, res1);
ComparePoints(x0, res1, errMsg + "x3 ", error);
if(cf2.Evaluate(0.4, res1))
{
error = true;
std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
}
if(cf2.Evaluate(1.1, res1))
{
error = true;
std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
}
}
/*Exact Cubic Function tests*/
void ExactCubicNoErrorTest(bool& error)
{
spline::T_Waypoint waypoints;
for(Real i = 0; i <= 1; i = i + 0.2)
{
waypoints.push_back(std::make_pair(i,Vector3(i,i,i)));
}
ExactCubic exactCubic(waypoints);
Vector3 res1;
if(!exactCubic.Evaluate(0, res1))
{
error = true;
std::cout << "Evaluation of exactCubic error, 0 should be in range value\n";
}
if(!exactCubic.Evaluate(1, res1))
{
error = true;
std::cout << "Evaluation of exactCubic error, 1 should be in range value\n";
}
if(exactCubic.Evaluate(1.2, res1))