added resolve-support for modified constraint-rhs

added example to test new functions

git-svn-id: http://www.openflipper.org/svnrepo/CoMISo/trunk@170 1355f012-dd97-4b2f-ae87-10fa9f823a57

CoMISo/CMakeLists.txt

@@ -342,3 +342,6 @@ endif()
 if( EXISTS "${CMAKE_SOURCE_DIR}/Examples/small_sparseqr/CMakeLists.txt" )
 	add_subdirectory (Examples/small_sparseqr)
 endif()
+if( EXISTS "${CMAKE_SOURCE_DIR}/Examples/small_quadratic_resolve_example/CMakeLists.txt" )
+	add_subdirectory (Examples/small_quadratic_resolve_example)
+endif()

CoMISo/Examples/small_nleast_squares/CMakeLists.txt

+include (ACGCommon)
+include (CoMISoExample)
+
+# source code directories
+set (directories 
+  . 
+)
+
+# collect all header and source files
+acg_append_files (headers "*.hh" ${directories})
+acg_append_files (sources "*.cc" ${directories})
+
+# remove template cc files from source file list
+acg_drop_templates (sources)
+
+if (WIN32)
+  acg_add_executable (small_nleast_squares WIN32 ${sources} ${headers} )
+elseif (APPLE)
+  # generate bundle on mac
+  acg_add_executable (small_nleast_squares MACOSX_BUNDLE ${sources} ${headers} )
+else ()
+  acg_add_executable (small_nleast_squares ${sources} ${headers} )
+endif ()
+
+# enable rpath linking
+set_target_properties(small_nleast_squares PROPERTIES INSTALL_RPATH_USE_LINK_PATH 1)
+
+target_link_libraries (small_nleast_squares
+  CoMISo
+  ${COMISO_LINK_LIBRARIES}
+)
+
+if (APPLE)
+   # create bundle in "Build" directory and set icon
+   # no install needed here, because the whole bundle will be installed in the
+   # toplevel CMakeLists.txt
+   set_target_properties (
+      small_nsolver PROPERTIES
+      RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/Build"
+      MACOSX_BUNDLE_INFO_STRING "CoMISo small_nleast_squares"
+   )
+endif ()

CoMISo/Examples/small_nleast_squares/main.cc

+/*===========================================================================*\
+ *                                                                           *
+ *                               CoMISo                                      *
+ *      Copyright (C) 2008-2009 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 <CoMISo/Utils/StopWatch.hh>
+#include <vector>
+#include <CoMISo/NSolver/LeastSquaresProblem.hh>
+#include <CoMISo/NSolver/LinearConstraint.hh>
+#include <CoMISo/NSolver/NPDerivativeChecker.hh>
+#include <CoMISo/NSolver/IPOPTSolver.hh>
+
+
+// solve least squares problem for x=1, y=2 and x-2y = 1
+
+// Example main
+int main(void)
+{
+  std::cout << "---------- 1) Get an instance of a LeastSquaresProblem..." << std::endl;
+  // number of unknowns
+  const int n = 2;
+  COMISO::LeastSquaresProblem lsqp(n);
+
+  // term0
+  COMISO::LinearConstraint::SVectorNC coeffs0(n);
+  coeffs0.coeffRef(0) = 1.0;
+  COMISO::LinearConstraint term0(coeffs0,-1.0,COMISO::NConstraintInterface::NC_EQUAL);
+  lsqp.add_term(&term0);
+
+    // term1
+  COMISO::LinearConstraint::SVectorNC coeffs1(n);
+  coeffs1.coeffRef(1) = 1.0;
+  COMISO::LinearConstraint term1(coeffs1,-2.0,COMISO::NConstraintInterface::NC_EQUAL);
+  lsqp.add_term(&term1);
+  
+    // term2
+  COMISO::LinearConstraint::SVectorNC coeffs2(n);
+  coeffs2.coeffRef(0) =  1.0;
+  coeffs2.coeffRef(1) = -2.0;
+  COMISO::LinearConstraint term2(coeffs2,-1.0,COMISO::NConstraintInterface::NC_EQUAL);
+  lsqp.add_term(&term2);
+  
+  std::cout << "---------- 2) (optional for debugging) Check derivatives of problem..." << std::endl;
+  COMISO::NPDerivativeChecker npd;
+  npd.check_all(&lsqp);
+
+// check if IPOPT solver available in current configuration
+#if( COMISO_IPOPT_AVAILABLE)
+  std::cout << "---------- 3) Get IPOPT solver... " << std::endl;
+  COMISO::IPOPTSolver ipsol;
+
+  std::cout << "---------- 4) Solve..." << std::endl;
+  // there are no constraints -> provide an empty vector
+  std::vector<COMISO::NConstraintInterface*> constraints;
+  ipsol.solve(&lsqp, constraints);
+#endif
+
+  std::cout << "---------- 5) Print solution..." << std::endl;
+  for( int i=0; i<n; ++i)
+    std::cerr << "x_" << i << " = " << lsqp.x()[i] << std::endl;
+  
+  return 0;
+}
+

CoMISo/Examples/small_quadratic_resolve_example/CMakeLists.txt

+include (ACGCommon)
+include (CoMISoExample)
+
+# source code directories
+set (directories 
+  . 
+)
+
+# collect all header and source files
+acg_append_files (headers "*.hh" ${directories})
+acg_append_files (sources "*.cc" ${directories})
+
+# remove template cc files from source file list
+acg_drop_templates (sources)
+
+if (WIN32)
+  acg_add_executable (small_quadratic_resolve WIN32 ${sources} ${headers} )
+elseif (APPLE)
+  # generate bundle on mac
+  acg_add_executable (small_quadratic_resolve MACOSX_BUNDLE ${sources} ${headers} )
+else ()
+  acg_add_executable (small_quadratic_resolve ${sources} ${headers} )
+endif ()
+
+# enable rpath linking
+set_target_properties(small_quadratic_resolve PROPERTIES INSTALL_RPATH_USE_LINK_PATH 1)
+
+target_link_libraries (small_quadratic_resolve
+  CoMISo
+  ${COMISO_LINK_LIBRARIES}
+)
+
+if (APPLE)
+   # create bundle in "Build" directory and set icon
+   # no install needed here, because the whole bundle will be installed in the
+   # toplevel CMakeLists.txt
+   set_target_properties (
+      small_quadratic_solver PROPERTIES
+      RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/Build"
+      MACOSX_BUNDLE_INFO_STRING "CoMISo small_quadratic_resolve"
+   )
+endif ()

CoMISo/Examples/small_quadratic_resolve_example/main.cc

+/*===========================================================================*\
+ *                                                                           *
+ *                               CoMISo                                      *
+ *      Copyright (C) 2008-2009 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/Utils/StopWatch.hh>
+#include <gmm/gmm.h>
+#include <vector>
+#include <CoMISo/Solver/ConstrainedSolver.hh>
+#include <CoMISo/Solver/MISolver.hh>
+#include <CoMISo/Solver/GMM_Tools.hh>
+
+
+/// function to initialize a simple system of linear equations
+template<class MatrixT>
+void init_les( MatrixT& _A, std::vector< double >& _b)
+{
+  _A(0,0) = 25  ; _A(0,1) = 0 ; _A(0,2) = -15; _A(0,3) = 0 ; 
+  _A(1,0) = 0   ; _A(1,1) = 20; _A(1,2) = -8 ; _A(1,3) = -4; 
+  _A(2,0) = -15 ; _A(2,1) = -8; _A(2,2) = 17 ; _A(2,3) = 0 ; 
+  _A(3,0) = 0   ; _A(3,1) = -4; _A(3,2) = 0  ; _A(3,3) = 4 ; 
+
+  _b[0] = 0; _b[1] = 4; _b[2] = -2; _b[3] = 0;
+}
+
+/// function to print the equations corresponding to the matrices of an equation system
+template<class MatrixT>
+void print_equations( const MatrixT& _B)
+{
+  int m = gmm::mat_nrows( _B);
+  int n = gmm::mat_ncols( _B);
+  for( int i = 0; i < m; ++i)
+  {
+    for( int j = 0; j < n-1; ++j)
+    {
+      if( _B(i,j) != 0.0)
+        std::cout << _B(i,j) << "*x" << j;
+      else
+        std::cout << "   0 ";
+      if( j < n-2 ) std::cout << " + ";
+    }
+    std::cout << " = " << -_B(i, n-1) << std::endl;
+  }
+}
+
+
+// Example main
+int main(void)
+{
+  std::cout << "---------- 1) Setup small (symmetric) test equation system Ax=b..." << std::endl;
+  int n = 4;
+  gmm::col_matrix< gmm::wsvector< double > > A(n,n);
+  std::vector< double > x(n);
+  std::vector< double > b(n);
+  
+  std::vector<double> x_bak;
+
+  std::cout << "---------- 1) Set up problem..." << std::endl;
+
+  init_les( A, b);
+
+  // create an empty constraint matrix (will be used later)
+  gmm::row_matrix< gmm::wsvector< double > > constraints(0,n+1); //n+1 because of right hand side
+  // create an empty vector of variable indices to be rounded (will be used later)
+  std::vector< int > ids_to_round;
+
+  std::cout << A << std::endl << b << std::endl;
+
+  // setup constraints
+  gmm::resize( constraints, 3, n+1);
+  constraints( 0, 0 ) = 1.0;
+  constraints( 0, 1 ) = -1.0;
+  constraints( 0, n ) = 2.0;
+  constraints( 1, 3 ) = 1.0;
+  constraints( 1, n ) = -1.0;
+  // add one redundant constraint (this will be filtered out during Gaussian elimination)
+  constraints( 2, 0 ) = 1.0;
+  constraints( 2, 1 ) = -1.0;
+  constraints( 2, n ) = 2.0;
+  std::cout << "           the constraint equations looks like this:" << std::endl;
+  print_equations( constraints);
+
+  std::cout << "---------- 2) Solve full ..." << std::endl;
+  COMISO::ConstrainedSolver cs;
+  cs.solve_const( constraints, A, x, b, ids_to_round, 0.0, false, true);  
+  x_bak = x;
+  
+  // first test: resolve with identical rhs's
+  std::vector<double> constraint_rhs(3); 
+  std::vector<double> b_new = b;
+  constraint_rhs[0] = -2.0;
+  constraint_rhs[1] =  1.0;
+  constraint_rhs[2] = -2.0;
+
+  std::cout << "---------- 2) Solve same rhs pre-factorized ..." << std::endl;
+  cs.resolve(x, &constraint_rhs, &b_new);
+  std::cout << "orig    result:    " << x_bak << std::endl;
+  std::cout << "resolve result:    " << x     << std::endl;
+
+  // second test: resolve with changed rhs
+  constraint_rhs[0] =  4.0;
+  constraint_rhs[1] = -2.0;
+  constraint_rhs[2] =  4.0;
+  b_new[0] =  1.0; 
+  b_new[1] = -2.0; 
+  b_new[2] =  3.0; 
+  b_new[3] = -5.0;
+
+  std::cout << "---------- 3) Solve different rhs pre-factorized ..." << std::endl;
+  cs.resolve(x, &constraint_rhs, &b_new);
+
+  
+  // solve with new factorization
+  constraints( 0, n ) = -4.0;
+  constraints( 1, n ) =  2.0;
+  constraints( 2, n ) = -4.0;
+  std::cout << "---------- 4) Solve different rhs full ..." << std::endl;
+  cs.solve_const( constraints, A, x_bak, b_new, ids_to_round, 0.0, false, true);  
+
+  std::cout << "orig     result (with different rhs's):    " << x_bak << std::endl;
+  std::cout << "resolve  result (with different rhs's):    " << x     << std::endl;
+  
+  return 0;
+}
+

CoMISo/Solver/ConstrainedSolver.hh

@@ -112,28 +112,16 @@ public:
             bool      _show_miso_settings = true,
             bool      _show_timings = true );
 
-  // efficent re-solve with modified _rhs by keeping previous _constraints and _A fixed
+  // efficent re-solve with modified _constraint_rhs and/or _rhs (if not modified use 0 pointer)
+  // by keeping previous _constraints and _A fixed
+  // _constraint_rhs and _rhs are constant, i.e. not changed
   template<class VectorT >
   void resolve(
        VectorT&  _x,
-       VectorT&  _rhs,
+       VectorT*  _constraint_rhs = 0,
+       VectorT*  _rhs            = 0,
        bool      _show_timings = true );
 
-//  // efficent re-solve with modified _constraint_rhs and/or _rhs (if not modified use 0 pointer)
-//  // by keeping previous _constraints and _A fixed
-//  template<class VectorT >
-//  void resolve(
-//       VectorT&  _x,
-//       VectorT*  _constraint_rhs = 0,
-//       VectorT*  _rhs            = 0,
-//       bool      _show_timings = true );
-
-  // const version of above function
-  template<class VectorT >
-  void resolve_const(
-       VectorT&  _x,
-       const VectorT&  _rhs,
-       bool      _show_timings = true );
 
 /// Non-Quadratic matrix constrained solver
 /**  
@@ -170,17 +158,12 @@ public:
       bool      _show_timings = true );
 
   // efficent re-solve with modified _rhs by keeping previous _constraints and _A fixed
+  // ATTENTION: only the rhs resulting from B^TB can be changed!!! otherwise use solve
   template<class RMatrixT, class VectorT >
     void resolve(
       const RMatrixT& _B,
       VectorT&  _x,
-      bool      _show_timings = true );
-
-  // const version of above function
-  template<class RMatrixT, class VectorT >
-  void resolve_const(
-      const RMatrixT& _B,
-      VectorT&  _x,
+      VectorT*  _constraint_rhs = 0,
       bool      _show_timings = true );
 
 /*@}*/
@@ -386,11 +369,15 @@ private:
   class rhsUpdateTable {
   public:
 
-    void append(int _i, double _f, int _j = -1) { table_.push_back(rhsUpdateTableEntry(_i, _j, _f)); }
+    void append(int _i, double _f, int _j, bool _flag)
+    {
+//      std::cerr << "append " << _i << ", " << _j << ", " << _f << ", " << int(_flag) << std::endl;
+      table_.push_back(rhsUpdateTableEntry(_i, _j, _f, _flag));
+    }
     void add_elim_id(int _i) { elim_var_ids_.push_back(_i); }
     void clear() { table_.clear(); elim_var_ids_.clear(); }
     // apply stored transformations to _rhs
-    void apply(std::vector<double>& _rhs)
+    void apply(std::vector<double>& _constraint_rhs, std::vector<double>& _rhs)
     {
       std::vector<rhsUpdateTableEntry>::const_iterator t_it, t_end;
       t_end = table_.end();
@@ -398,13 +385,13 @@ private:
       double cur_rhs = 0.0;
       for(t_it = table_.begin(); t_it != t_end; ++t_it)
       {
-        if(t_it->j >= 0)
+        if(t_it->rhs_flag)
+            _rhs[t_it->i] += t_it->f*_constraint_rhs[t_it->j];
+        else
         {
           if(t_it->j != cur_j) { cur_j = t_it->j; cur_rhs = _rhs[cur_j]; }
           _rhs[t_it->i] += t_it->f * cur_rhs;
         }
-        else
-          _rhs[t_it->i] += t_it->f;
       }
     }
     // remove eliminated elements from _rhs
@@ -438,18 +425,26 @@ private:
   private:
     class rhsUpdateTableEntry {
     public:
-      rhsUpdateTableEntry(int _i, int _j, double _f) : i(_i), j(_j), f(_f) {}
+      rhsUpdateTableEntry(int _i, int _j, double _f, bool _rhs_flag) : i(_i), j(_j), f(_f), rhs_flag(_rhs_flag) {}
       int i;
       int j;
       double f;
+      bool rhs_flag;
     };
 
     std::vector<rhsUpdateTableEntry> table_;
     std::vector<int> elim_var_ids_;
+
   public:
     std::vector<int> c_elim_;
     std::vector<int> new_idx_;
     RowMatrix constraints_p_;
+
+    // cache current rhs_ and constraint_rhs_ and linear transformation of constraint_rhs_ D_
+    RowMatrix D_;
+    std::vector<double> cur_rhs_;
+    // constraint_rhs after Gaussian elimination update D*constraint_rhs_orig_
+    std::vector<double> cur_constraint_rhs_;
   } rhs_update_table_;
 
 };

CoMISo/Solver/ConstrainedSolverT.cc

@@ -271,48 +271,13 @@ solve(
 //-----------------------------------------------------------------------------
 
 
-template<class VectorT >
-void
-ConstrainedSolver::
-resolve_const(
-     VectorT&  _x,
-     const VectorT&  _rhs,
-     bool      _show_timings )
-{
-  VectorT  rhs(_rhs);
-
-  // call non-const function
-  resolve(_x,
-  rhs,
-  _show_timings);
-}
-
-
-//-----------------------------------------------------------------------------
-
-
-template<class RMatrixT, class VectorT >
-void
-ConstrainedSolver::
-resolve_const( const RMatrixT& _B,
-       VectorT&  _x,
-       bool      _show_timings )
-{
-  resolve(_B,
-  _x,
-  _show_timings);
-}
-
-
-//-----------------------------------------------------------------------------
-
-
 template<class RMatrixT, class VectorT >
 void
 ConstrainedSolver::
 resolve(
     const RMatrixT& _B,
     VectorT&  _x,
+    VectorT*  _constraint_rhs,
     bool      _show_timings )
 {
   // extract rhs from quadratic system
@@ -350,7 +315,7 @@ resolve(
   }
 
   // solve
-  resolve(_x, rhs,
+  resolve(_x, _constraint_rhs, &rhs,
    _show_timings);
 }
 
@@ -362,19 +327,43 @@ template<class VectorT >
 void
 ConstrainedSolver::
 resolve(
-    VectorT&  _x,
-    VectorT&  _rhs,
-    bool      _show_timings )
+     VectorT&  _x,
+     VectorT*  _constraint_rhs,
+     VectorT*  _rhs           ,
+     bool      _show_timings   )
 {
   // StopWatch for Timings
   COMISO::StopWatch sw;
 
   sw.start();
   // apply stored updates and eliminations to exchanged rhs
-  rhs_update_table_.apply(_rhs);
-  rhs_update_table_.eliminate(_rhs);
+  if(_constraint_rhs)
+  {
+    // apply linear transformation of Gaussian elimination
+    rhs_update_table_.cur_constraint_rhs_.resize(gmm::mat_nrows(rhs_update_table_.D_));
+    gmm::mult(rhs_update_table_.D_, *_constraint_rhs, rhs_update_table_.cur_constraint_rhs_);
+
+    // update rhs of stored constraints
+    unsigned int nc = gmm::mat_ncols(rhs_update_table_.constraints_p_);
+    for(unsigned int i=0; i<rhs_update_table_.cur_constraint_rhs_.size(); ++i)
+      rhs_update_table_.constraints_p_(i,nc-1) = -rhs_update_table_.cur_constraint_rhs_[i];
+  }
+  if(_rhs)
+    rhs_update_table_.cur_rhs_ = *_rhs;
+
+  std::vector<double> rhs_red = rhs_update_table_.cur_rhs_;
+
+  rhs_update_table_.apply(rhs_update_table_.cur_constraint_rhs_, rhs_red);
+  rhs_update_table_.eliminate(rhs_red);
 
-  miso_.resolve(_x, _rhs);
+  //  std::cerr << "############### Resolve info ##############" << std::endl;
+  //  std::cerr << rhs_update_table_.D_ << std::endl;
+  //  std::cerr << rhs_update_table_.cur_rhs_ << std::endl;
+  //  std::cerr << rhs_update_table_.cur_constraint_rhs_ << std::endl;
+  //  std::cerr << rhs_update_table_.table_.size() << std::endl;
+  //  std::cerr << "rhs_red: " << rhs_red << std::endl;
+
+  miso_.resolve(_x, rhs_red);
 
   double time_miso = sw.stop()/1000.0; sw.start();
 
@@ -401,6 +390,12 @@ make_constraints_independent(
 		VectorIT&         _idx_to_round,
 		std::vector<int>& _c_elim)
 {
+  // setup linear transformation for rhs, start with identity
+  unsigned int nr = gmm::mat_nrows(_constraints);
+  gmm::resize(rhs_update_table_.D_, nr, nr);
+  gmm::clear(rhs_update_table_.D_);
+  for(unsigned int i=0; i<nr; ++i) rhs_update_table_.D_(i,i) = 1.0;
+
   //  COMISO::StopWatch sw;
   // number of variables
   int n_vars = gmm::mat_ncols(_constraints);
@@ -559,10 +554,16 @@ make_constraints_independent(
         if( c_it.index() > i)
         {
 	  //          sw.start();
-          add_row_simultaneously( c_it.index(), -(*c_it)/elim_val_cur, gmm::mat_row(_constraints, i), _constraints, constraints_c);
+          double val = -(*c_it)/elim_val_cur;
+
+          add_row_simultaneously( c_it.index(), val, gmm::mat_row(_constraints, i), _constraints, constraints_c);
           // make sure the eliminated entry is 0 on all other rows and not 1e-17
           _constraints( c_it.index(), elim_j) = 0;
           constraints_c(c_it.index(), elim_j) = 0;
+
+          // update linear transition of rhs
+          gmm::add(gmm::scaled(gmm::mat_row(rhs_update_table_.D_, i), val),
+                   gmm::mat_row(rhs_update_table_.D_, c_it.index()));
         }
     }
   }
@@ -581,6 +582,12 @@ make_constraints_independent_reordering(
 		VectorIT&         _idx_to_round,
 		std::vector<int>& _c_elim)
 {
+  // setup linear transformation for rhs, start with identity
+  unsigned int nr = gmm::mat_nrows(_constraints);
+  gmm::resize(rhs_update_table_.D_, nr, nr);
+  gmm::clear(rhs_update_table_.D_);
+  for(unsigned int i=0; i<nr; ++i) rhs_update_table_.D_(i,i) = 1.0;
+
   //  COMISO::StopWatch sw;
   // number of variables
   int n_vars = gmm::mat_ncols(_constraints);
@@ -602,7 +609,6 @@ make_constraints_independent_reordering(
   gmm::copy(_constraints, constraints_c);
 
   // init priority queue
-  unsigned int nr = gmm::mat_nrows(_constraints);