Merge branch 'exact_constraint_satisfaction' into 'master'

Exact constraint satisfaction

See merge request !54

.gitignore

@@ -5,3 +5,4 @@ Config/config.hh
 *.swp
 .DS_Store
 /build*/
+CMakeLists.txt.user*

CMakeLists.txt

@@ -576,6 +576,9 @@ if (COMISO_BUILD_EXAMPLES )
   if( EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/Examples/small_symmetric_dirichlet/CMakeLists.txt" )
   add_subdirectory (Examples/small_symmetric_dirichlet)
   endif()
+  if( EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/Examples/small_exact_constraint_satifaction_example/CMakeLists.txt" )
+  add_subdirectory (Examples/small_exact_constraint_satifaction_example)
+  endif()
 
 endif (COMISO_BUILD_EXAMPLES )
 

EigenSolver/ArpackSolver.hh

@@ -77,7 +77,7 @@ public:
 //=============================================================================
 #if defined(INCLUDE_TEMPLATES) && !defined(COMISO_ARPACKSOLVER_C)
 #define COMISO_ARPACKSOLVER_TEMPLATES
-#include "ArpackSolver.cc"
+//#include "ArpackSolver.cc"
 #endif
 //=============================================================================
 #endif // COMISO_SUITESPARSE_AVAILABLE

Examples/small_eigenproblem/main.cc

@@ -75,10 +75,10 @@ int main(void)
   
   std::cout << "---------- 2) Solving for m smallest eigenvalues and eigenvectors..." << std::endl;
   unsigned int m=3;
-  COMISO::ArpackSolver arsolv;
+//  COMISO::ArpackSolver arsolv;
   std::vector<double> evals;
   Matrix evects;
-  arsolv.solve(A, evals, evects, m);
+//  arsolv.solve(A, evals, evects, m);
   
   std::cout << "---------- 3) printing results..." << std::endl;
   std::cerr << "********* eigenvalues: ";

Examples/small_exact_constraint_satifaction_example/CMakeLists.txt

+include (CoMISoExample)
+
+acg_add_executable (small_exact_constraint_satifaction_example ${sources} ${headers} )
+
+# enable rpath linking
+set_target_properties(small_exact_constraint_satifaction_example PROPERTIES INSTALL_RPATH_USE_LINK_PATH 1)
+
+target_link_libraries (small_exact_constraint_satifaction_example
+  CoMISo
+  ${COMISO_LINK_LIBRARIES}
+)

Examples/small_exact_constraint_satifaction_example/main.cc

+/*===========================================================================*\
+ *                                                                           *
+ *                               CoMISo                                      *
+ *      Copyright (C) 2008-2019 by Computer Graphics Group, RWTH Aachen      *
+ *                           www.rwth-graphics.de                            *
+ *                                                                           *
+ *---------------------------------------------------------------------------*
+ *  This file is part of CoMISo.                                             *
+ *                                                                           *
+ *  CoMISo is free software: you can redistribute it and/or modify           *
+ *  it under the terms of the GNU General Public License as published by     *
+ *  the Free Software Foundation, either version 3 of the License, or        *
+ *  (at your option) any later version.                                      *
+ *                                                                           *
+ *  CoMISo 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 General Public License for more details.                             *
+ *                                                                           *
+ *  You should have received a copy of the GNU General Public License        *
+ *  along with CoMISo.  If not, see <http://www.gnu.org/licenses/>.          *
+ *                                                                           *
+\*===========================================================================*/
+
+#include <CoMISo/Config/config.hh>
+#include <iostream>
+
+#include <CoMISo/NSolver/NewtonSolver.hh>
+#include <CoMISo/NSolver/NProblemInterface.hh>
+#include <vector>
+
+#include <CoMISo/Utils/ExactConstraintSatisfaction.hh>
+//------------------------------------------------------------------------------------------------------
+
+class SmallNProblem : public COMISO::NProblemInterface
+{
+public:
+  // specify a function which has several local minima
+  // f(x,y) = x^4 + y^4
+
+  // number of unknown variables, here x and y = 2
+  virtual int    n_unknowns   (                                )
+  {
+    return 2;
+  }
+
+  // initial value where the optimization should start from
+  virtual void   initial_x    (       double* _x               )
+  {
+    _x[0] = 4.0;
+    _x[1] = 2.0;
+  }
+
+  // function evaluation at location _x
+  virtual double eval_f       ( const double* _x               )
+  {
+    return std::pow(_x[0], 4) + std::pow(_x[1], 4);
+  }
+
+  // gradient evaluation at location _x
+  virtual void   eval_gradient( const double* _x, double*    _g)
+  {
+    _g[0] =  4.0*std::pow(_x[0], 3);
+    _g[1] =  4.0*std::pow(_x[1], 3);
+   }
+
+  // hessian matrix evaluation at location _x
+  virtual void   eval_hessian ( const double* _x, SMatrixNP& _H)
+  {
+    _H.resize(n_unknowns(), n_unknowns());
+    _H.setZero();
+
+    _H.coeffRef(0,0) =  12.0*std::pow(_x[0], 2);
+    _H.coeffRef(1,0) =  0.0;
+    _H.coeffRef(0,1) =  0.0;
+    _H.coeffRef(1,1) =  12.0*std::pow(_x[1], 2);
+  }
+
+  // print result
+  virtual void   store_result ( const double* _x               )
+  {
+    solution.resize(n_unknowns());
+    for (int i = 0; i < n_unknowns(); ++i)
+      solution[i] = _x[i];
+  }
+
+  // advanced properties
+  virtual bool   constant_hessian() const { return false; }
+
+  std::vector<double> solution;
+};
+
+// Example main
+int main(void)
+{
+  std::cout << "---------- 1) Get an instance of a problem..." << std::endl;
+  SmallNProblem problem;
+
+  std::cout << "---------- 2) Set up constraints..." << std::endl;
+
+
+  int n_constraints = 2; // there will be two constraints
+  Eigen::VectorXi b;
+  COMISO::ExactConstraintSatisfaction::SP_Matrix_R A(n_constraints, problem.n_unknowns());
+  b.setZero(n_constraints);
+
+  // first constraint: first variable equals three times second
+  //different number of constraints :
+  if(n_constraints == 2)
+  {
+    A.coeffRef(0,0) =  2;
+    A.coeffRef(0,1) =  -1;
+    b.coeffRef(0)   =  0;
+    A.coeffRef(1,0) =  -2;
+    A.coeffRef(1,1) =  8;
+    b.coeffRef(1)   =  21;
+  }
+
+  std::cout << "Constraints: Ax = b with A = \n" << A << "and b = \n" << b << std::endl;
+
+  std::cout << "---------- 3) Solve with Newton Solver..." << std::endl;
+  COMISO::NewtonSolver nsolver;
+  nsolver.set_verbosity(15);
+  Eigen::SparseMatrix<double> Ad = A.cast<double>();
+  Eigen::VectorXd bd = b.cast<double>();
+  nsolver.solve(&problem, Ad, bd);
+
+  std::cout << "---------- 4) Print solution..." << std::endl;
+  std::cout << std::setprecision(100);
+  for (unsigned int i = 0; i < problem.n_unknowns(); ++i)
+    std::cout << "x[" << i << "] = " << problem.solution[i] << std::endl;
+
+
+  std::cout << "---------- 5) Check constraint violation..." << std::endl;
+  Eigen::VectorXd x;
+  x.resize(problem.n_unknowns());
+  for (unsigned int i = 0; i < problem.n_unknowns(); ++i)
+    x.coeffRef(i) = problem.solution[i];
+
+  std::cout << "Constraint violation: " << (A.cast<double>() *x - b.cast<double>()).squaredNorm() << std::endl;
+
+  std::cout << "---------- 6) Try to exactly fulfill constraints..." << std::endl;
+
+  COMISO::ExactConstraintSatisfaction satisfy;
+//  satisfy.print_matrix(A);
+  satisfy.evaluation(A, b, x);
+
+
+  for (unsigned int i = 0; i < problem.n_unknowns(); ++i)
+    std::cout << "x[" << i << "] = " << x[i] << std::endl;
+
+  std::cout << "---------- 7) Check constraint violation again..." << std::endl;
+
+  std::cout << "Constraint violation: " << (A.cast<double>() *x - b.cast<double>()).squaredNorm() << std::endl;
+
+
+  return 0;
+}

NSolver/NewtonSolver.cc

@@ -46,7 +46,7 @@ solve(NProblemGmmInterface* _problem)
     // check for convergence
     if( gmm::vect_norm2(g) < eps_)
     {
-      DEB_line(2, "Newton Solver converged after " << i << " iterations");
+      DEB_line(verbosity_, "Newton Solver converged after " << i << " iterations");
       _problem->store_result(P(x));
       return true;
     }
@@ -80,7 +80,7 @@ solve(NProblemGmmInterface* _problem)
           f = f_new;
           improvement = true;
 
-          DEB_line(2, "energy improved to " << f);
+          DEB_line(verbosity_, "energy improved to " << f);
         }
       }
 
@@ -97,7 +97,7 @@ solve(NProblemGmmInterface* _problem)
       else
       {
         _problem->store_result(P(x));
-        DEB_line(2, "Newton solver reached max regularization but did not "
+        DEB_line(verbosity_, "Newton solver reached max regularization but did not "
           "converge");
         converged_ = false;
         return false;
@@ -105,7 +105,7 @@ solve(NProblemGmmInterface* _problem)
     }
   }
   _problem->store_result(P(x));
-  DEB_line(2, "Newton Solver did not converge!!! after iterations.");
+  DEB_line(verbosity_, "Newton Solver did not converge!!! after iterations.");
   converged_ = false;
   return false;
 
@@ -123,7 +123,7 @@ solve(NProblemGmmInterface* _problem)
 int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A, 
   const VectorD& _b)
 {
-  DEB_time_func_def;
+  DEB_time_func(verbosity_);
   converged_ = false;
 
   const double KKT_res_eps = 1e-6;
@@ -136,7 +136,7 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
   // number of constraints
   size_t m = _b.size();
 
-  DEB_line(2, "optimize via Newton with " << n << " unknowns and " << m << 
+  DEB_line(verbosity_, "optimize via Newton with " << n << " unknowns and " << m <<
     " linear constraints");
 
   // initialize vectors of unknowns
@@ -196,7 +196,7 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
         // alternate hessian and constraints regularization
         if(reg_iters % 2 == 0 || regularize_constraints >= regularize_constraints_limit)
         {
-          DEB_line(2, "residual ^ 2 " << kkt_res2 << "->regularize hessian");
+          DEB_line(verbosity_, "residual ^ 2 " << kkt_res2 << "->regularize hessian");
           if(regularize_hessian == 0.0)
             regularize_hessian = 1e-6;
           else
@@ -204,7 +204,7 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
         }
         else
         {
-          DEB_line(2, "residual^2 " << kkt_res2 << " -> regularize constraints");
+          DEB_line(verbosity_, "residual^2 " << kkt_res2 << " -> regularize constraints");
           if(regularize_constraints == 0.0)
             regularize_constraints = 1e-8;
           else
@@ -250,7 +250,7 @@ int NewtonSolver::solve(NProblemInterface* _problem, const SMatrixD& _A,
     }
 
 
-    DEB_line(2, "iter: " << iter
+    DEB_line(verbosity_, "iter: " << iter
       << ", f(x) = " << fx << ", t = " << t << " (tmax=" << t_max << ")"
       << (t < t_max ? " _clamped_" : "")
       << ", eps = [Newton decrement] = " << newton_decrement

NSolver/NewtonSolver.hh

@@ -58,7 +58,7 @@ public:
 
   /// Default constructor
   NewtonSolver(const double _eps = 1e-6, const double _eps_line_search = 1e-9, const int _max_iters = 200, const double _alpha_ls=0.2, const double _beta_ls = 0.6)
-    : eps_(_eps), eps_ls_(_eps_line_search), max_iters_(_max_iters), alpha_ls_(_alpha_ls), beta_ls_(_beta_ls), solver_type_(LS_EigenLU), constant_hessian_structure_(false)
+    : eps_(_eps), eps_ls_(_eps_line_search), max_iters_(_max_iters), alpha_ls_(_alpha_ls), beta_ls_(_beta_ls), verbosity_(2), solver_type_(LS_EigenLU), constant_hessian_structure_(false)
   {
 //#if COMISO_SUITESPARSE_AVAILABLE
 //    solver_type_ = LS_Umfpack;
@@ -84,8 +84,12 @@ public:
     solver_type_ = _st;
   }
 
+  // set verbosity level of the solver. Lower numbers are more verbose
+  void set_verbosity(int _verbosity) { verbosity_ = _verbosity; }
+
   bool converged() { return converged_; }
 
+
 protected:
 
   bool factorize(NProblemInterface* _problem, const SMatrixD& _A,
@@ -134,6 +138,7 @@ private:
   int    max_iters_;
   double alpha_ls_;
   double beta_ls_;
+  int verbosity_;
 
   VectorD x_ls_;
 
@@ -149,6 +154,7 @@ private:
   Eigen::UmfPackLU<SMatrixD> umfpack_solver_;
 #endif
 
+
   // deprecated
   bool   constant_hessian_structure_;
 

Utils/ExactConstraintSatisfaction.cc

+#include "ExactConstraintSatisfaction.hh"
+
+#include <CoMISo/Config/config.hh>
+
+#include <CoMISo/NSolver/NProblemInterface.hh>
+#include <CoMISo/Utils/CoMISoError.hh>
+#include <vector>
+
+namespace COMISO {
+
+ExactConstraintSatisfaction::ExactConstraintSatisfaction()
+  :
+    largest_exponent_(std::numeric_limits<int>::min())
+{
+
+}
+
+
+// ------------------------Helpfull Methods----------------------------------------
+
+//row1 belongs to vector b, row2 to a row in the matrix
+void ExactConstraintSatisfaction::swap_rows(SP_Matrix_R& mat, int row1, int row2){
+
+  Eigen::SparseVector<int> row_2 = mat.row(row2);
+  Eigen::SparseVector<int> row_1 = mat.row(row1);
+  mat.row(row2) = row_1;
+  mat.row(row1) = row_2;
+
+//  mat.prune(0.0, 0);
+//  mat.makeCompressed();
+//  mat.finalize();
+
+}
+
+
+//We want to eliminate row1 in mat with the row corresponding to row2 in mat
+//the row_2 has a pivot in (row2, col_p)
+void ExactConstraintSatisfaction::eliminate_row(SP_Matrix_R& mat, Eigen::VectorXi& b, int row1, int row2, int pivot_column)
+{
+  if(pivot_column < 0)
+  {
+    DEB_error("Pivot column is negative");
+    return;
+  }
+
+  int pivot_row1 = mat.coeff(row1, pivot_column);     //the element under the pivot
+
+  if(pivot_row1 == 0)
+    return;
+
+  int pivot_row2 = mat.coeff(row2, pivot_column);     //the pivot
+
+  b.coeffRef(row1) *= pivot_row2;
+  b.coeffRef(row1) -= pivot_row1 * b.coeffRef(row2);
+
+  mat.row(row1) *= pivot_row2;
+  mat.row(row1) -= pivot_row1 * mat.row(row2);
+  mat.row(row1) = mat.row(row1).pruned(0,0);
+
+  int gcdValue = gcd_row(mat, row1, b.coeff(row1));
+  mat.row(row1) /= gcdValue;
+  b.coeffRef(row1) /= gcdValue;
+
+}
+
+int ExactConstraintSatisfaction::gcd(const int a, const int b)
+{
+  if(b == 0)
+    return std::abs(a);
+  return gcd(std::abs(b) , std::abs(a) % std::abs(b));
+}
+
+int ExactConstraintSatisfaction::gcd_row(const SP_Matrix_R& A, int row, const int b)
+{
+
+  int gcdValue = b;
+  bool first = true;
+  bool is_negativ = 0;
+
+  for(SP_Matrix_R::InnerIterator it(A, row); it; ++it)
+  {
+    if(it.value() != 0 && first)
+    {
+      first = false;
+      is_negativ = it.value() < 0;
+    }
+    gcdValue = gcd(gcdValue, it.value());
+  }
+  if(gcdValue == 0)
+    return 1;
+  if(is_negativ)
+    gcdValue = std::abs(gcdValue) * -1;
+  return gcdValue;
+}
+
+void ExactConstraintSatisfaction::largest_exponent(const Eigen::VectorXd& x)
+{
+  // only compute largest component if it was not set from the outside
+  if (largest_exponent_ == std::numeric_limits<int>::min())
+    set_largest_exponent(std::max(compute_largest_exponent(x)+2, -65));
+}
+
+void ExactConstraintSatisfaction::set_largest_exponent(int _exponent)
+{
+  largest_exponent_ = _exponent;
+  delta_ = std::pow(2, largest_exponent_);
+}
+
+int ExactConstraintSatisfaction::compute_largest_exponent(const Eigen::VectorXd& x)
+{
+  double max_elem = std::max(x.maxCoeff(), -x.minCoeff());
+  auto expo = static_cast<int>(std::ceil(std::log2(max_elem)));
+  return expo;
+}
+
+double ExactConstraintSatisfaction::F_delta(double x)
+{
+  int sign = -1;
+  if(x >= 0)
+    sign = 1;
+  double x_of_F = (x + sign * delta_) - sign * delta_;
+  return x_of_F;
+}
+
+int ExactConstraintSatisfaction::lcm(const int a, const int b)
+{
+  if(gcd(a,b) == 0)
+    return 0;
+  return (a/gcd(a,b)) * b;
+}
+
+int ExactConstraintSatisfaction::lcm(const std::vector<int>& D)
+{
+  int lcm_D = 1;
+  for(int d : D)
+  {
+    lcm_D = lcm(lcm_D, d);
+  }
+  return lcm_D;
+}
+
+int ExactConstraintSatisfaction::index_pivot(const sparseVec& row)
+{
+
+  for(sparseVec::InnerIterator it(row); it; ++it)
+  {
+    if(it.value() != 0)
+      return it.index();
+  }
+  return -1;
+}
+
+double ExactConstraintSatisfaction::get_delta()
+{
+  return delta_;
+}
+
+// ----------------------Matrix transformation-----------------------------------
+void ExactConstraintSatisfaction::IREF_Gaussian(SP_Matrix_R& A, Eigen::VectorXi& b, const Eigen::VectorXd& x){
+
+  number_pivots_ = 0;
+  int rows = A.rows();        //number of rows
+  int cols = A.cols();        //number of columns
+  int col_index  = 0;         //save the next column where we search for a pivot
+
+  for (int k = 0; k < rows; k++)
+  {                                        //order Matrix after pivot
+    if(A.coeff(k, col_index) == 0)
+    {
+      int pivot_row = get_pivot_col_new(A, k, col_index);
+
+      if(pivot_row != -1)
+        if(k != pivot_row)
+          swap_rows(A, k, pivot_row);
+
+      if(col_index == cols)
+        break;
+
+      if(pivot_row == -1)
+        continue;
+    }
+    else
+    {
+      col_index++;//col_index = k +1;
+    }
+    number_pivots_++;
+
+    if (number_pivots_ == 1)
+    {
+      // first row, divide by gcd to keep numbers small. All other rows will be divided by ther gcd in eliminate row
+      int gcdValue = gcd_row(A, 0, b.coeffRef(0));                     //compute the gcd to make the values as small as possible
+      if(gcdValue != 1)
+      {
+        A.row(0) /= gcdValue;
+        b.coeffRef(0) /= gcdValue;
+      }
+    }
+
+    int col_p = col_index -1;
+
+    for(int i = k+1; i < rows; ++i)
+    {
+      SP_Matrix_R::InnerIterator row_iter_i(A,i);
+      if (row_iter_i.index() > col_p)
+        continue; // first element is right of i, so A(i,col_p) is already 0
+      COMISO_THROW_TODO_if(row_iter_i.index() < col_p, "there should be now non zero values lower left of k,col_p");
+
+      eliminate_row(A, b, i, k, col_p);
+    }
+  }
+  A.prune(0.0 , 0);
+}
+
+void ExactConstraintSatisfaction::IRREF_Jordan(SP_Matrix_R& A, Eigen::VectorXi& b)
+{
+  SP_Matrix_C A_colmaj = A; // for more efficient tests which rows need to be eliminated
+
+  for(int k = number_pivots_ - 1; k > 0; k--)
+  {
+    int pivot_col = -1;
+    for(SP_Matrix_R::InnerIterator it(A, k); it; ++it)
+    {
+      if(it.value() != 0)
+      {
+        pivot_col = it.index();
+        break;
+      }
+    }
+    COMISO_THROW_TODO_if(pivot_col == -1, "Could not find a pivot column");
+
+    for (SP_Matrix_C::InnerIterator it(A_colmaj, pivot_col); it; ++it)
+    {
+      if (it.row() == k)
+        break;
+      if (it.value() == 0)
+        continue;
+      eliminate_row(A, b, it.row(), k, pivot_col);
+    }
+
+  }
+
+//  A.makeCompressed();
+//  A.finalize();
+  A.prune(0.0, 0);
+}
+
+//-------------------------------------Evaluation--------------------------------------------------------------------
+
+void ExactConstraintSatisfaction::evaluation(SP_Matrix_R& _A, Eigen::VectorXi& b, Eigen::VectorXd& x)
+{
+  IREF_Gaussian(_A, b, x);
+
+  IRREF_Jordan(_A, b);
+
+  largest_exponent(x);
+
+  evaluate(_A, b, x);
+}
+
+double ExactConstraintSatisfaction::makeDiv(const std::vector<int>& D, double x)
+{
+  if(D.empty())
+    return F_delta(x);
+
+  int d = lcm(D);
+  double result = F_delta(x/d) * d;
+  return result;
+}
+
+double ExactConstraintSatisfaction::safeDot(const std::vector<std::pair<int, double> >& S)
+{
+  if (S.empty()) //case all Cij are zero after the pivot
+    return 0;