LinearConstraintHandlerEliminationT.cc 3.58 KB
Newer Older
1 2 3 4 5 6
//=============================================================================
//
//  CLASS LinearConstraintHandlerElimination - IMPLEMENTATION
//
//=============================================================================

David Bommes's avatar
David Bommes committed
7
#define COMISO_LINEARCONSTRAINTHANDLERELIMINATION_C
8 9 10 11

//== INCLUDES =================================================================

#include "LinearConstraintHandlerElimination.hh"
David Bommes's avatar
changed  
David Bommes committed
12

13 14 15 16
#include <CoMISo/Solver/SparseQRSolver.hh>

//== NAMESPACES ===============================================================

David Bommes's avatar
David Bommes committed
17
namespace COMISO {
18 19 20 21 22 23 24 25 26 27 28 29 30 31 32

//== IMPLEMENTATION ==========================================================

template<class MatrixT, class VectorT>
void
LinearConstraintHandlerElimination::
initialize( const MatrixT& _C, const VectorT& _c)
{
  // no constraints?
  if( gmm::mat_nrows(_C) == 0)
  {
    initialize_identity( gmm::mat_ncols(_C));
  }
  else
  {
David Bommes's avatar
changed  
David Bommes committed
33 34 35

	  // require SPQR SOLVER!!!
#if(COMISO_SUITESPARSE_SPQR_AVAILABLE)
36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65
    // Construct constraints basis form via QR-factorization (see Nocedal 426...)
    // Constraints in basis transformation form x_orig = b_ + C_*x_reduced
    // notice that C_ is a basis of the nullspace of the constraints
    // and _C*b_ = _c (means b_ is one solution of the constraints)

    std::cerr << "Initialize Linear Constraint handler...\n";
    COMISO::SparseQRSolver sqr;
    //  sqr.calc_system_gmm(_C);
    CMatrix Q;
    CMatrix R;
    std::vector<int> P;
    int rank = sqr.factorize_system_gmm(gmm::transposed(_C), Q, R, P);

    // Q[0..m-1,rank..m-1] is basis of the nullspace -> C_, Ct_
    int m = gmm::mat_nrows(Q);
    gmm::resize(C_, m, m-rank);
    gmm::clear (C_);
    gmm::copy( gmm::sub_matrix(Q, gmm::sub_interval(0,m), gmm::sub_interval(rank,m-rank)), C_);
    gmm::resize(Ct_, gmm::mat_ncols(C_), gmm::mat_nrows(C_));
    gmm::copy( gmm::transposed(C_), Ct_);

    // compute b_
    b_.resize(gmm::mat_ncols(_C));
    // hack (too expensive, directly exploit Q,R,P from above)
    sqr.calc_system_gmm(_C);
    sqr.solve(b_, _c);

    n_     = gmm::mat_nrows(C_);
    n_red_ = gmm::mat_ncols(C_);
    m_     = n_ - n_red_;
David Bommes's avatar
David Bommes committed
66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81

    // hack -> store initial system
    if(0)
    {
      gmm::resize(A_orig_, gmm::mat_nrows(_C), gmm::mat_ncols(_C));
      gmm::resize(b_orig_, _c.size());
      gmm::copy(_C, A_orig_);
      gmm::copy(_c, b_orig_);
    }

//    RMatrix CtC(n_red_, n_red_);
//    gmm::mult(Ct_,C_, CtC);
//    std::cerr << "CtC\n";
//    std::cerr << CtC << std::endl;


82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99
    /*
  // set up least squares problem
  gmm::resize(Mtemp_, gmm::mat_ncols(C_), gmm::mat_ncols(C_));
  gmm::mult(Ct_, C_, Mtemp_);
  chol_CtC_.calc_system_gmm(Mtemp_);
     */

    //  std::cerr << "Q: " << Q  << std::endl;
    //  std::cerr << "R: " << R  << std::endl;
    //  std::cerr << "P: " << P  << std::endl;
    //  std::cerr << "C_:" << C_ << std::endl;
    //  std::cerr << "b_:" << b_ << std::endl;
    //
    //  std::cerr << "#rows: " << gmm::mat_nrows(_C) << std::endl;
    //  std::cerr << "#nullspace: " << m << std::endl;
    //  std::cerr << "rank: " << rank << std::endl;
    //  std::cerr << "dim Q = " << gmm::mat_nrows(Q) << " x " << gmm::mat_ncols(Q) << std::endl;
    //  std::cerr << "dim R = " << gmm::mat_nrows(R) << " x " << gmm::mat_ncols(R) << std::endl;
David Bommes's avatar
changed  
David Bommes committed
100 101 102
#else
    std::cerr << "ERROR: SQPR-Solver required by LinearConstraintHandlerElimination not available !!!" << std::endl;
#endif
103 104 105 106 107 108 109 110
  }
}

//-----------------------------------------------------------------------------



//=============================================================================
David Bommes's avatar
David Bommes committed
111
} // namespace COMISO
112
//=============================================================================