first commit for spline library and tests.

"Unit tests" not seriously done yet (just string outputs) , should use mockpp or something

CMakeLists.txt

+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)
+

build/FindEigen3.cmake

+# - 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)

src/spline/API/CubicFunction.h

+/**
+* \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

src/spline/API/ExactCubic.h

+/**
+* \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

src/spline/API/Exports.h

+/**
+* \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
+

src/spline/API/MathDefs.h

+/**
+* \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

src/spline/API/SplineVisitor.h

+/**
+* \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

src/spline/CMakeLists.txt

+cmake_minimum_required(VERSION 2.6)
+
+add_definitions(-DSPLINE_DLLEXPORT)
+
+file(
+        GLOB_RECURSE
+        source_files
+        ./*
+        ./API/*
+)
+ 
+add_library(
+        spline
+        SHARED
+        ${source_files}
+)

src/spline/CubicFunction.cpp

+#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);
+	}
+}

src/spline/ExactCubic.cpp

+#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);
+	}
+}

src/tests/spline_test/CMakeLists.txt

+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)

src/tests/spline_test/Main.cpp

+
+#include "CubicFunction.h"
+#include "ExactCubic.h"
+#include "SplineVisitor.h"
+
+#include <string>
+#include <iostream>
+#include <cmath>
+
+using namespace std;