Skip to content

Commit

Permalink
[ellips] updates (homogenization + Eigen optimizations)
Browse files Browse the repository at this point in the history
  • Loading branch information
SimonRohou committed Dec 15, 2024
1 parent bce42f6 commit a81e68d
Show file tree
Hide file tree
Showing 15 changed files with 226 additions and 167 deletions.
87 changes: 51 additions & 36 deletions examples/ellipsoid_example/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,11 @@ int main() {
fig1.set_window_properties({0, 100}, {500, 500});

// initial ellipsoid
Vector mu({1., 0.});
Matrix G({{0.05, 0.0},
{0., 0.05}});
Ellipsoid e1(mu, G);
Ellipsoid e1(
{1., 0.}, // mu
{{0.05, 0.0}, // G
{0., 0.05}}
);
fig1.draw_ellipsoid(e1, {Color::red(), Color::red(0.3)});
cout << "Initial ellipsoid e1 (red):" << endl;
cout << e1 << endl;
Expand Down Expand Up @@ -51,30 +52,35 @@ int main() {
int Np = 200;
for (int i = 0; i < Np; i++) {
Vector x0 = e1.rand();
fig1.draw_box(IntervalVector(x0).inflate(0.0001), {Color::black(), Color::black(0.3)});
fig1.draw_point(x0, {Color::black(), Color::black(0.3)});
for (int j = 0; j < N; j++) {
x0 = h.eval(x0).mid();
fig1.draw_box(IntervalVector(x0).inflate(0.0001), {Color::black(), Color::black(0.3)});
fig1.draw_point(x0, {Color::black(), Color::black(0.3)});
}
}

// ----------------------------------------------------------
// ellipsoid projections
// ----------------------------------------------------------

Vector mu4({1., 0., 0.});
Matrix G4({{1., 0.5, 0.},
{0.5, 2., 0.2},
{0., 0.2, 3.}});
Ellipsoid e4(mu4, G4);

Matrix G5 = 0.7 * G4;
Ellipsoid e5(mu4, G5);

Matrix G6({{2., 0., 0.5},
{0., 1., 0.2},
{0., 0.2, 3.}});
Ellipsoid e6(mu4, G6);
Ellipsoid e4 {
{1., 0., 0.}, // mu
{{1., 0.5, 0.}, // G
{0.5, 2., 0.2},
{0., 0.2, 3.}}
};

Ellipsoid e5 {
e4.mu, // mu
0.7 * e4.G // G
};

Ellipsoid e6 {
e4.mu, // mu
{{2., 0., 0.5}, // G
{0., 1., 0.2},
{0., 0.2, 3.}}
};

Figure2D fig2("Projected ellipsoid xy", GraphicOutput::VIBES);
Figure2D fig3("Projected ellipsoid yz", GraphicOutput::VIBES);
Expand Down Expand Up @@ -102,11 +108,10 @@ int main() {

// particle cloud (draw the evolution of 200 points in the ellipsoid e5)
for (int i = 0; i < Np; i++) {
IntervalVector x5(e5.rand());
x5.inflate(0.001);
fig2.draw_box(x5, {Color::black(), Color::black(0.3)});
fig3.draw_box(x5, {Color::black(), Color::black(0.3)});
fig4.draw_box(x5, {Color::black(), Color::black(0.3)});
Vector x5 = e5.rand();
fig2.draw_point(x5, {Color::black(), Color::black(0.3)});
fig3.draw_point(x5, {Color::black(), Color::black(0.3)});
fig4.draw_point(x5, {Color::black(), Color::black(0.3)});
}

// ----------------------------------------------------------
Expand Down Expand Up @@ -149,10 +154,16 @@ int main() {
fig5.set_axes(axis(0, {-0.5, 2}), axis(1, {-1.5, 1.}));
fig5.set_window_properties({700, 600}, {500, 500});

Ellipsoid e9(Vector({0., 0.5}), Matrix({{0.25, 0.},
{0., 0.}}));
Ellipsoid e10(Vector({0., -0.5}), Matrix({{0.25, 0.},
{0., 0.25}}));
Ellipsoid e9 {
{0., 0.5}, // mu
{{0.25, 0.}, // G
{0., 0.}}
};
Ellipsoid e10 {
{0., -0.5}, // mu
{{0.25, 0.}, // G
{0., 0.25}}
};

fig5.draw_ellipsoid(e9, {Color::blue(), Color::red(0.3)});
fig5.draw_ellipsoid(e10, {Color::red(), Color::red(0.3)});
Expand All @@ -176,14 +187,12 @@ int main() {
// particle cloud (draw the evolution of 200 points in the ellipsoid)
for (int i = 0; i < Np; i++) {
Vector x0 = e9.rand();
fig5.draw_box(IntervalVector(x0).inflate(0.0001), {Color::black(), Color::black(0.3)});
x0 = h2.eval(x0).mid();
fig5.draw_box(IntervalVector(x0).inflate(0.0001), {Color::black(), Color::black(0.3)});
fig5.draw_point(x0, {Color::black(), Color::black(0.3)});
fig5.draw_point(h2.eval(x0).mid(), {Color::black(), Color::black(0.3)});

x0 = e10.rand();
fig5.draw_box(IntervalVector(x0).inflate(0.0001), {Color::black(), Color::black(0.3)});
x0 = h3.eval(x0).mid();
fig5.draw_box(IntervalVector(x0).inflate(0.0001), {Color::black(), Color::black(0.3)});
fig5.draw_point(x0, {Color::black(), Color::black(0.3)});
fig5.draw_point(h3.eval(x0).mid(), {Color::black(), Color::black(0.3)});
}

// ----------------------------------------------------------
Expand All @@ -193,8 +202,14 @@ int main() {
// pendulum example
AnalyticFunction h4{
{x}, vec(x[0] + 0.5 * x[1] , x[1] + 0.5 * (-x[1]-sin(x[0])))};
Ellipsoid e13(Vector(2), Matrix(2,2));
Ellipsoid e13_out(Vector(2), Matrix(2,2));
Ellipsoid e13 {
Vector::zero(2), // mu
Matrix::zero(2,2) // G
};
Ellipsoid e13_out {
Vector::zero(2), // mu
Matrix::zero(2,2) // G
};
int alpha_max = 1;

if(stability_analysis(h4,alpha_max, e13, e13_out) == BoolInterval::TRUE)
Expand Down
1 change: 1 addition & 0 deletions python/src/core/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
contractors/codac2_py_linear_ctc.cpp

domains/ellipsoid/codac2_py_Ellipsoid.cpp
domains/ellipsoid/codac2_py_Ellipsoid_utils.cpp
domains/interval/codac2_py_Interval.cpp
domains/interval/codac2_py_Interval_operations.cpp
domains/interval/codac2_py_IntervalMatrix.cpp
Expand Down
2 changes: 2 additions & 0 deletions python/src/core/codac2_py_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ void export_linear_ctc(py::module& m);
// domains
void export_BoolInterval(py::module& m);
void export_Ellipsoid(py::module& m);
void export_Ellipsoid_utils(py::module& m);
py::class_<Interval> export_Interval(py::module& m);
void export_Interval_operations(py::module& m, py::class_<Interval>& py_Interval);
py::class_<IntervalRow> export_IntervalRow(py::module& m);
Expand Down Expand Up @@ -164,6 +165,7 @@ PYBIND11_MODULE(_core, m)
// domains
export_BoolInterval(m);
export_Ellipsoid(m);
export_Ellipsoid_utils(m);
auto py_Interval = export_Interval(m);
export_Interval_operations(m, py_Interval);
auto py_IR = export_IntervalRow(m);
Expand Down
23 changes: 14 additions & 9 deletions python/src/core/domains/ellipsoid/codac2_py_Ellipsoid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ void export_Ellipsoid(py::module& m)
.def_readwrite("G", &Ellipsoid::G,
MATRIX_ELLIPSOID_G)

.def(py::init<size_t>(),
.def(py::init<Index>(),
ELLIPSOID_ELLIPSOID_INDEX,
"n"_a)

Expand All @@ -40,13 +40,6 @@ void export_Ellipsoid(py::module& m)
.def(py::init<const Ellipsoid&>(),
"e"_a)

.def("__repr__", [](const Ellipsoid& e) {
std::ostringstream s;
s << e;
return string(s.str());
},
OSTREAM_REF_OPERATOROUT_OSTREAM_REF_CONST_ELLIPSOID_REF)

.def("size", &Ellipsoid::size,
INDEX_ELLIPSOID_SIZE_CONST)

Expand All @@ -60,8 +53,20 @@ void export_Ellipsoid(py::module& m)
BOOLINTERVAL_ELLIPSOID_IS_CONCENTRIC_SUBSET_CONST_ELLIPSOID_REF_CONST,
"e"_a)

.def("proj_2d", &Ellipsoid::proj_2d,
ELLIPSOID_ELLIPSOID_PROJ_2D_CONST_VECTOR_REF_CONST_VECTOR_REF_CONST_VECTOR_REF_CONST,
"d"_a, "v"_a, "u"_a)

.def("__repr__", [](const Ellipsoid& e) {
std::ostringstream s;
s << e;
return string(s.str());
},
OSTREAM_REF_OPERATOROUT_OSTREAM_REF_CONST_ELLIPSOID_REF)

.def(py::self + py::self,
ELLIPSOID_OPERATORPLUS_CONST_ELLIPSOID_REF_CONST_ELLIPSOID_REF)
ELLIPSOID_OPERATORPLUS_CONST_ELLIPSOID_REF_CONST_ELLIPSOID_REF,
py::is_operator())

;

Expand Down
35 changes: 35 additions & 0 deletions python/src/core/domains/ellipsoid/codac2_py_Ellipsoid_utils.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,35 @@
/**
* Codac binding (core)
* ----------------------------------------------------------------------------
* \date 2024
* \author Simon Rohou
* \copyright Copyright 2024 Codac Team
* \license GNU Lesser General Public License (LGPL)
*/

#include <sstream>
#include <pybind11/pybind11.h>
#include <pybind11/operators.h>
#include <pybind11/stl.h>
#include <codac2_Ellipsoid_utils.h>
#include "codac2_py_Ellipsoid_utils_docs.h" // Generated file from Doxygen XML (doxygen2docstring.py):

using namespace std;
using namespace codac2;
namespace py = pybind11;
using namespace pybind11::literals;

void export_Ellipsoid_utils(py::module& m)
{
m

.def("stability_analysis", &codac2::stability_analysis,
BOOLINTERVAL_STABILITY_ANALYSIS_CONST_ANALYTICFUNCTION_VECTOROPVALUE_REF_UNSIGNED_INT_ELLIPSOID_REF_ELLIPSOID_REF_BOOL,
"f"_a, "alpha_max"_a, "e"_a, "e_out"_a, "verbose"_a=false)

.def("solve_discrete_lyapunov", &codac2::solve_discrete_lyapunov,
MATRIX_SOLVE_DISCRETE_LYAPUNOV_CONST_MATRIX_REF_CONST_MATRIX_REF,
"A"_a, "Q"_a)

;
}
6 changes: 6 additions & 0 deletions python/src/graphics/figures/codac2_py_Figure2D.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,12 @@ void export_Figure2D(py::module& m)
VOID_FIGURE2D_SET_AXES_CONST_FIGUREAXIS_REF_CONST_FIGUREAXIS_REF,
"axis1"_a, "axis2"_a)

.def("i", &Figure2D::i,
CONST_INDEX_REF_FIGURE2D_I_CONST)

.def("j", &Figure2D::j,
CONST_INDEX_REF_FIGURE2D_J_CONST)

.def("pos", &Figure2D::pos,
CONST_VECTOR_REF_FIGURE2D_POS_CONST)

Expand Down
69 changes: 32 additions & 37 deletions src/core/domains/ellipsoid/codac2_Ellipsoid.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ using codac2::Vector;

namespace codac2 {
Ellipsoid::Ellipsoid(Index n)
: mu(Vector(n)), G(Matrix(n, n)) {
: mu(Vector::zero(n)), G(Matrix::zero(n, n)) {
assert_release(n > 0);
}

Expand All @@ -27,6 +27,7 @@ namespace codac2 {
}

Index Ellipsoid::size() const {
assert(mu.size() == G.cols() && G.is_squared());
return mu.size();
}

Expand Down Expand Up @@ -89,13 +90,13 @@ namespace codac2 {
return BoolInterval::TRUE;
}

void Ellipsoid::projection2D(const Vector& d, const Vector& v, const Vector& u)
Ellipsoid Ellipsoid::proj_2d(const Vector& d, const Vector& v, const Vector& u) const
{
// from [Pope S. B. - Algorithms for Ellipsoids - 2008]
assert( d.size() == v.size());
assert( d.size() == u.size());
assert( d.size() == this->size());
assert( (v.transpose()*u).norm() == 0); // u & v orthogonal
assert_release( d.size() == v.size());
assert_release( d.size() == u.size());
assert_release( d.size() == this->size());
assert_release( (v.transpose()*u).norm() == 0); // u & v orthogonal

// Normalized Projection matrix
// the plane (d,u,v) is also the affine plan {x|x=d+Tt} with T = [u,v]
Expand All @@ -104,22 +105,19 @@ namespace codac2 {
T.col(1) = u/u.norm();

auto TTG = T.transpose() * this->G;
Eigen::BDCSVD<Eigen::MatrixXd> bdcsvd(TTG, Eigen::ComputeFullU);
Matrix U(bdcsvd.matrixU());
Matrix E((Eigen::MatrixXd) bdcsvd.singularValues().asDiagonal());
this->G = U * E;
this->mu = T.transpose() * (d + T * T.transpose() * (this->mu - d));
Eigen::BDCSVD<Matrix,Eigen::ComputeFullU> bdcsvd(TTG);
auto U = bdcsvd.matrixU();
auto E = bdcsvd.singularValues().asDiagonal();

return {
T.transpose() * (d + T * T.transpose() * (this->mu - d)),
U * E
};
}

Ellipsoid operator+(const Ellipsoid &e1, const Ellipsoid &e2) {
assert_release(e1.size() == e2.size());

//if(e1.is_unbounded() || e2.is_unbounded())
// return Ellipsoid(e1.size());

//if(e1.is_empty() || e2.is_empty())
// return Ellipsoid::empty(e1.size());

auto Q1 = e1.G * e1.G.transpose();
auto Q2 = e2.G * e2.G.transpose();

Expand Down Expand Up @@ -176,14 +174,13 @@ namespace codac2 {
assert(G.is_squared() && J.is_squared() && J_box.is_squared());
assert(n == J.cols() && n == J_box.cols() && n == q.size());

Matrix JG = J * G; // note: reliability may be lost here!
IntervalMatrix G_(G);
IntervalMatrix JG_ = IntervalMatrix(JG);
IntervalVector unit_box(G.rows()); unit_box.init(Interval(-1,1));
auto JG = J * G; // note: reliability may be lost here!
auto G_ = G.template cast<Interval>();
IntervalVector unit_box = IntervalVector::constant(G.rows(), {-1,1});

// normal case
IntervalMatrix I_ = IntervalMatrix(Eigen::MatrixXd::Identity(G.rows(),G.cols()));
IntervalMatrix JG_inv_(inverse_enclosure(JG)); // non rigourous inversion
IntervalMatrix I_ = IntervalMatrix::eye(G.rows(),G.cols());
IntervalMatrix JG_inv_ = inverse_enclosure(JG); // rigourous inversion
Matrix M(JG);
auto W = JG_inv_;
auto Z = I_;
Expand All @@ -199,28 +196,26 @@ namespace codac2 {
assert(q.size() == G.rows());

// SVD decomposition of JG = U*E*V.T
Eigen::BDCSVD<Eigen::MatrixXd> bdcsvd(JG,Eigen::ComputeFullU);
IntervalMatrix U_(bdcsvd.matrixU()); // which is also the right part
Vector Sv(bdcsvd.singularValues()); // vectors of singular values
Eigen::BDCSVD<Matrix,Eigen::ComputeFullU> bdcsvd(JG);
auto U_ = bdcsvd.matrixU().template cast<Interval>(); // which is also the right part
auto Sv = bdcsvd.singularValues(); // vectors of singular values

// select new singular values
int dim = G.rows();
IntervalVector s_box(U_.transpose()*J_box*G_*unit_box);
IntervalMatrix S_(Eigen::MatrixXd::Zero(dim,dim)); // diagonal matrix of the new singular value
IntervalMatrix S_pinv_(Eigen::MatrixXd::Zero(dim,dim)); // pseudo inverse of S
for(int i=0;i<dim;i++){
Index dim = G.rows();
auto s_box = U_.transpose()*J_box*G_*unit_box;
IntervalMatrix S_ = IntervalMatrix::zero(dim,dim); // diagonal matrix of the new singular value
IntervalMatrix S_pinv_ = IntervalMatrix::zero(dim,dim); // pseudo inverse of S
for(Index i=0;i<dim;i++){
if (Sv[i]>trig[1]){ // normal size singular values
S_(i,i) = Interval(Sv[i]);
S_pinv_(i,i) = 1/S_(i,i);
}else{ // for very small singular values (0 included) use s_box
double val = s_box[i].ub();
S_(i,i) = Interval(q[i]*val);
S_pinv_(i,i)=1/S_(i,i);
S_(i,i) = Interval(q[i])*s_box[i].ub();
}
S_pinv_(i,i) = 1./S_(i,i);
}
M = (U_*S_).mid();
W = S_pinv_*U_.transpose();
Z = W*JG_;
Z = W*JG.template cast<Interval>();
}

auto b_box = (W * J_box * G_ - Z) * unit_box;
Expand Down Expand Up @@ -253,7 +248,7 @@ namespace codac2 {

ostream& operator<<(ostream& os, const Ellipsoid& e)
{
os << "Ellipsoid:\n"
os << "Ellipsoid " << e.size() << "d:\n"
<< " mu=" << e.mu << "\n"
<< " G=\n" << e.G;
return os;
Expand Down
Loading

0 comments on commit a81e68d

Please sign in to comment.