Repository: hungpham2511/toppra (language: python)

Issue:
[CPP] Construct piecewise poly from boundary conditions (first derivatives at curve endpoints)
Hi. I am trying to find a way to construct a 3rd order piecewise-polynomial / cubic spline by specifying first-derivatives at the endpoints of the curve in cpp.

I found the implementation in **Python** here https://github.com/hungpham2511/toppra/blob/644e263269e45be8fc595bae59e959b9570edaa3/toppra/interpolator.py#L360
With the python implementation, a spline can be constructed by specifying boundary conditions such as the first-derivatives at the endpoints of the curve as documented here https://docs.scipy.org/doc/scipy/reference/generated/scipy.interpolate.CubicSpline.html

However, in **cpp**, all that I can find related to piecewise poly are here https://github.com/hungpham2511/toppra/blob/644e263269e45be8fc595bae59e959b9570edaa3/cpp/src/toppra/geometric_path/piecewise_poly_path.hpp#L23
The spline in cpp can only be constructed from these 2 scenarios:
- Specifying coefficients for each segment of the line and its breakpoints https://github.com/hungpham2511/toppra/blob/644e263269e45be8fc595bae59e959b9570edaa3/cpp/src/toppra/geometric_path/piecewise_poly_path.hpp#L35
- Constructing from the Cubic Hermite Form (specifying the values and its first-order derivatives at each knot) https://github.com/hungpham2511/toppra/blob/644e263269e45be8fc595bae59e959b9570edaa3/cpp/src/toppra/geometric_path/piecewise_poly_path.hpp#L57

In the scenario where the first-order derivatives at each knot are unknown, 
with a general form of a cubic spline in the following form (4n unknowns),
![Screenshot from 2021-01-08 16-28-45](https://user-images.githubusercontent.com/45716700/103992281-d2ff6d80-51ce-11eb-992b-957737c569e2.png)


a cubic spline still can be constructed by specifying the following:
- The values at each knot (2n equations)
![Screenshot from 2021-01-08 16-31-03](https://user-images.githubusercontent.com/45716700/103992465-1954cc80-51cf-11eb-89f9-0774f11bf72a.png)

- The first and second derivative of all polynomials are identical in the points where they touch their adjacent polynomial 
(2n - 2 equations)
![Screenshot from 2021-01-08 16-33-52](https://user-images.githubusercontent.com/45716700/103992650-63d64900-51cf-11eb-8124-6f84319fa4c8.png)
![Screenshot from 2021-01-08 16-31-08](https://user-images.githubusercontent.com/45716700/103992459-16f27280-51cf-11eb-8986-7dc24f51c14e.png)
 
- The first derivative at the curve endpoints (2 equations)
f<sub>1</sub>'(x<sub>1</sub>) = a
f<sub>n</sub>'(x<sub>n+1</sub>) = b, where a and b are known values


However, currently, this method is not available in cpp. Are there any suggestions to go about this?



Discussion / hints:
As far as I know, this is not implemented. However, this is easy to implement since this boils down to solving `A x = b` and Eigen provides everything you need for it, you only have to build `A` and `b`. A bit cumbersome but nothing hard.
Noted, thanks
@leonardoedgar If you have an implementation, feel free to open a PR!
@hungpham2511 noted

Fix patch:
diff --git a/HISTORY.md b/HISTORY.md
index befe9711..467c9561 100644
--- a/HISTORY.md
+++ b/HISTORY.md
@@ -14,6 +14,7 @@
 - [cpp] [#146][#146] Add constraint::CartesianVelocityNorm
 - [cpp] [#158][#158] Add Seidel solver.
 - [cpp] Enhance Seidel solver numerical behaviour.
+- [cpp] [#161] Enable to construct spline from 1st and 2nd order derivatives at the curve endpoints
 
 ### Changed
 - [cpp] [#153] Fix variable mismatch in constraint::CartesianVelocityNorm
diff --git a/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp b/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp
index e6a930f7..0ccfd9a0 100644
--- a/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp
+++ b/cpp/src/toppra/geometric_path/piecewise_poly_path.cpp
@@ -2,6 +2,7 @@
 #include <ostream>
 #include <toppra/geometric_path/piecewise_poly_path.hpp>
 #include <toppra/toppra.hpp>
+#include <Eigen/Dense>
 
 #ifdef TOPPRA_OPT_MSGPACK
 #include <msgpack.hpp>
@@ -28,6 +29,89 @@ PiecewisePolyPath::PiecewisePolyPath(const Matrices & coefficients,
   computeDerivativesCoefficients();
 }
 
+PiecewisePolyPath::PiecewisePolyPath(const Vectors &positions, const Vector &times,
+        const std::array<BoundaryCond, 2> &bc_type) {
+    // Prepare input
+    checkInputArgs(positions, times, bc_type);
+    Matrices coefficients(times.size() - 1);
+    computeCubicSplineCoefficients(positions, times, bc_type, coefficients);
+    std::vector<value_type> breakpoints (times.data(), times.data() + times.size());
+    *this = PiecewisePolyPath(coefficients, breakpoints);
+}
+
+void PiecewisePolyPath::computeCubicSplineCoefficients(const Vectors &positions, const Vector &times,
+        const std::array<BoundaryCond, 2> &bc_type, Matrices &coefficients) {
+    // h(i) = t(i+1) - t(i)
+    Vector h (times.rows() - 1);
+    for (size_t i = 0; i < h.rows(); i++){
+        h(i) = times(i + 1) - times(i);
+    }
+
+    // Construct the tri-diagonal matrix A based on spline continuity criteria
+    Matrix A = Matrix::Zero(times.rows(), times.rows());
+    for (size_t i = 1; i < A.rows() - 1; i++) {
+        A.row(i).segment(i - 1, 3) << h(i - 1), 2 * (h(i - 1) + h(i)), h(i);
+    }
+
+    // Construct B based on spline continuity criteria
+    Vectors B (positions.at(0).rows());
+    for (size_t i = 0; i < B.size(); i++) {
+        B[i].resize(times.rows());
+        for (size_t j = 1; j < A.rows() - 1; j++) {
+            B[i](j) = 3 * (positions[j + 1](i) - positions[j](i)) / h(j) -
+                      3 * (positions[j](i) - positions[j - 1](i)) / h(j - 1);
+        }
+    }
+
+    // Insert boundary conditions to A and B
+    if (bc_type[0].order == 1) {
+        A.row(0).segment(0, 2) << 2 * h(0), h(0);
+        for (size_t i = 0; i < B.size(); i++) {
+            B[i](0) = 3 * (positions[1](i) - positions[0](i)) / h(0) - 3 * bc_type[0].values(i);
+        }
+    }
+    else if (bc_type[0].order == 2) {
+        A(0, 0) = 2;
+        for (size_t i = 0; i < B.size(); i++) {
+            B[i](0) = bc_type[0].values(i);
+        }
+    }
+
+    if (bc_type[1].order == 1) {
+        A.row(A.rows() - 1).segment(A.cols() - 2, 2) << h(h.rows() - 1), 2 * h(h.rows() - 1);
+        for (size_t i = 0; i < B.size(); i++) {
+            B[i](B[i].rows() - 1) =
+                    3 * bc_type[1].values(i) -
+                    3 * (positions[positions.size() - 1](i) - positions[positions.size() - 2](i)) / h(h.rows() - 1);
+        }
+    }
+    else if (bc_type[1].order == 2) {
+        A(A.rows() - 1, A.cols() - 1) = 2;
+        for (size_t i = 0; i < B.size(); i++) {
+            B[i](B[i].rows() - 1) = bc_type[1].values(i);
+        }
+    }
+
+    // Solve AX = B
+    Vectors X (positions[0].rows());
+    for (size_t i = 0; i < X.size(); i++) {
+        X[i].resize(times.rows());
+        X[i] = A.colPivHouseholderQr().solve(B[i]);
+    }
+
+    // Insert spline coefficients
+    for (size_t i = 0; i < coefficients.size() ; i++) {
+        coefficients[i].resize(4, positions[0].rows());
+        for (size_t j = 0; j < coefficients[i].cols(); j++) {
+            coefficients[i](0, j) = (X[j](i + 1) - X[j](i)) / (3 * h(i));
+            coefficients[i](1, j) = X[j](i);
+            coefficients[i](2, j) = (positions[i + 1](j) - positions[i](j)) / h(i) -
+                    h(i) / 3 * (2 * X[j](i) + X[j](i + 1));
+            coefficients[i](3, j) = positions[i](j);
+        }
+    }
+}
+
 Bound PiecewisePolyPath::pathInterval() const {
   Bound v;
   v << m_breakpoints.front(), m_breakpoints.back();
@@ -48,7 +132,7 @@ Vector PiecewisePolyPath::eval_single(value_type pos, int order) const {
 }
 
 // Not the most efficient implementation. Coefficients are
-// recompoted. Should be refactorred.
+// recomputed. Should be refactored.
 Vectors PiecewisePolyPath::eval(const Vector &positions, int order) const {
   assert(order < 3 && order >= 0);
   Vectors outputs;
@@ -90,6 +174,41 @@ void PiecewisePolyPath::checkInputArgs() {
   }
 }
 
+void PiecewisePolyPath::checkInputArgs(const Vectors &positions, const Vector &times,
+        const std::array<BoundaryCond, 2> &bc_type) {
+    if (positions.size() != times.rows()) {
+        throw std::runtime_error("The length of 'positions' doesn't match the length of 'times'.");
+    }
+    if (times.rows() < 2) {
+        throw std::runtime_error("'times' must contain at least 2 elements.");
+    }
+    for (size_t i = 1; i < positions.size(); i++) {
+        if (positions[i].rows() != positions[i - 1].rows()) {
+            throw std::runtime_error("The number of elements in each position has to be equal.");
+        }
+    }
+    Vector dtimes (times.rows() - 1);
+    for (size_t i = 1; i < times.rows(); i++) {
+        dtimes(i - 1) = times(i) - times(i - 1);
+        if (dtimes(i - 1) <= 0) {
+            throw std::runtime_error("'times' must be a strictly increasing sequence.");
+        }
+    }
+
+    // Validate boundary conditions
+    int expected_deriv_size = positions[0].size();
+    for (const BoundaryCond &bc: bc_type) {
+        if (bc.order != 1 && bc.order != 2) {
+            throw std::runtime_error("The specified derivative order must be 1 or 2.");
+        }
+        if (bc.values.size() != expected_deriv_size) {
+            throw std::runtime_error(
+                    "`deriv_value` size " + std::to_string(bc.values.size()) + " is not the expected one " +
+                    std::to_string(expected_deriv_size) + ".");
+        }
+    }
+}
+
 void PiecewisePolyPath::computeDerivativesCoefficients() {
   m_coefficients_1.reserve(m_coefficients.size());
   m_coefficients_2.reserve(m_coefficients.size());
@@ -124,7 +243,7 @@ void PiecewisePolyPath::serialize(std::ostream &O) const {
   msgpack::pack(O, allraw);
   msgpack::pack(O, m_breakpoints);
 #endif
-};
+}
 
 void PiecewisePolyPath::deserialize(std::istream &I) {
 #ifdef TOPPRA_OPT_MSGPACK
@@ -163,7 +282,7 @@ void PiecewisePolyPath::deserialize(std::istream &I) {
   checkInputArgs();
   computeDerivativesCoefficients();
 #endif
-};
+}
 
 void PiecewisePolyPath::reset() {
   m_breakpoints.clear();
@@ -180,7 +299,7 @@ void PiecewisePolyPath::initAsHermite(const Vectors &positions,
   assert(velocities.size() == times.size());
   TOPPRA_LOG_DEBUG("Constructing new Hermite polynomial");
   m_configSize = m_dof = positions[0].size();
-  m_degree = 3;  // cublic spline
+  m_degree = 3;  // cubic spline
   m_breakpoints = times;
   for (std::size_t i = 0; i < times.size() - 1; i++) {
     TOPPRA_LOG_DEBUG("Processing segment index: " << i << "/" << times.size() - 1);
diff --git a/cpp/src/toppra/geometric_path/piecewise_poly_path.hpp b/cpp/src/toppra/geometric_path/piecewise_poly_path.hpp
index 9222fa86..77dd00e0 100644
--- a/cpp/src/toppra/geometric_path/piecewise_poly_path.hpp
+++ b/cpp/src/toppra/geometric_path/piecewise_poly_path.hpp
@@ -6,12 +6,18 @@
 
 namespace toppra {
 
+/// Boundary condition
+typedef struct {
+    int order;
+    Vector values;
+} BoundaryCond;
+
 /**
  * \brief Piecewise polynomial geometric path.
  *
- * An implemetation of a piecewise polynoamial geometric path.
+ * An implementation of a piecewise polynomial geometric path.
  *
- * The coefficent vector has shape (N, P, D), where N is the number
+ * The coefficient vector has shape (N, P, D), where N is the number
  * segments. For each segment, the i-th row (P index) denotes the
  * power, while the j-th column is the degree of freedom. In
  * particular,
@@ -29,11 +35,19 @@ class PiecewisePolyPath : public GeometricPath {
    *
    * See class docstring for details.
    *
-   * @param coefficients Polynoamial coefficients.
+   * @param coefficients Polynomial coefficients.
    * @param breakpoints Vector of breakpoints.
    */
   PiecewisePolyPath(const Matrices &coefficients, std::vector<value_type> breakpoints);
 
+  /**
+   * \brief Construct a new piecewise 3rd degree polynomial.
+   * @param positions Vectors of path positions at given times.
+   * @param times Vector of times in a strictly increasing order.
+   * @param bc_type Boundary conditions at the curve start and end.
+   */
+  PiecewisePolyPath(const Vectors &positions, const Vector &times, const std::array<BoundaryCond, 2> &bc_type);
+
   /**
    * /brief Evaluate the path at given position.
    */
@@ -61,9 +75,13 @@ class PiecewisePolyPath : public GeometricPath {
  protected:
   void initAsHermite(const Vectors &positions, const Vectors &velocities,
                      const std::vector<value_type> times);
+  void computeCubicSplineCoefficients(const Vectors &positions, const Vector &times,
+          const std::array<BoundaryCond, 2> &bc_type, Matrices &coefficients);
   void reset();
   size_t findSegmentIndex(value_type pos) const;
   void checkInputArgs();
+  void checkInputArgs(const Vectors &positions, const Vector &times,
+                      const std::array<BoundaryCond, 2> &bc_type);
   void computeDerivativesCoefficients();
   const Matrix &getCoefficient(int seg_index, int order) const;
   Matrices m_coefficients, m_coefficients_1, m_coefficients_2;


Test patch:
diff --git a/cpp/tests/CMakeLists.txt b/cpp/tests/CMakeLists.txt
index bd182a0d..133afa68 100644
--- a/cpp/tests/CMakeLists.txt
+++ b/cpp/tests/CMakeLists.txt
@@ -6,6 +6,7 @@ set(TEST_SOURCES
   test_solver.cpp
   test_poly_geometric_path.cpp
   test_hermite_spline.cpp
+  test_cubic_spline.cpp
   test_algorithm.cpp
   test_parametrizer.cpp
   test_toppra_approach.cpp
@@ -13,6 +14,7 @@ set(TEST_SOURCES
 
 add_executable(all_tests ${TEST_SOURCES})
 target_link_libraries(all_tests PUBLIC toppra gtest ${CMAKE_THREAD_LIBS_INIT})
+target_include_directories(all_tests PRIVATE .)
 
 if(BUILD_WITH_PINOCCHIO)
   target_compile_definitions(all_tests PRIVATE -DBUILD_WITH_PINOCCHIO)
diff --git a/cpp/tests/test_cubic_spline.cpp b/cpp/tests/test_cubic_spline.cpp
new file mode 100644
index 00000000..c9b60cdb
--- /dev/null
+++ b/cpp/tests/test_cubic_spline.cpp
@@ -0,0 +1,146 @@
+#include <toppra/geometric_path/piecewise_poly_path.hpp>
+#include <toppra/toppra.hpp>
+#include <iostream>
+#include <algorithm>
+#include "gtest/gtest.h"
+#include "utils.hpp"
+
+namespace toppra {
+
+BoundaryCond makeBoundaryCond(const int order, const std::vector<value_type> &values) {
+    BoundaryCond cond;
+    cond.order = order;
+    cond.values.resize(values.size());
+    for (std::size_t i = 0; i < values.size(); i++) cond.values(i) = values[i];
+    return cond;
+}
+
+class CubicSpline : public testing::Test {
+protected:
+    CubicSpline() {}
+    virtual ~CubicSpline() {}
+    static void SetUpTestSuite() {
+        positions =
+                makeVectors({{1.3, 2.1, 4.35, 2.14, -7.31, 4.31},
+                             {1.5, -4.3, 1.23, -4.3, 2.13, 6.24},
+                             {-3.78, 1.53, 8.12, 12.75, 9.11, 5.42},
+                             {6.25, 8.12, 9.52, 20.42, 5.21, 8.31},
+                             {7.31, 3.53, 8.41, 9.56, -3.15, 4.83}});
+        times.resize (5);
+        times << 0.9, 1.3, 2.2, 2.4, 2.6;
+    }
+
+    void ConstructCubicSpline(const Vectors &positions, const Vector &times,
+                              const std::array<BoundaryCond, 2> &bc_type) {
+        path = toppra::PiecewisePolyPath(positions, times, bc_type);
+    }
+
+    void AssertSplineKnots() {
+        Vectors actual_positions = path.eval(times, 0);
+        for (size_t i = 0; i < actual_positions.size(); i++) {
+            ASSERT_TRUE(positions[i].isApprox(actual_positions[i]));
+        }
+    }
+
+    void AssertSplineBoundaryConditions(const std::array<BoundaryCond, 2> &bc_type) {
+        ASSERT_TRUE((path.eval_single(times(0), bc_type[0].order) - bc_type[0].values).norm() < TOPPRA_NEARLY_ZERO);
+        ASSERT_TRUE((path.eval_single(times(times.rows() - 1), bc_type[1].order) - bc_type[1].values).norm()
+            < TOPPRA_NEARLY_ZERO);
+    }
+
+    toppra::PiecewisePolyPath path;
+    static Vectors positions;
+    static Vector times;
+};
+
+Vectors CubicSpline::positions;
+Vector CubicSpline::times;
+
+TEST_F(CubicSpline, ClampedSpline) {
+    BoundaryCond bc = makeBoundaryCond(1, {0, 0, 0, 0, 0, 0});
+    std::array<BoundaryCond, 2> bc_type {bc, bc};
+    ConstructCubicSpline(positions, times, bc_type);
+    AssertSplineKnots();
+    AssertSplineBoundaryConditions(bc_type);
+}
+
+TEST_F(CubicSpline, NaturalSpline) {
+    BoundaryCond bc = makeBoundaryCond(2, {0, 0, 0, 0, 0, 0});
+    std::array<BoundaryCond, 2> bc_type {bc, bc};
+    ConstructCubicSpline(positions, times, bc_type);
+    AssertSplineKnots();
+    AssertSplineBoundaryConditions(bc_type);
+}
+
+TEST_F(CubicSpline, FirstOrderBoundaryConditions) {
+    std::array<BoundaryCond, 2> bc_type {makeBoundaryCond(1, {1.25, 0, 4.12, 1.75, 7.43, 5.31}),
+                                         makeBoundaryCond(1, {3.51, 5.32, 4.63, 0, -3.12, 3.53})};
+    ConstructCubicSpline(positions, times, bc_type);
+    AssertSplineKnots();
+    AssertSplineBoundaryConditions(bc_type);
+}
+
+TEST_F(CubicSpline, SecondOrderBoundaryConditions) {
+    std::array<BoundaryCond, 2> bc_type {makeBoundaryCond(2, {1.52, -4.21, 7.21, 9.31, -1.53, 7.54}),
+                                         makeBoundaryCond(2, {-5.12, 8.21, 9.12, 5.12, 24.12, 9.42})};
+    ConstructCubicSpline(positions, times, bc_type);
+    AssertSplineKnots();
+    AssertSplineBoundaryConditions(bc_type);
+}
+
+TEST_F(CubicSpline, FirstOrderAndSecondOrderBoundaryConditions) {
+    std::array<BoundaryCond, 2> bc_type {makeBoundaryCond(1, {1.52, -4.21, 7.21, 9.31, -1.53, 7.54}),
+                                         makeBoundaryCond(2, {-5.12, 8.21, 9.12, 5.12, 24.12, 9.42})};
+    ConstructCubicSpline(positions, times, bc_type);
+    AssertSplineKnots();
+    AssertSplineBoundaryConditions(bc_type);
+}
+
+TEST_F(CubicSpline, BadPositions) {
+    Vectors bad_positions = makeVectors({{1.3, 2.1, 4.35, 2.14, -7.31, 4.31},
+                                         {1.5, -4.3, 1.23, -4.3, 2.13, 6.24},
+                                         {-3.78, 1.53, 8.12, 12.75, 9.11, 5.42},
+                                         {6.25, 8.12, 9.52, 5.21, 8.31},
+                                         {7.31, 3.53, 8.41, 9.56, -3.15, 4.83}});
+    std::array<BoundaryCond, 2> bc_type {makeBoundaryCond(2, {1.52, -4.21, 7.21, 9.31, -1.53, 7.54}),
+                                         makeBoundaryCond(2, {-5.12, 8.21, 9.12, 5.12, 24.12, 9.42})};
+    ASSERT_THROW(ConstructCubicSpline(bad_positions, times, bc_type), std::runtime_error);
+
+    bad_positions = makeVectors({{1.3, 2.1, 4.35, 2.14, -7.31, 4.31},
+                                 {1.5, -4.3, 1.23, -4.3, 2.13, 6.24},
+                                 {-3.78, 1.53, 8.12, 12.75, 9.11, 5.42},
+                                 {6.25, 8.12, 11.23, 9.52, 5.21, 8.31}});
+    ASSERT_THROW(ConstructCubicSpline(bad_positions, times, bc_type), std::runtime_error);
+}
+
+TEST_F(CubicSpline, BadTimes) {
+    std::array<BoundaryCond, 2> bc_type {makeBoundaryCond(2, {1.52, -4.21, 7.21, 9.31, -1.53, 7.54}),
+                                         makeBoundaryCond(2, {-5.12, 8.21, 9.12, 5.12, 24.12, 9.42})};
+    Vector bad_times (1);
+    bad_times << 1;
+    ASSERT_THROW(ConstructCubicSpline(positions, bad_times, bc_type), std::runtime_error);
+
+    bad_times.resize(4);
+    bad_times << 0.11, 1.23, 4.35, 6.75;
+    ASSERT_THROW(ConstructCubicSpline(positions, bad_times, bc_type), std::runtime_error);
+
+    bad_times.resize(5);
+    bad_times << 0.11, 1.23, 4.35, 6.75, 1.23;
+    ASSERT_THROW(ConstructCubicSpline(positions, bad_times, bc_type), std::runtime_error);
+}
+
+TEST_F(CubicSpline, BadBoundaryConditions) {
+    std::array<BoundaryCond, 2> bc_type {makeBoundaryCond(2, {1.52, 7.21, 9.31, -1.53, 7.54}),
+                                         makeBoundaryCond(2, {-5.12, 8.21, 9.12, 5.12, 24.12, 9.42})};
+    ASSERT_THROW(ConstructCubicSpline(positions, times, bc_type), std::runtime_error);
+
+    bc_type = {makeBoundaryCond(2, {1.52, 7.21, 9.31}),
+               makeBoundaryCond(2, {-5.12, 8.21, 9.12})};
+    ASSERT_THROW(ConstructCubicSpline(positions, times, bc_type), std::runtime_error);
+
+    bc_type = {makeBoundaryCond(3, {1.52, 7.21, 9.31, 2.52, 4.41, 5.54}),
+               makeBoundaryCond(3, {-5.12, 8.21, 9.12, -1.32, 3.53, 9.21})};
+    ASSERT_THROW(ConstructCubicSpline(positions, times, bc_type), std::runtime_error);
+}
+
+}  // namespace toppra
diff --git a/cpp/tests/test_hermite_spline.cpp b/cpp/tests/test_hermite_spline.cpp
index ccfb6e8b..75d2858d 100644
--- a/cpp/tests/test_hermite_spline.cpp
+++ b/cpp/tests/test_hermite_spline.cpp
@@ -1,20 +1,10 @@
 #include <toppra/geometric_path/piecewise_poly_path.hpp>
 #include <toppra/toppra.hpp>
 #include "gtest/gtest.h"
+#include "utils.hpp"
 
 namespace toppra {
 
-using vvvectors = std::vector<std::vector<value_type> >;
-Vectors makeVectors(vvvectors v) {
-  Vectors ret;
-  for (auto vi : v) {
-    Vector vi_eigen(vi.size());
-    for (std::size_t i = 0; i < vi.size(); i++) vi_eigen(i) = vi[i];
-    ret.push_back(vi_eigen);
-  }
-  return ret;
-}
-
 TEST(SplineHermite, BasicUsage) {
   toppra::Vectors pos = makeVectors({{0, 0}, {1, 1}, {0, 2}});
   toppra::Vectors vec = makeVectors({{1, 1}, {0, 0}, {1, 2}});
diff --git a/cpp/tests/utils.hpp b/cpp/tests/utils.hpp
new file mode 100644
index 00000000..c887d61d
--- /dev/null
+++ b/cpp/tests/utils.hpp
@@ -0,0 +1,19 @@
+#ifndef TOPPRA_TEST_UTILS_HPP
+#define TOPPRA_TEST_UTILS_HPP
+
+namespace toppra {
+
+using vvvectors = std::vector<std::vector<value_type> >;
+inline Vectors makeVectors(vvvectors v) {
+    Vectors ret;
+    for (auto vi : v) {
+        Vector vi_eigen(vi.size());
+        for (std::size_t i = 0; i < vi.size(); i++) vi_eigen(i) = vi[i];
+        ret.push_back(vi_eigen);
+        }
+    return ret;
+}
+
+}  // namespace toppra
+
+#endif