Merge pull request #22 from florent-lamiraux/fix-cubic-b-spline-bug

Fix bug in cubic B spline and enhance class

doc/cubic-b-spline.tex

@@ -11,7 +11,7 @@
 \maketitle
 \section {Definition}
 
-\paragraph {Knots} $m \leq 4$ increasing real values:
+\paragraph {Knots} $m \geq 4$ increasing real values:
 $$
 t_0 \leq t_1 \leq \cdots \leq t_{m-1} 
 $$

include/roboptim/trajectory/cubic-b-spline.hh

@@ -44,6 +44,7 @@ namespace trajectory
 
     typedef std::vector<polynomials3vector_t> polynomials3vectors_t;
 
+    typedef std::vector <value_type> knots_t;
 
     /// \brief Instantiate a cubic B-Spline from its definition.
     ///
@@ -58,6 +59,19 @@ namespace trajectory
 		  const vector_t& parameters,
 		  const std::string name = "cubic B-Spline");
 
+    /// \brief Instantiate a cubic B-Spline from its definition.
+    ///
+    /// \param dimension spline dimension: \f$n\f$
+    /// \param knots vector of knots,
+    /// \param parameters vector of parameters defining control points
+    /// \param name function title
+    ///
+    /// The number of control points is inferred from the dimension of
+    /// the parameter vector.
+    CubicBSpline (size_type dimension, const knots_t& knots,
+		  const vector_t& parameters,
+		  const std::string name = "cubic B-Spline");
+
     /// \brief Copy constructor.
     /// \param spline spline that will be copied
     CubicBSpline (const CubicBSpline& spline);
@@ -193,7 +207,7 @@ namespace trajectory
     size_type nbp_;
 
     /// \brief Vector of knots.
-    std::vector <value_type> knots_;
+    knots_t knots_;
 
     /// \brief Basis polynomials.
     /// basisPolynomials_[i][j] = B_{i,i+j}

src/cubic-b-spline.cc

@@ -50,6 +50,26 @@ namespace trajectory
       knots_.push_back (ti);
       ti += delta_t;
     }
+    // interval lower bound should be rigorously equal to knot 3.
+    knots_ [3] = tr.first;
+    setParameters (p);
+    computeBasisPolynomials ();
+  }
+
+  CubicBSpline::CubicBSpline (size_type outputSize, const knots_t& knots,
+			      const vector_t& p, std::string name)
+    : Trajectory<3> (std::make_pair (knots [3], knots [knots.size ()-4]),
+		     outputSize, p, name), nbp_ (p.size () / outputSize),
+      knots_ (knots), uniform_ (false)
+  {
+    // Parameter size should be a multiple of spline dimension.
+    assert (parameters_.size () % outputSize == 0);
+
+    // Number of control points should be at least 4.
+    assert (nbp_ >= 4);
+
+    // Fill vector of regularly spaced knots.
+    assert (knots_.size () - (nbp_ + 4) == 0);
     setParameters (p);
     computeBasisPolynomials ();
   }
@@ -107,7 +127,7 @@ namespace trajectory
 	  * Monomial3 (t4) * Monomial3 (t1) * Monomial3 (t1);
 
 	Polynomial3 B2 =
-	  1./((t3-t0)*(t3-t1)*(t2-t1))
+	  1./((t3-t0)*(t3-t1)*(t3-t2))
 	  * Monomial3 (t0) * Monomial3 (t3)
 	  * Monomial3 (t3)
 	  + 1./((t4-t1)*(t3-t1)*(t3-t2))
@@ -162,54 +182,40 @@ namespace trajectory
     t = detail::fixTime (t, *this);
     typedef boost::numeric::converter<size_type, double> Double2SizeType;
 
-    // t_3
-    double tmin = timeRange ().first;
     size_type imin = 3;
-
-    // t_{m-4}
-    double tmax = timeRange ().second;
-    size_type imax = nbp_;
+    size_type imax = knots_.size () - 5;
 
     unsigned int count = 0;
     bool found = false;
     size_type i = 1;
     std::size_t i_ = static_cast<std::size_t> (i);
-    size_type iPrev = 0;
 
-    while (!found && iPrev != i)
+    while (!found)
       {
 	i = Double2SizeType::convert
-	  (std::floor (static_cast<double> (imin) + (t - tmin)
-		       / (tmax - tmin) * static_cast<double> (imax - imin)));
+	  (std::floor (.5 * static_cast<double> (imin + imax) + .5));
 	i_ = static_cast<std::size_t> (i);
-
 	if (t < knots_ [i_])
 	  {
-	    tmax = knots_ [i_ - 1];
 	    imax = i - 1;
 	  }
 	else if (t >= knots_ [i_ + 1])
 	  {
-	    if (t < knots_ [i_ + 2])
-	      {
-		i = i + 1;
-		found = true;
-	      }
 	    imin = i + 1;
-	    tmin = knots_ [i_ + 1];
 	  }
 	else
 	  {
 	    found = true;
 	  }
 	++count;
 	assert (count < 10000);
-	iPrev = i;
       }
     if (i > nbp_ - 1)
       i = nbp_-1;
     if (i < 3)
       i = 3;
+    assert (knots_ [i] <= t);
+    assert (t <= knots_ [i+1]);
     return i;
   }
 

tests/CMakeLists.txt

@@ -111,6 +111,11 @@ ROBOPTIM_TRAJECTORY_TEST(state-function)
 
 # Spline.
 ROBOPTIM_TRAJECTORY_TEST(cubic-b-spline)
+CONFIGURE_FILE ("cubic-b-spline-gradient.stdout"
+  "cubic-b-spline-gradient.stdout")
+ROBOPTIM_TRAJECTORY_TEST(cubic-b-spline-2)
+CONFIGURE_FILE ("cubic-b-spline-2.stdout"
+  "cubic-b-spline-2.stdout")
 ROBOPTIM_TRAJECTORY_TEST(cubic-b-spline-gradient)
 ROBOPTIM_TRAJECTORY_TEST(b-spline)
 ROBOPTIM_TRAJECTORY_TEST(constrained-b-spline)

tests/cubic-b-spline-2.cc

@@ -0,0 +1,127 @@
+// Copyright (C) 2009 by Thomas Moulard, AIST, CNRS, INRIA.
+//
+// This file is part of the roboptim.
+//
+// roboptim is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// roboptim is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with roboptim.  If not, see <http://www.gnu.org/licenses/>.
+
+#undef NDEBUG
+
+#include <iostream>
+#include <fstream>
+#include <boost/format.hpp>
+#include "shared-tests/fixture.hh"
+
+#include <roboptim/core/finite-difference-gradient.hh>
+
+#include <roboptim/core/visualization/gnuplot.hh>
+#include <roboptim/core/visualization/gnuplot-commands.hh>
+#include <roboptim/core/visualization/gnuplot-function.hh>
+
+#include <roboptim/trajectory/visualization/trajectory.hh>
+
+#include <roboptim/trajectory/fwd.hh>
+#include <roboptim/trajectory/cubic-b-spline.hh>
+
+using namespace roboptim;
+using namespace roboptim::trajectory;
+using namespace roboptim::visualization;
+using namespace roboptim::visualization::gnuplot;
+
+typedef CubicBSpline::size_type size_type;
+typedef CubicBSpline::value_type value_type;
+
+typedef std::vector < std::vector < value_type > > matrix_t;
+
+matrix_t getReference (const std::string& file)
+{
+  matrix_t result;
+  size_type nbRows = 0;
+
+  std::ifstream f; f.open (file.c_str ());
+  std::string line;
+
+  while (f.good ()) {
+    std::vector <value_type> data;
+    std::getline (f, line);
+    std::stringstream ss (line);
+    std::copy (std::istream_iterator< value_type > (ss),
+	       std::istream_iterator< value_type > (),
+	       std::back_inserter (data));
+    ++nbRows;
+    result.push_back (data);
+  }
+  return result;
+}
+
+BOOST_FIXTURE_TEST_SUITE (trajectory, TestSuiteConfiguration)
+
+BOOST_AUTO_TEST_CASE (trajectory_cubic_b_spline)
+{
+  using namespace roboptim::visualization::gnuplot;
+  size_type nbRows = 0;
+  matrix_t reference = getReference ("cubic-b-spline-2.stdout");
+  if (reference.size () == 0) {
+    throw std::runtime_error
+      ("failed to open file \"cubic-b-spline-2.stdout\"");
+  }
+  double tol = 1e-6;
+
+  size_type nbKnots = 12;
+  double knotArray [] =
+    { -3, -2, -1, 0, .2, 1., 1.2, 1.8, 1.9, 2.2, 3.2, 4.2};
+  CubicBSpline::knots_t knots (nbKnots);
+  for (std::size_t i=0; i<(std::size_t)nbKnots ; ++i) knots [i] = knotArray [i];
+  std::vector <CubicBSpline::vector_t> params;
+
+  for (std::size_t i=0; i < (std::size_t)(nbKnots-4); ++i) {
+    params.push_back (CubicBSpline::vector_t (nbKnots - 4));
+    params [i].setZero ();
+    params [i][i] = 1;
+  }
+
+  std::ofstream f; f.open ("output");
+  for (std::size_t i=0; i<(std::size_t)nbKnots-4; ++i) {
+    CubicBSpline spline (1, knots, params [i], "Cubic B-spline");
+
+    discreteInterval_t interval (spline.timeRange ().first,
+				 spline.timeRange ().second, 0.002);
+
+    std::cout
+      <<
+      (boost::format
+       ("set term wxt persist title 'Spline: base functions' %1% font ',5'")
+       % i) << std::endl
+      << "set grid xtics noytics linewidth 0.5" << std::endl
+      << "set xlabel 't'" << std::endl
+      << "set ylabel 'value'" << std::endl;
+    std::string title ("spline");
+    std::cout << "plot '-' title '"<< title <<"' with line\n";
+
+    spline (0.95);
+    // Loop over the interval of definition
+    for (double t = boost::get<0> (interval); t < boost::get<1> (interval);
+	 t += boost::get<2> (interval)) {
+      CubicBSpline::vector_t value = spline (t);
+      std::cout << (boost::format ("%1.3f %2.8f\n")
+		    % normalize (t)
+		    % normalize (value (0))).str ();
+      BOOST_CHECK_SMALL (t - reference [nbRows][0], tol);
+      BOOST_CHECK_SMALL (value (0) - reference [nbRows][1], tol);
+      ++nbRows;
+    }
+    std::cout << "e" << std::endl;
+  }
+  std::cout << "unset multiplot" << std::endl;
+}
+BOOST_AUTO_TEST_SUITE_END ()