ConstraintTools.cc 6.65 KB
Newer Older
1
2
3
4
5
6
7
8
9
//== COMPILE-TIME PACKAGE REQUIREMENTS ========================================
#include <CoMISo/Config/config.hh>
#if COMISO_EIGEN3_AVAILABLE

//== INCLUDES =================================================================
#include "ConstraintTools.hh"

#include <CoMISo/Utils/MutablePriorityQueueT.hh>

10
11
#include <Base/Debug/DebOut.hh>

12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59


namespace COMISO {

ConstraintTools::ConstraintTools()
{
}

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


ConstraintTools::~ConstraintTools()
{
}

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

void
ConstraintTools::
remove_dependent_linear_constraints( std::vector<NConstraintInterface*>& _constraints, const double _eps )
{
  // split into linear and nonlinear
  std::vector<NConstraintInterface*> lin_const, nonlin_const;

  for(unsigned int i=0; i<_constraints.size(); ++i)
  {
    if(_constraints[i]->is_linear() && _constraints[i]->constraint_type() == NConstraintInterface::NC_EQUAL)
      lin_const.push_back(_constraints[i]);
    else
      nonlin_const.push_back(_constraints[i]);
  }

  remove_dependent_linear_constraints_only_linear_equality( lin_const);

  for(unsigned int i=0; i<lin_const.size(); ++i)
    nonlin_const.push_back(lin_const[i]);

  // return filtered constraints
  _constraints.swap(nonlin_const);
}


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

void
ConstraintTools::
remove_dependent_linear_constraints_only_linear_equality( std::vector<NConstraintInterface*>& _constraints, const double _eps)
{
60
  DEB_enter_func;
61
62
63
64
  // make sure that constraints are available
  if(_constraints.empty()) return;

  // 1. copy (normalized) data into gmm dynamic sparse matrix
Max Lyon's avatar
Max Lyon committed
65
66
  size_t n(_constraints[0]->n_unknowns());
  size_t m(_constraints.size());
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
  std::vector<double> x(n, 0.0);
  NConstraintInterface::SVectorNC g;
  RMatrixGMM A;
  gmm::resize(A,m, n+1);
  for(unsigned int i=0; i<_constraints.size(); ++i)
  {
    // store rhs in last column
    A(i,n) = _constraints[i]->eval_constraint(x.data());
    // get and store coefficients
    _constraints[i]->eval_gradient(x.data(), g);
    double v_max(0.0);
    for (NConstraintInterface::SVectorNC::InnerIterator it(g); it; ++it)
    {
      A(i,it.index()) = it.value();
      v_max = std::max(v_max, std::abs(it.value()));
    }
    // normalize row
    if(v_max != 0.0)
      gmm::scale(A.row(i), 1.0/v_max);
  }

  // 2. get additionally column matrix to exploit column iterators
  CMatrixGMM Ac;
  gmm::resize(Ac, gmm::mat_nrows(A), gmm::mat_ncols(A));
  gmm::copy(A, Ac);

  // 3. initialize priorityqueue for sorting
  // init priority queue
Max Lyon's avatar
Max Lyon committed
95
  MutablePriorityQueueT<gmm::size_type, gmm::size_type> queue;
96
  queue.clear(m);
Max Lyon's avatar
Max Lyon committed
97
  for (gmm::size_type i = 0; i<m; ++i)
98
  {
Max Lyon's avatar
Max Lyon committed
99
100
101
    gmm::size_type cur_nnz = gmm::nnz( gmm::mat_row(A,i));
    if (A(i,n) != 0.0)
      --cur_nnz;
102
103
104
105
106
107

    queue.update(i, cur_nnz);
  }

  // track row status -1=undecided, 0=remove, 1=keep
  std::vector<int> row_status(m, -1);
Max Lyon's avatar
Max Lyon committed
108
  std::vector<gmm::size_type> keep;
109
110
111
112
113
114
//  std::vector<int> remove;

  // for all conditions
  while(!queue.empty())
  {
    // get next row
Max Lyon's avatar
Max Lyon committed
115
116
    gmm::size_type i = queue.get_next();
    gmm::size_type j = find_max_abs_coeff(A.row(i));
117
118
119
    double aij = A(i,j);
    if(std::abs(aij) <= _eps)
    {
120
121
//      std::cerr << "drop " << aij << "in row " << i << "and column " << j << std::endl;
      // constraint is linearly dependent
122
      row_status[i] = 0;
123
124
      if(std::abs(A(i,n)) > _eps)
        std::cerr << "Warning: found dependent constraint with nonzero rhs " << A(i,n) << std::endl;
125
126
127
    }
    else
    {
128
129
//      std::cerr << "keep " << aij << "in row " << i << "and column " << j << std::endl;

130
131
132
133
134
135
136
137
      // constraint is linearly independent
      row_status[i] = 1;
      keep.push_back(i);

      // update undecided constraints
      // copy col
      SVectorGMM col = Ac.col(j);

138
      // copy row
Max Lyon's avatar
Max Lyon committed
139
      SVectorGMM row = A.row(i);
140

141
      // iterate over column
Jan Möbius's avatar
Jan Möbius committed
142
143
      gmm::linalg_traits<SVectorGMM>::const_iterator c_it   = gmm::vect_const_begin(col);
      gmm::linalg_traits<SVectorGMM>::const_iterator c_end  = gmm::vect_const_end(col);
144
145
146
147
148

      for(; c_it != c_end; ++c_it)
        if( row_status[c_it.index()] == -1) // only process unvisited rows
        {
          // row idx
Max Lyon's avatar
Max Lyon committed
149
          gmm::size_type k = c_it.index();
150
151
152
153
154
155
156

          double s = -(*c_it)/aij;
          add_row_simultaneously( k, s, row, A, Ac, _eps);
          // make sure the eliminated entry is 0 on all other rows
          A( k, j) = 0;
          Ac(k, j) = 0;

Max Lyon's avatar
Max Lyon committed
157
158
159
          gmm::size_type cur_nnz = gmm::nnz( gmm::mat_row(A,k));
          if( A(k,n) != 0.0)
            --cur_nnz;
160
161
162
163
164
165

          queue.update(k, cur_nnz);
        }
    }
  }

166
167
  DEB_line(2, "removed " << _constraints.size()-keep.size() << 
    " dependent linear constraints out of " << _constraints.size());
168
169
170
171
172
173
174
175
176
177
178
179
180
181

  // 4. store result
  std::vector<NConstraintInterface*> new_constraints;
  for(unsigned int i=0; i<keep.size(); ++i)
    new_constraints.push_back(_constraints[keep[i]]);

  // return linearly independent ones
  _constraints.swap(new_constraints);
}


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


Max Lyon's avatar
Max Lyon committed
182
gmm::size_type
183
184
185
ConstraintTools::
find_max_abs_coeff(SVectorGMM& _v)
{
Max Lyon's avatar
Max Lyon committed
186
187
  size_t n = _v.size();
  gmm::size_type imax(0);
188
189
  double       vmax(0.0);

Jan Möbius's avatar
Jan Möbius committed
190
191
  gmm::linalg_traits<SVectorGMM>::const_iterator c_it   = gmm::vect_const_begin(_v);
  gmm::linalg_traits<SVectorGMM>::const_iterator c_end  = gmm::vect_const_end(_v);
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209

  for(; c_it != c_end; ++c_it)
    if(c_it.index() != n-1)
      if(std::abs(*c_it) > vmax)
      {
        imax = c_it.index();
        vmax = *c_it;
      }

  return imax;
}


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


void
ConstraintTools::
Max Lyon's avatar
Max Lyon committed
210
add_row_simultaneously( gmm::size_type _row_i,
211
212
213
214
215
216
                        double      _coeff,
                        SVectorGMM& _row,
                        RMatrixGMM& _rmat,
                        CMatrixGMM& _cmat,
                        const double _eps )
{
Jan Möbius's avatar
Jan Möbius committed
217
  typedef gmm::linalg_traits<SVectorGMM>::const_iterator RIter;
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
  RIter r_it  = gmm::vect_const_begin(_row);
  RIter r_end = gmm::vect_const_end(_row);

  for(; r_it!=r_end; ++r_it)
  {
    _rmat(_row_i, r_it.index()) += _coeff*(*r_it);
    _cmat(_row_i, r_it.index()) += _coeff*(*r_it);
    if( std::abs(_rmat(_row_i, r_it.index())) < _eps )
    {
      _rmat(_row_i, r_it.index()) = 0.0;
      _cmat(_row_i, r_it.index()) = 0.0;
    }
  }
}

//=============================================================================
} // namespace COMISO
//=============================================================================
#endif // COMISO_EIGEN3_AVAILABLE
//=============================================================================