[tube] polynomial representation of tubes

python/src/core/domains/tube/codac_py_TubeVector.cpp

@@ -424,8 +424,8 @@ void export_TubeVector(py::module& m)
   // Tree synthesis structure
 
     .def("enable_synthesis", &TubeVector::enable_synthesis,
-      TUBEVECTOR_VOID_ENABLE_SYNTHESIS_SYNTHESISMODE,
-      "enable"_a=SynthesisMode::BINARY_TREE)
+      TUBEVECTOR_VOID_ENABLE_SYNTHESIS_SYNTHESISMODE_DOUBLE,
+      "enable"_a=SynthesisMode::BINARY_TREE, "eps"_a=1.e-3)
 
     .def("integral", (const IntervalVector (TubeVector::*)(double) const)&TubeVector::integral,
       TUBEVECTOR_CONSTINTERVALVECTOR_INTEGRAL_DOUBLE,

python/src/robotics/codac_py_TPlane.cpp

@@ -44,7 +44,7 @@ void export_TPlane(py::module& m)
         p_.enable_synthesis(SynthesisMode::BINARY_TREE);
         v_.enable_synthesis(SynthesisMode::BINARY_TREE);
         tplane.compute_detections(precision, p_, v_);
-        //p_.enable_synthesis(SynthesisMode::POLYNOMIAL);
+        //p_.enable_synthesis(SynthesisMode::POLYNOMIAL, 5.e-7);
         tplane.compute_proofs(p_, v_);
       },
       TPLANE_VOID_COMPUTE_DETECTIONS_FLOAT_TUBEVECTOR_TUBEVECTOR,

src/core/CMakeLists.txt

@@ -69,6 +69,7 @@
                   ${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_Tube_operators.cpp
                   ${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubePolynomialSynthesis.h
                   ${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubePolynomialSynthesis.cpp
+                  ${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubeSynthesis.h
                   ${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubeTreeSynthesis.h
                   ${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubeTreeSynthesis.cpp
                   ${CMAKE_CURRENT_SOURCE_DIR}/domains/slice/codac_Slice.h

src/core/domains/tube/codac_Tube.h

@@ -22,6 +22,7 @@
 #include "codac_tube_arithmetic.h"
 #include "codac_TubeTreeSynthesis.h"
 #include "codac_TubePolynomialSynthesis.h"
+#include "codac_TubeSynthesis.h"
 #include "codac_Polygon.h"
 #include "codac_BoolInterval.h"
 
@@ -37,8 +38,6 @@ namespace codac
   class TubeTreeSynthesis;
   class TubePolynomialSynthesis;
 
-  enum class SynthesisMode { NONE, BINARY_TREE, POLYNOMIAL };
-
   /**
    * \class Tube
    * \brief One dimensional tube \f$[x](\cdot)\f$, defined as an interval of scalar trajectories
@@ -1014,7 +1013,8 @@ namespace codac
        *
        * \note The synthesis tree speeds up computations such as integrals or evaluations
        *
-       * \param enable boolean
+       * \param mode mode of synthesis
+       * \param eps precision of the polynomial approximation, if selected
        */
       void enable_synthesis(SynthesisMode mode = SynthesisMode::BINARY_TREE, double eps = 1.e-3) const;
 

src/core/domains/tube/codac_TubePolynomialSynthesis.cpp

@@ -39,10 +39,12 @@ namespace codac
     m_tube_ref(x),
     m_tdomain(tdomain),
     m_upper_bound(upper_bound),
-    m_p(polynomial_traj(upper_bound)),
+    m_p(polynomial_factoredform(upper_bound)),
+    m_codomain(m_p.f(m_tdomain)),
     m_fig(fig)
   {
-    if(fabs(m_p.offset) > eps)
+    if(fabs(m_p.offset) > eps
+      && x.time_to_index(tdomain.ub()) - x.time_to_index(tdomain.lb()) > 3)
     {
       LargestFirst bisector;
       pair<IntervalVector,IntervalVector> t =
@@ -57,7 +59,7 @@ namespace codac
       ostringstream ostr;
       ostr << m_upper_bound << m_tdomain;
       m_fig.add_trajectory(traj, ostr.str(), m_upper_bound ? "blue" : "red");
-      m_fig.draw_box(IntervalVector({m_tdomain, m_p.codomain}), m_upper_bound ? "blue" : "red");
+      m_fig.draw_box(IntervalVector({m_tdomain, m_codomain}), m_upper_bound ? "blue" : "red");
       // todo: manage delete
     }
   }
@@ -80,24 +82,13 @@ namespace codac
       return m_left->operator()(t) | m_right->operator()(t);
 
     else if(t.is_superset(m_tdomain))
-      return m_p.codomain;
+      return m_codomain;
 
     else
-      return f(m_p, t & m_tdomain);
+      return m_p.f(t & m_tdomain);
   }
 
-  Interval TubePolynomialTreeSynthesis::f(const PolynomialSubtraj& p, const Interval& t) const
-  {
-    if(p.factor_form)
-      return p.c*sqr(t-p.a)+p.b+p.offset;
-
-    Interval x = p.offset;
-    for(int i = 0 ; i < POLYNOMIAL_ORDER + 1 ; i++)
-      x += p.coeff[i]*pow(t,i);
-    return x;
-  }
-
-  void TubePolynomialTreeSynthesis::polyfit(const vector<double> &t, const vector<double> &v, array<double,POLYNOMIAL_ORDER+1> &coeff) const
+  Polynomial TubePolynomialTreeSynthesis::polyfit(const vector<double> &t, const vector<double> &v) const
   {
     // Create a matrix placeholder of size (n x k),
     // n = nb of datapoints,
@@ -113,11 +104,11 @@ namespace codac
         T(i, j) = std::pow(t.at(i), j);
 
     // Linear least square fit
+    Polynomial p;
     result = T.householderQr().solve(V);
     for(int k = 0 ; k < POLYNOMIAL_ORDER + 1 ; k++)
-      coeff[k] = result[k];
-
-
+      p.coeff[k] = result[k];
+    return p;
   }
 
   void TubePolynomialTreeSynthesis::get_bounds(const Interval& tdomain, bool upper_bound, vector<double>& t, vector<double>& v) const
@@ -139,50 +130,51 @@ namespace codac
     assert(t.size() > 3);
   }
 
-  Trajectory TubePolynomialTreeSynthesis::traj_from_polynom(const PolynomialSubtraj& p) const
+  Trajectory TubePolynomialTreeSynthesis::traj_from_polynom(const PolynomialFactoredForm& p) const
   {
     // todo: analytical traj?
     Trajectory traj;
-    double dt = m_tdomain.diam()/1000.;
+    double dt = m_tdomain.diam()/10000.;
     for(double t = m_tdomain.lb() ; t < m_tdomain.ub() + dt ; t += dt)
     {
       double t_ = std::min(t, m_tdomain.ub());
-      traj.set(f(p,t_).mid(), t_);
+      traj.set(p.f(t_).mid(), t_);
     }
     return traj;
   }
 
-  PolynomialSubtraj TubePolynomialTreeSynthesis::polynomial_traj(bool upper_bound) const
+  PolynomialFactoredForm TubePolynomialTreeSynthesis::polynomial_factoredform(bool upper_bound) const
   {
-    PolynomialSubtraj p;
+    PolynomialFactoredForm p_factored;
 
     vector<double> t,v;
     get_bounds(m_tdomain, upper_bound, t, v);
-    polyfit(t, v, p.coeff);
+    Polynomial p = polyfit(t, v);
 
-    double offset = 0.;
-    for(size_t i = 0 ; i < t.size() ; i++)
+    if(p.coeff[2] != 0.) // avoid div by zero
     {
-      Interval diff = v[i]-f(p, t[i]);
-      if(upper_bound)
-        offset = std::max(offset, diff.ub());
-      else
-        offset = std::min(offset, diff.lb());
+      p_factored.a = -p.coeff[1]/(2*p.coeff[2]); 
+      p_factored.b = p.f(p_factored.a).mid();
+
+      double x = m_tdomain.lb();
+      if(x == p_factored.a)
+        x = m_tdomain.ub(); // selecting another x: avoiding div by zero
+
+      p_factored.c = (p.f(x).mid()-p_factored.b)/std::pow(x-p_factored.a,2);
     }
 
-    if(p.coeff[2] != 0.)
+    else
+      assert(false && "TubePolynomialTreeSynthesis: unable to factorize");
+
+    for(size_t i = 0 ; i < t.size() ; i++)
     {
-      p.a = -p.coeff[1]/(2*p.coeff[2]); 
-      p.b = f(p, p.a).mid();
-      if(m_tdomain.mid() != p.a)
-      {
-        p.c = (f(p, m_tdomain.mid()).mid()-p.b)/std::pow(m_tdomain.mid()-p.a,2);
-        p.factor_form = true;
-      }
+      Interval diff = v[i]-p.f(t[i]);
+      if(upper_bound)
+        p_factored.offset = (i == 0) ? diff.ub() : std::max(p_factored.offset, diff.ub());
+      else
+        p_factored.offset = (i == 0) ? diff.lb() : std::min(p_factored.offset, diff.lb());
     }
 
-    p.offset = offset;
-    p.codomain = f(p,m_tdomain);
-    return p;
+    return p_factored;
   }
 }

src/core/domains/tube/codac_TubePolynomialSynthesis.h

@@ -23,13 +23,31 @@
 
 namespace codac
 {
-  struct PolynomialSubtraj
+  struct Polynomial
   {
-    Interval codomain;
+    // Form: ax² + bx + c + offset = 0
     std::array<double,POLYNOMIAL_ORDER+1> coeff;
     double offset = 0.;
+
+    Interval f(const Interval& t) const
+    {
+      Interval x = offset;
+      for(int i = 0 ; i < POLYNOMIAL_ORDER+1 ; i++)
+        x += coeff[i]*pow(t,i);
+      return x;
+    }
+  };
+
+  struct PolynomialFactoredForm
+  {
+    // Form: c(x-a)² + b + offset = 0
     double a, b, c;
-    bool factor_form = false;
+    double offset = 0.;
+
+    Interval f(const Interval& t) const
+    {
+      return c*sqr(t-a) + b + offset;
+    }
   };
 
   class VIBesFigTube;
@@ -45,19 +63,19 @@ namespace codac
 
     protected:
 
-      Interval f(const PolynomialSubtraj& p, const Interval& t) const;
-      void polyfit(const std::vector<double> &t, const std::vector<double> &v, std::array<double,POLYNOMIAL_ORDER+1> &coeff) const;
+      Polynomial polyfit(const std::vector<double> &t, const std::vector<double> &v) const;
       void get_bounds(const Interval& tdomain, bool upper_bound, std::vector<double>& t, std::vector<double>& v) const;
-      Trajectory traj_from_polynom(const PolynomialSubtraj& p) const;
-      PolynomialSubtraj polynomial_traj(bool upper_bound) const;
+      Trajectory traj_from_polynom(const PolynomialFactoredForm& p) const;
+      PolynomialFactoredForm polynomial_factoredform(bool upper_bound) const;
 
 
     protected:
 
       const Tube& m_tube_ref;
       const Interval m_tdomain;
       bool m_upper_bound;
-      PolynomialSubtraj m_p;
+      PolynomialFactoredForm m_p;
+      Interval m_codomain;
       TubePolynomialTreeSynthesis *m_left = nullptr, *m_right = nullptr;
       VIBesFigTube& m_fig;
   };

src/core/domains/tube/codac_TubeSynthesis.h

@@ -0,0 +1,19 @@
+/** 
+ *  TubeSynthesis class
+ * ----------------------------------------------------------------------------
+ *  \date       2022
+ *  \author     Simon Rohou
+ *  \copyright  Copyright 2022 Codac Team
+ *  \license    This program is distributed under the terms of
+ *              the GNU Lesser General Public License (LGPL).
+ */
+
+#ifndef __CODAC_TUBESYNTHESIS_H__
+#define __CODAC_TUBESYNTHESIS_H__
+
+namespace codac
+{
+  enum class SynthesisMode { NONE, BINARY_TREE, POLYNOMIAL };
+}
+
+#endif

src/core/domains/tube/codac_TubeVector.cpp

@@ -877,10 +877,10 @@ namespace codac
 
     // Tree synthesis structure
 
-    void TubeVector::enable_synthesis(SynthesisMode mode) const
+    void TubeVector::enable_synthesis(SynthesisMode mode, double eps) const
     {
       for(int i = 0 ; i < size() ; i++)
-        (*this)[i].enable_synthesis(mode);
+        (*this)[i].enable_synthesis(mode, eps);
     }
 
     // Integration

src/core/domains/tube/codac_TubeVector.h

@@ -21,13 +21,13 @@
 #include "codac_tube_arithmetic.h"
 #include "codac_serialize_tubes.h"
 #include "codac_BoolInterval.h"
+#include "codac_TubeSynthesis.h"
 
 namespace codac
 {
   class TFnc;
   class Tube;
   class Trajectory;
-  enum class SynthesisMode;
 
   /**
    * \class TubeVector
@@ -1022,10 +1022,11 @@ namespace codac
        * \brief Enables the computation of a synthesis tree
        *
        * \note The synthesis tree speeds up computations such as integrals or evaluations
-       *
-       * \param enable boolean
+       *   
+       * \param mode mode of synthesis
+       * \param eps precision of the polynomial approximation, if selected
        */
-      void enable_synthesis(SynthesisMode mode) const;
+      void enable_synthesis(SynthesisMode mode = SynthesisMode::BINARY_TREE, double eps = 1.e-3) const;
 
       /// @}
       /// \name Integration

src/robotics/loops/codac_TPlane.cpp

@@ -147,16 +147,18 @@ namespace codac
   void TPlane::compute_proofs(const TubeVector& p)
   {
     auto f = std::bind(&f_p, p, _1);
-
-    for(size_t i = 0 ; i < m_v_detected_loops.size() ; i++)
-      if(m_v_detected_loops[i].zero_proven(f))
-        m_v_proven_loops.push_back(m_v_detected_loops[i]);
+    compute_proofs(f);
   }
 
   void TPlane::compute_proofs(const TubeVector& p, const TubeVector& v)
   {
-    clock_t t_start = clock();
     auto f = std::bind(&f_pv, p, v, _1);
+    compute_proofs(f);
+  }
+
+  void TPlane::compute_proofs(const function<IntervalVector(const IntervalVector&)>& f)
+  {
+    clock_t t_start = clock();
     m_v_proven_loops.clear();
 
     for(size_t i = 0 ; i < m_v_detected_loops.size() ; i++)

src/robotics/loops/codac_TPlane.h

@@ -171,6 +171,15 @@ namespace codac
 
     protected:
 
+      /**
+       * \brief Tries to prove the existence of loops in each detection set
+       *
+       * \note The tplane must have been computed beforehand.
+       *
+       * \param f the inclusion function \f$[\mathbf{f}]:\mathbb{IR}^2\to\mathbb{IR}^2\f$
+       */
+      void compute_proofs(const std::function<IntervalVector(const IntervalVector&)>& f);
+
       /**
        * \brief Recursive computation of the tplane, from the tube of positions \f$[\mathbf{p}](\cdot)\f$
        *        and the tube of velocities \f$[\mathbf{v}](\cdot)\f$.