Skip to content

Commit f53c7e9

Browse files
committed
[tube] polynomial representation of tubes
1 parent 29ecece commit f53c7e9

File tree

11 files changed

+111
-69
lines changed

11 files changed

+111
-69
lines changed

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -424,8 +424,8 @@ void export_TubeVector(py::module& m)
424424
// Tree synthesis structure
425425

426426
.def("enable_synthesis", &TubeVector::enable_synthesis,
427-
TUBEVECTOR_VOID_ENABLE_SYNTHESIS_SYNTHESISMODE,
428-
"enable"_a=SynthesisMode::BINARY_TREE)
427+
TUBEVECTOR_VOID_ENABLE_SYNTHESIS_SYNTHESISMODE_DOUBLE,
428+
"enable"_a=SynthesisMode::BINARY_TREE, "eps"_a=1.e-3)
429429

430430
.def("integral", (const IntervalVector (TubeVector::*)(double) const)&TubeVector::integral,
431431
TUBEVECTOR_CONSTINTERVALVECTOR_INTEGRAL_DOUBLE,

python/src/robotics/codac_py_TPlane.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ void export_TPlane(py::module& m)
4444
p_.enable_synthesis(SynthesisMode::BINARY_TREE);
4545
v_.enable_synthesis(SynthesisMode::BINARY_TREE);
4646
tplane.compute_detections(precision, p_, v_);
47-
//p_.enable_synthesis(SynthesisMode::POLYNOMIAL);
47+
//p_.enable_synthesis(SynthesisMode::POLYNOMIAL, 5.e-7);
4848
tplane.compute_proofs(p_, v_);
4949
},
5050
TPLANE_VOID_COMPUTE_DETECTIONS_FLOAT_TUBEVECTOR_TUBEVECTOR,

src/core/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -69,6 +69,7 @@
6969
${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_Tube_operators.cpp
7070
${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubePolynomialSynthesis.h
7171
${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubePolynomialSynthesis.cpp
72+
${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubeSynthesis.h
7273
${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubeTreeSynthesis.h
7374
${CMAKE_CURRENT_SOURCE_DIR}/domains/tube/codac_TubeTreeSynthesis.cpp
7475
${CMAKE_CURRENT_SOURCE_DIR}/domains/slice/codac_Slice.h

src/core/domains/tube/codac_Tube.h

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include "codac_tube_arithmetic.h"
2323
#include "codac_TubeTreeSynthesis.h"
2424
#include "codac_TubePolynomialSynthesis.h"
25+
#include "codac_TubeSynthesis.h"
2526
#include "codac_Polygon.h"
2627
#include "codac_BoolInterval.h"
2728

@@ -37,8 +38,6 @@ namespace codac
3738
class TubeTreeSynthesis;
3839
class TubePolynomialSynthesis;
3940

40-
enum class SynthesisMode { NONE, BINARY_TREE, POLYNOMIAL };
41-
4241
/**
4342
* \class Tube
4443
* \brief One dimensional tube \f$[x](\cdot)\f$, defined as an interval of scalar trajectories
@@ -1014,7 +1013,8 @@ namespace codac
10141013
*
10151014
* \note The synthesis tree speeds up computations such as integrals or evaluations
10161015
*
1017-
* \param enable boolean
1016+
* \param mode mode of synthesis
1017+
* \param eps precision of the polynomial approximation, if selected
10181018
*/
10191019
void enable_synthesis(SynthesisMode mode = SynthesisMode::BINARY_TREE, double eps = 1.e-3) const;
10201020

src/core/domains/tube/codac_TubePolynomialSynthesis.cpp

Lines changed: 36 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -39,10 +39,12 @@ namespace codac
3939
m_tube_ref(x),
4040
m_tdomain(tdomain),
4141
m_upper_bound(upper_bound),
42-
m_p(polynomial_traj(upper_bound)),
42+
m_p(polynomial_factoredform(upper_bound)),
43+
m_codomain(m_p.f(m_tdomain)),
4344
m_fig(fig)
4445
{
45-
if(fabs(m_p.offset) > eps)
46+
if(fabs(m_p.offset) > eps
47+
&& x.time_to_index(tdomain.ub()) - x.time_to_index(tdomain.lb()) > 3)
4648
{
4749
LargestFirst bisector;
4850
pair<IntervalVector,IntervalVector> t =
@@ -57,7 +59,7 @@ namespace codac
5759
ostringstream ostr;
5860
ostr << m_upper_bound << m_tdomain;
5961
m_fig.add_trajectory(traj, ostr.str(), m_upper_bound ? "blue" : "red");
60-
m_fig.draw_box(IntervalVector({m_tdomain, m_p.codomain}), m_upper_bound ? "blue" : "red");
62+
m_fig.draw_box(IntervalVector({m_tdomain, m_codomain}), m_upper_bound ? "blue" : "red");
6163
// todo: manage delete
6264
}
6365
}
@@ -80,24 +82,13 @@ namespace codac
8082
return m_left->operator()(t) | m_right->operator()(t);
8183

8284
else if(t.is_superset(m_tdomain))
83-
return m_p.codomain;
85+
return m_codomain;
8486

8587
else
86-
return f(m_p, t & m_tdomain);
88+
return m_p.f(t & m_tdomain);
8789
}
8890

89-
Interval TubePolynomialTreeSynthesis::f(const PolynomialSubtraj& p, const Interval& t) const
90-
{
91-
if(p.factor_form)
92-
return p.c*sqr(t-p.a)+p.b+p.offset;
93-
94-
Interval x = p.offset;
95-
for(int i = 0 ; i < POLYNOMIAL_ORDER + 1 ; i++)
96-
x += p.coeff[i]*pow(t,i);
97-
return x;
98-
}
99-
100-
void TubePolynomialTreeSynthesis::polyfit(const vector<double> &t, const vector<double> &v, array<double,POLYNOMIAL_ORDER+1> &coeff) const
91+
Polynomial TubePolynomialTreeSynthesis::polyfit(const vector<double> &t, const vector<double> &v) const
10192
{
10293
// Create a matrix placeholder of size (n x k),
10394
// n = nb of datapoints,
@@ -113,11 +104,11 @@ namespace codac
113104
T(i, j) = std::pow(t.at(i), j);
114105

115106
// Linear least square fit
107+
Polynomial p;
116108
result = T.householderQr().solve(V);
117109
for(int k = 0 ; k < POLYNOMIAL_ORDER + 1 ; k++)
118-
coeff[k] = result[k];
119-
120-
110+
p.coeff[k] = result[k];
111+
return p;
121112
}
122113

123114
void TubePolynomialTreeSynthesis::get_bounds(const Interval& tdomain, bool upper_bound, vector<double>& t, vector<double>& v) const
@@ -139,50 +130,51 @@ namespace codac
139130
assert(t.size() > 3);
140131
}
141132

142-
Trajectory TubePolynomialTreeSynthesis::traj_from_polynom(const PolynomialSubtraj& p) const
133+
Trajectory TubePolynomialTreeSynthesis::traj_from_polynom(const PolynomialFactoredForm& p) const
143134
{
144135
// todo: analytical traj?
145136
Trajectory traj;
146-
double dt = m_tdomain.diam()/1000.;
137+
double dt = m_tdomain.diam()/10000.;
147138
for(double t = m_tdomain.lb() ; t < m_tdomain.ub() + dt ; t += dt)
148139
{
149140
double t_ = std::min(t, m_tdomain.ub());
150-
traj.set(f(p,t_).mid(), t_);
141+
traj.set(p.f(t_).mid(), t_);
151142
}
152143
return traj;
153144
}
154145

155-
PolynomialSubtraj TubePolynomialTreeSynthesis::polynomial_traj(bool upper_bound) const
146+
PolynomialFactoredForm TubePolynomialTreeSynthesis::polynomial_factoredform(bool upper_bound) const
156147
{
157-
PolynomialSubtraj p;
148+
PolynomialFactoredForm p_factored;
158149

159150
vector<double> t,v;
160151
get_bounds(m_tdomain, upper_bound, t, v);
161-
polyfit(t, v, p.coeff);
152+
Polynomial p = polyfit(t, v);
162153

163-
double offset = 0.;
164-
for(size_t i = 0 ; i < t.size() ; i++)
154+
if(p.coeff[2] != 0.) // avoid div by zero
165155
{
166-
Interval diff = v[i]-f(p, t[i]);
167-
if(upper_bound)
168-
offset = std::max(offset, diff.ub());
169-
else
170-
offset = std::min(offset, diff.lb());
156+
p_factored.a = -p.coeff[1]/(2*p.coeff[2]);
157+
p_factored.b = p.f(p_factored.a).mid();
158+
159+
double x = m_tdomain.lb();
160+
if(x == p_factored.a)
161+
x = m_tdomain.ub(); // selecting another x: avoiding div by zero
162+
163+
p_factored.c = (p.f(x).mid()-p_factored.b)/std::pow(x-p_factored.a,2);
171164
}
172165

173-
if(p.coeff[2] != 0.)
166+
else
167+
assert(false && "TubePolynomialTreeSynthesis: unable to factorize");
168+
169+
for(size_t i = 0 ; i < t.size() ; i++)
174170
{
175-
p.a = -p.coeff[1]/(2*p.coeff[2]);
176-
p.b = f(p, p.a).mid();
177-
if(m_tdomain.mid() != p.a)
178-
{
179-
p.c = (f(p, m_tdomain.mid()).mid()-p.b)/std::pow(m_tdomain.mid()-p.a,2);
180-
p.factor_form = true;
181-
}
171+
Interval diff = v[i]-p.f(t[i]);
172+
if(upper_bound)
173+
p_factored.offset = (i == 0) ? diff.ub() : std::max(p_factored.offset, diff.ub());
174+
else
175+
p_factored.offset = (i == 0) ? diff.lb() : std::min(p_factored.offset, diff.lb());
182176
}
183177

184-
p.offset = offset;
185-
p.codomain = f(p,m_tdomain);
186-
return p;
178+
return p_factored;
187179
}
188180
}

src/core/domains/tube/codac_TubePolynomialSynthesis.h

Lines changed: 26 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -23,13 +23,31 @@
2323

2424
namespace codac
2525
{
26-
struct PolynomialSubtraj
26+
struct Polynomial
2727
{
28-
Interval codomain;
28+
// Form: ax² + bx + c + offset = 0
2929
std::array<double,POLYNOMIAL_ORDER+1> coeff;
3030
double offset = 0.;
31+
32+
Interval f(const Interval& t) const
33+
{
34+
Interval x = offset;
35+
for(int i = 0 ; i < POLYNOMIAL_ORDER+1 ; i++)
36+
x += coeff[i]*pow(t,i);
37+
return x;
38+
}
39+
};
40+
41+
struct PolynomialFactoredForm
42+
{
43+
// Form: c(x-a)² + b + offset = 0
3144
double a, b, c;
32-
bool factor_form = false;
45+
double offset = 0.;
46+
47+
Interval f(const Interval& t) const
48+
{
49+
return c*sqr(t-a) + b + offset;
50+
}
3351
};
3452

3553
class VIBesFigTube;
@@ -45,19 +63,19 @@ namespace codac
4563

4664
protected:
4765

48-
Interval f(const PolynomialSubtraj& p, const Interval& t) const;
49-
void polyfit(const std::vector<double> &t, const std::vector<double> &v, std::array<double,POLYNOMIAL_ORDER+1> &coeff) const;
66+
Polynomial polyfit(const std::vector<double> &t, const std::vector<double> &v) const;
5067
void get_bounds(const Interval& tdomain, bool upper_bound, std::vector<double>& t, std::vector<double>& v) const;
51-
Trajectory traj_from_polynom(const PolynomialSubtraj& p) const;
52-
PolynomialSubtraj polynomial_traj(bool upper_bound) const;
68+
Trajectory traj_from_polynom(const PolynomialFactoredForm& p) const;
69+
PolynomialFactoredForm polynomial_factoredform(bool upper_bound) const;
5370

5471

5572
protected:
5673

5774
const Tube& m_tube_ref;
5875
const Interval m_tdomain;
5976
bool m_upper_bound;
60-
PolynomialSubtraj m_p;
77+
PolynomialFactoredForm m_p;
78+
Interval m_codomain;
6179
TubePolynomialTreeSynthesis *m_left = nullptr, *m_right = nullptr;
6280
VIBesFigTube& m_fig;
6381
};
Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
/**
2+
* TubeSynthesis class
3+
* ----------------------------------------------------------------------------
4+
* \date 2022
5+
* \author Simon Rohou
6+
* \copyright Copyright 2022 Codac Team
7+
* \license This program is distributed under the terms of
8+
* the GNU Lesser General Public License (LGPL).
9+
*/
10+
11+
#ifndef __CODAC_TUBESYNTHESIS_H__
12+
#define __CODAC_TUBESYNTHESIS_H__
13+
14+
namespace codac
15+
{
16+
enum class SynthesisMode { NONE, BINARY_TREE, POLYNOMIAL };
17+
}
18+
19+
#endif

src/core/domains/tube/codac_TubeVector.cpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -877,10 +877,10 @@ namespace codac
877877

878878
// Tree synthesis structure
879879

880-
void TubeVector::enable_synthesis(SynthesisMode mode) const
880+
void TubeVector::enable_synthesis(SynthesisMode mode, double eps) const
881881
{
882882
for(int i = 0 ; i < size() ; i++)
883-
(*this)[i].enable_synthesis(mode);
883+
(*this)[i].enable_synthesis(mode, eps);
884884
}
885885

886886
// Integration

src/core/domains/tube/codac_TubeVector.h

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -21,13 +21,13 @@
2121
#include "codac_tube_arithmetic.h"
2222
#include "codac_serialize_tubes.h"
2323
#include "codac_BoolInterval.h"
24+
#include "codac_TubeSynthesis.h"
2425

2526
namespace codac
2627
{
2728
class TFnc;
2829
class Tube;
2930
class Trajectory;
30-
enum class SynthesisMode;
3131

3232
/**
3333
* \class TubeVector
@@ -1022,10 +1022,11 @@ namespace codac
10221022
* \brief Enables the computation of a synthesis tree
10231023
*
10241024
* \note The synthesis tree speeds up computations such as integrals or evaluations
1025-
*
1026-
* \param enable boolean
1025+
*
1026+
* \param mode mode of synthesis
1027+
* \param eps precision of the polynomial approximation, if selected
10271028
*/
1028-
void enable_synthesis(SynthesisMode mode) const;
1029+
void enable_synthesis(SynthesisMode mode = SynthesisMode::BINARY_TREE, double eps = 1.e-3) const;
10291030

10301031
/// @}
10311032
/// \name Integration

src/robotics/loops/codac_TPlane.cpp

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -147,16 +147,18 @@ namespace codac
147147
void TPlane::compute_proofs(const TubeVector& p)
148148
{
149149
auto f = std::bind(&f_p, p, _1);
150-
151-
for(size_t i = 0 ; i < m_v_detected_loops.size() ; i++)
152-
if(m_v_detected_loops[i].zero_proven(f))
153-
m_v_proven_loops.push_back(m_v_detected_loops[i]);
150+
compute_proofs(f);
154151
}
155152

156153
void TPlane::compute_proofs(const TubeVector& p, const TubeVector& v)
157154
{
158-
clock_t t_start = clock();
159155
auto f = std::bind(&f_pv, p, v, _1);
156+
compute_proofs(f);
157+
}
158+
159+
void TPlane::compute_proofs(const function<IntervalVector(const IntervalVector&)>& f)
160+
{
161+
clock_t t_start = clock();
160162
m_v_proven_loops.clear();
161163

162164
for(size_t i = 0 ; i < m_v_detected_loops.size() ; i++)

0 commit comments

Comments
 (0)