ConstrainedSolverT.cc 46.6 KB
Newer Older
Henrik Zimmer's avatar
Henrik Zimmer committed
1
2
/*===========================================================================*\
 *                                                                           *
Henrik Zimmer's avatar
Henrik Zimmer committed
3
 *                               CoMISo                                      *
Henrik Zimmer's avatar
Henrik Zimmer committed
4
 *      Copyright (C) 2008-2009 by Computer Graphics Group, RWTH Aachen      *
Henrik Zimmer's avatar
Henrik Zimmer committed
5
 *                           www.rwth-graphics.de                            *
Henrik Zimmer's avatar
Henrik Zimmer committed
6
 *                                                                           *
Henrik Zimmer's avatar
Henrik Zimmer committed
7
8
 *---------------------------------------------------------------------------* 
 *  This file is part of CoMISo.                                             *
Henrik Zimmer's avatar
Henrik Zimmer committed
9
 *                                                                           *
Henrik Zimmer's avatar
Henrik Zimmer committed
10
11
12
13
 *  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.                                      *
Henrik Zimmer's avatar
Henrik Zimmer committed
14
15
16
17
 *                                                                           *
 *  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            *
Henrik Zimmer's avatar
Henrik Zimmer committed
18
 *  GNU General Public License for more details.                             *
Henrik Zimmer's avatar
Henrik Zimmer committed
19
 *                                                                           *
Henrik Zimmer's avatar
Henrik Zimmer committed
20
21
 *  You should have received a copy of the GNU General Public License        *
 *  along with CoMISo.  If not, see <http://www.gnu.org/licenses/>.          *
Henrik Zimmer's avatar
Henrik Zimmer committed
22
 *                                                                           *
Henrik Zimmer's avatar
Henrik Zimmer committed
23
\*===========================================================================*/ 
Henrik Zimmer's avatar
Henrik Zimmer committed
24
25


26
27
28
29
30
31
//=============================================================================
//
//  CLASS MISolver - IMPLEMENTATION
//
//=============================================================================

32
#define COMISO_CONSTRAINEDSOLVER_C
33
34
35
36
//== INCLUDES =================================================================

#include "ConstrainedSolver.hh"
#include <gmm/gmm.h>
David Bommes's avatar
David Bommes committed
37
#include "GMM_Tools.hh"
38
#include <float.h>
David Bommes's avatar
David Bommes committed
39
#include <CoMISo/Utils/MutablePriorityQueueT.hh>
40

41
42
43
44
#include <Base/Utils/StopWatch.hh>
#include <Base/Debug/DebOut.hh>


45
46
//== NAMESPACES ===============================================================

47
namespace COMISO {
48
49
50
51

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


David Bommes's avatar
David Bommes committed
52
53
54
template<class RMatrixT, class CMatrixT, class VectorT, class VectorIT >
void 
ConstrainedSolver::
David Bommes's avatar
David Bommes committed
55
solve_const(	 const RMatrixT& _constraints,
David Bommes's avatar
David Bommes committed
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
		 const CMatrixT& _A, 
		 VectorT&  _x,
		 const VectorT&  _rhs,
		 const VectorIT& _idx_to_round,
		 double    _reg_factor,
		 bool      _show_miso_settings,
		 bool      _show_timings )
{
  // copy matrices
  RMatrixT constraints( gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
  gmm::copy(_constraints, constraints);

  CMatrixT A( gmm::mat_nrows(_A), gmm::mat_ncols(_A));
  gmm::copy(_A, A);

  // ... and vectors
  VectorT  rhs(_rhs);
  VectorIT idx_to_round(_idx_to_round);

  // call non-const function
  solve(constraints,
	A,
	_x,
	rhs,
	idx_to_round,
	_reg_factor,
	_show_miso_settings,
	_show_timings);
}


87
88
89
//-----------------------------------------------------------------------------


David Bommes's avatar
David Bommes committed
90
91
92
template<class RMatrixT, class VectorT, class VectorIT >
void 
ConstrainedSolver::
93
94
solve_const( const RMatrixT& _constraints,
	     const RMatrixT& _B,
David Bommes's avatar
David Bommes committed
95
	     VectorT&  _x,
96
	     const VectorIT& _idx_to_round,
David Bommes's avatar
David Bommes committed
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
	     double    _reg_factor,
	     bool      _show_miso_settings,
	     bool      _show_timings )
{
  // copy matrices
  RMatrixT constraints( gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
  gmm::copy(_constraints, constraints);

  RMatrixT B( gmm::mat_nrows(_B), gmm::mat_ncols(_B));
  gmm::copy(_B, B);

  // ... and vectors
  VectorIT idx_to_round(_idx_to_round);

  // call non-const function
  solve(constraints,
	B,
	_x,
	idx_to_round,
	_reg_factor,
	_show_miso_settings,
	_show_timings);
}


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


125
126
127
128
129
130
131
132
133
134
135
136
template<class RMatrixT, class VectorT, class VectorIT >
void 
ConstrainedSolver::
solve(
    RMatrixT& _constraints,
    RMatrixT& _B, 
    VectorT&  _x,
    VectorIT& _idx_to_round,
    double    _reg_factor,
    bool      _show_miso_settings,
    bool      _show_timings )
{
David Bommes's avatar
David Bommes committed
137
138
139
  // convert into quadratic system
  VectorT rhs;
  gmm::col_matrix< gmm::rsvector< double > > A;
David Bommes's avatar
David Bommes committed
140
  COMISO_GMM::factored_to_quadratic(_B, A, rhs);
David Bommes's avatar
David Bommes committed
141
142
143
144
145
146
147
148
149
150
151
152

  // solve
  solve( _constraints, A, _x, rhs, 
	 _idx_to_round, _reg_factor, 
	 _show_miso_settings,
	 _show_timings);

//   int nrows = gmm::mat_nrows(_B);
//   int ncols = gmm::mat_ncols(_B);
//   int ncons = gmm::mat_nrows(_constraints);

//   if( _show_timings) std::cerr << __FUNCTION__ << "\n Initial dimension: " << nrows << " x " << ncols << ", number of constraints: " << ncons << std::endl;
153
 
David Bommes's avatar
David Bommes committed
154
//   // StopWatch for Timings
155
//   Base::StopWatch sw, sw2; sw.start(); sw2.start();
156

David Bommes's avatar
David Bommes committed
157
158
159
//   // c_elim[i] = index of variable which is eliminated in condition i
//   // or -1 if condition is invalid
//   std::vector<int> c_elim( ncons);
160

David Bommes's avatar
David Bommes committed
161
162
163
//   // apply sparse gauss elimination to make subsequent _constraints independent
//   make_constraints_independent( _constraints, _idx_to_round, c_elim);
//   double time_gauss = sw.stop()/1000.0; sw.start();
164

David Bommes's avatar
David Bommes committed
165
166
//   // eliminate conditions and return column matrix Bcol
//   gmm::col_matrix< gmm::rsvector< double > > Bcol( nrows, ncols);
167

David Bommes's avatar
David Bommes committed
168
169
//   // reindexing vector
//   std::vector<int>                          new_idx;
170

David Bommes's avatar
David Bommes committed
171
172
//   eliminate_constraints( _constraints, _B, _idx_to_round, c_elim, new_idx, Bcol);
//   double time_eliminate = sw.stop()/1000.0; 
173

David Bommes's avatar
David Bommes committed
174
//   if( _show_timings) std::cerr << "Eliminated dimension: " << gmm::mat_nrows(Bcol) << " x " << gmm::mat_ncols(Bcol) << std::endl;
175

David Bommes's avatar
David Bommes committed
176
177
178
//   // setup and solve system
//   double time_setup = setup_and_solve_system( Bcol, _x, _idx_to_round, _reg_factor, _show_miso_settings);
//   sw.start();
179

David Bommes's avatar
David Bommes committed
180
//   //  double time_setup_solve = sw.stop()/1000.0; sw.start();
181
  
David Bommes's avatar
David Bommes committed
182
183
//   // restore eliminated vars to fulfill the given conditions
//   restore_eliminated_vars( _constraints, _x, c_elim, new_idx);
184

David Bommes's avatar
David Bommes committed
185
//   double time_resubstitute = sw.stop()/1000.0; sw.start();
186

David Bommes's avatar
David Bommes committed
187
//   //  double time_total = sw2.stop()/1000.0;
188

David Bommes's avatar
David Bommes committed
189
190
191
192
193
194
195
//   if( _show_timings) std::cerr << "Timings: \n\t" <<
//     "Gauss Elimination  " << time_gauss          << " s\n\t" <<
//     "System Elimination " << time_eliminate      << " s\n\t" <<
//     "Setup              " << time_setup          << " s\n\t" <<
//    // "Setup + Mi-Solver  " << time_setup_solve    << " s\n\t" <<
//     "Resubstitution     " << time_resubstitute   << " s\n\t" << std::endl << std::endl;
//     //"Total              " << time_total          << std::endl;
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
}


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


template<class RMatrixT, class CMatrixT, class VectorT, class VectorIT>
void 
ConstrainedSolver::
solve(
    RMatrixT& _constraints,
    CMatrixT& _A, 
    VectorT&  _x,
    VectorT&  _rhs,
    VectorIT& _idx_to_round,
    double    _reg_factor,
    bool      _show_miso_settings, 
    bool      _show_timings )
{
215
  DEB_enter_func;
216
217
218
219
  // show options dialog
  if( _show_miso_settings)
    miso_.show_options_dialog();

220
221
222
  gmm::size_type nrows = gmm::mat_nrows(_A);
  gmm::size_type ncols = gmm::mat_ncols(_A);
  gmm::size_type ncons = gmm::mat_nrows(_constraints);
223

224
225
226
  DEB_out_if( _show_timings, 1, "Initital dimension: " << nrows << " x " << ncols 
                               << ", number of constraints: " << ncons
             << " use reordering: " << use_constraint_reordering() << "\n")
227
228

  // StopWatch for Timings
229
  Base::StopWatch sw, sw2; sw.start(); sw2.start();
230
231
232
233
234
235

  // c_elim[i] = index of variable which is eliminated in condition i
  // or -1 if condition is invalid
  std::vector<int> c_elim( ncons);

  // apply sparse gauss elimination to make subsequent _conditions independent
236
237
238
239
240
  if(use_constraint_reordering())
    make_constraints_independent_reordering( _constraints, _idx_to_round, c_elim);
  else
    make_constraints_independent( _constraints, _idx_to_round, c_elim);

241
242
243
244
245
246
247
248
249
  double time_gauss = sw.stop()/1000.0; sw.start();

  // re-indexing vector
  std::vector<int>                          new_idx;

  gmm::csc_matrix< double > Acsc;
  eliminate_constraints( _constraints, _A, _x, _rhs, _idx_to_round, c_elim, new_idx, Acsc);
  double time_eliminate = sw.stop()/1000.0;

250
251
252
253
  /// TODO: temporary disable this since it was causing performance issues
  //DEB_out_if( _show_timings, 2,
  //  "Eliminated dimension: " << Acsc.nr << " x " << Acsc.nc 
  //  << "\n#nonzeros: " << gmm::nnz(Acsc) << "\n");
254
255

  sw.start();
David Bommes's avatar
David Bommes committed
256
  miso_.solve( Acsc, _x, _rhs, _idx_to_round);
257
258
  double time_miso = sw.stop()/1000.0; sw.start();

259
  rhs_update_table_.store(_constraints, c_elim, new_idx);
260
261
262
263
264
  // restore eliminated vars to fulfill the given conditions
  restore_eliminated_vars( _constraints, _x, c_elim, new_idx);

  double time_resubstitute = sw.stop()/1000.0; sw.start();
  double time_total = time_gauss + time_eliminate + time_miso + time_resubstitute;
265
  DEB_out_if( _show_timings, 1,"Timings: \n\t" <<
266
267
268
269
    "\tGauss Elimination  " << time_gauss          << " s\n\t" <<
    "\tSystem Elimination " << time_eliminate      << " s\n\t" <<
    "\tMi-Solver          " << time_miso           << " s\n\t" <<
    "\tResubstitution     " << time_resubstitute   << " s\n\t" << 
270
    "\tTotal              " << time_total          << "\n\n");
271
272
273
274
275
276
}


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


277
278
279
280
281
282
template<class RMatrixT, class VectorT >
void
ConstrainedSolver::
resolve(
    const RMatrixT& _B,
    VectorT&  _x,
283
    VectorT*  _constraint_rhs,
284
285
286
287
288
289
290
291
    bool      _show_timings )
{
  // extract rhs from quadratic system
  VectorT rhs;
 // gmm::col_matrix< gmm::rsvector< double > > A;
 // COMISO_GMM::factored_to_quadratic(_B, A, rhs);
  //TODO only compute rhs, not complete A for efficiency

Andreas Fabri's avatar
Andreas Fabri committed
292
293
  gmm::size_type m = gmm::mat_nrows(_B);
  gmm::size_type n = gmm::mat_ncols(_B);
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320

  typedef typename gmm::linalg_traits<RMatrixT>::const_sub_row_type CRowT;
  typedef typename gmm::linalg_traits<RMatrixT>::sub_row_type       RowT;
  typedef typename gmm::linalg_traits<CRowT>::const_iterator        RIter;
  typedef typename gmm::linalg_traits<CRowT>::value_type            VecValT;

  gmm::resize(rhs, n-1);
  gmm::clear(rhs);
  for(unsigned int i = 0; i < m; ++i)
  {
    // get current condition row
    CRowT row       = gmm::mat_const_row( _B, i);
    RIter row_it    = gmm::vect_const_begin( row);
    RIter row_end   = gmm::vect_const_end( row);

    if(row_end == row_it) continue;
    --row_end;
    if(row_end.index() != n-1) continue;
    VecValT n_i = *row_end;
    while(row_end != row_it)
    {
      --row_end;
      rhs[row_end.index()] -= (*row_end) * n_i;
    }
  }

  // solve
321
  resolve(_x, _constraint_rhs, &rhs,
322
323
324
325
326
327
328
   _show_timings);
}


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


David Bommes's avatar
David Bommes committed
329
template<class VectorT >
330
331
332
void
ConstrainedSolver::
resolve(
333
334
335
336
     VectorT&  _x,
     VectorT*  _constraint_rhs,
     VectorT*  _rhs           ,
     bool      _show_timings   )
337
{
338
  DEB_enter_func;
339
  // StopWatch for Timings
340
  Base::StopWatch sw;
341
342
343

  sw.start();
  // apply stored updates and eliminations to exchanged rhs
344
345
346
347
348
349
350
  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
Andreas Fabri's avatar
Andreas Fabri committed
351
352
    gmm::size_type nc = gmm::mat_ncols(rhs_update_table_.constraints_p_);
    for(gmm::size_type i=0; i<rhs_update_table_.cur_constraint_rhs_.size(); ++i)
353
354
355
356
357
358
359
360
361
      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);
362

363
364
365
366
367
368
369
370
  //  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);
371
372
373
374
375
376
377
378

  double time_miso = sw.stop()/1000.0; sw.start();

  // restore eliminated vars to fulfill the given conditions
  restore_eliminated_vars( rhs_update_table_.constraints_p_, _x, rhs_update_table_.c_elim_, rhs_update_table_.new_idx_);

  double time_resubstitute = sw.stop()/1000.0; sw.start();
  double time_total = time_miso + time_resubstitute;
379
  DEB_out_if( _show_timings, 1, "Timings: \n\t" <<
380
381
    "\tMi-Solver          " << time_miso           << " s\n\t" <<
    "\tResubstitution     " << time_resubstitute   << " s\n\t" <<
382
    "\tTotal              " << time_total          << "\n\n");
383
384
385
386
387
388
}


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


389
390
391
392
393
394
395
396
template<class RMatrixT, class VectorIT >
void 
ConstrainedSolver::
make_constraints_independent(
    RMatrixT&         _constraints,
		VectorIT&         _idx_to_round,
		std::vector<int>& _c_elim)
{
397
  DEB_enter_func;
398
  // setup linear transformation for rhs, start with identity
Andreas Fabri's avatar
Andreas Fabri committed
399
  gmm::size_type nr = gmm::mat_nrows(_constraints);
400
401
  gmm::resize(rhs_update_table_.D_, nr, nr);
  gmm::clear(rhs_update_table_.D_);
Andreas Fabri's avatar
Andreas Fabri committed
402
  for(gmm::size_type i=0; i<nr; ++i) rhs_update_table_.D_(i,i) = 1.0;
403

404
  //  Base::StopWatch sw;
405
  // number of variables
Andreas Fabri's avatar
Andreas Fabri committed
406
  const gmm::size_type n_vars = gmm::mat_ncols(_constraints);
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428

  // TODO Check: HZ added 14.08.09 
  _c_elim.clear();
  _c_elim.resize( gmm::mat_nrows(_constraints), -1);

  // build round map
  std::vector<bool> roundmap( n_vars, false);
  for(unsigned int i=0; i<_idx_to_round.size(); ++i)
    roundmap[_idx_to_round[i]] = true;

  // copy constraints into column matrix (for faster update via iterators)
  typedef gmm::wsvector<double>      CVector;
  typedef gmm::col_matrix< CVector > CMatrix;
  CMatrix constraints_c;
  gmm::resize(constraints_c, gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
  gmm::copy(_constraints, constraints_c);

  // for all conditions
  for(unsigned int i=0; i<gmm::mat_nrows(_constraints); ++i)
  {
    // get elimination variable
    int elim_j = -1;
429
    int elim_int_j = -1;
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444

    // iterate over current row, until variable found
    // first search for real valued variable
    // if not found for integers with value +-1
    // and finally take the smallest integer variable

    typedef typename gmm::linalg_traits<RMatrixT>::const_sub_row_type CRowT;
    typedef typename gmm::linalg_traits<RMatrixT>::sub_row_type       RowT;
    typedef typename gmm::linalg_traits<CRowT>::const_iterator        RIter;

    // get current condition row
    CRowT row       = gmm::mat_const_row( _constraints, i);
    RIter row_it    = gmm::vect_const_begin( row);
    RIter row_end   = gmm::vect_const_end( row);
    double elim_val = FLT_MAX;
445
    double max_elim_val = -FLT_MAX;
446

Henrik Zimmer's avatar
   
Henrik Zimmer committed
447
448
449
450
    // new: gcd
    std::vector<int> v_gcd;
    v_gcd.resize(gmm::nnz(row),-1);
    int n_ints(0);
David Bommes's avatar
   
David Bommes committed
451
    bool gcd_update_valid(true);
Henrik Zimmer's avatar
   
Henrik Zimmer committed
452

453
454
    for(; row_it != row_end; ++row_it)
    {
Andreas Fabri's avatar
Andreas Fabri committed
455
      int cur_j = static_cast<int>(row_it.index());
456
457
458
      // do not use the constant part
      if(  cur_j != n_vars - 1 )
      {
459
        // found real valued var? -> finished (UPDATE: no not any more, find biggest real value to avoid x/1e-13)
460
461
        if( !roundmap[ cur_j ])
        {
462
463
          if( fabs(*row_it) > max_elim_val)
          {
Max Lyon's avatar
Max Lyon committed
464
            elim_j = (int)cur_j;
465
466
467
            max_elim_val = fabs(*row_it);
          }
          //break;
468
469
        }
        else
470
        {
471
          double cur_row_val(fabs(*row_it));
Henrik Zimmer's avatar
   
Henrik Zimmer committed
472
473
474
          // gcd
          // if the coefficient of an integer variable is not an integer, then
          // the variable most problably will not be (expect if all coeffs are the same, e.g. 0.5)
Max Lyon's avatar
Max Lyon committed
475
476
477
478
479
          if ((double(int(cur_row_val))- cur_row_val) != 0.0)
          {
            DEB_warning(2, "coefficient of integer variable is NOT integer : " << cur_row_val)
            gcd_update_valid = false;
          }
Henrik Zimmer's avatar
   
Henrik Zimmer committed
480

Andreas Fabri's avatar
Andreas Fabri committed
481
          v_gcd[n_ints] = static_cast<int>(cur_row_val);
Henrik Zimmer's avatar
   
Henrik Zimmer committed
482
483
          ++n_ints;

484
485
          // store integer closest to 1, must be greater than epsilon_
          if( fabs(cur_row_val-1.0) < elim_val && cur_row_val > epsilon_)
Andreas Fabri's avatar
Andreas Fabri committed
486
            {
487
            elim_int_j   = cur_j;
Henrik Zimmer's avatar
   
Henrik Zimmer committed
488
            elim_val     = fabs(cur_row_val-1.0);
489
          }
490
        }
491
492
493
      }
    }

494
495
496
497
498
499
    // first try to eliminate a valid (>epsilon_) real valued variable (safer)
    if( max_elim_val > epsilon_)
    {}
    else // use the best found integer
      elim_j = elim_int_j;

500
501
502
    // if no integer or real valued variable greater than epsilon_ existed, then
    // elim_j is now -1 and this row is not considered as a valid constraint

503
504


Henrik Zimmer's avatar
   
Henrik Zimmer committed
505

506
507
508
    // store result
    _c_elim[i] = elim_j;
    // error check result
Henrik Zimmer's avatar
   
Henrik Zimmer committed
509
    if( elim_j == -1)
510
    {
Henrik Zimmer's avatar
   
Henrik Zimmer committed
511
512
      // redundant or incompatible?
      if( noisy_ > 0)
513
514
        DEB_warning_if( (fabs(gmm::mat_const_row(_constraints, i)[n_vars-1]) > epsilon_ ), 1,
          "incompatible condition: " << fabs(gmm::mat_const_row(_constraints, i)[n_vars-1]) )
Henrik Zimmer's avatar
   
Henrik Zimmer committed
515
516
      //         else
      //           std::cerr << "Warning: redundant condition:\n";
517
    }
Henrik Zimmer's avatar
   
Henrik Zimmer committed
518
519
520
    else
      if(roundmap[elim_j] && elim_val > 1e-6) 
      {
David Bommes's avatar
   
David Bommes committed
521
        if( do_gcd_ && gcd_update_valid)
Henrik Zimmer's avatar
   
Henrik Zimmer committed
522
523
524
        {
          // perform gcd update
          bool gcd_ok = update_constraint_gcd( _constraints, i, elim_j, v_gcd, n_ints);
525
526
          DEB_warning_if( (noisy_ > 0) && !gcd_ok, 1," GCD update failed! " 
            << DEB_os_str(gmm::mat_const_row(_constraints, i)) )
Henrik Zimmer's avatar
   
Henrik Zimmer committed
527
528
529
        }
        else
        {
530
531
532
533
534
535
                 DEB_warning_if( ( noisy_ > 0) && !do_gcd_, 1,
             "NO +-1 coefficient found, integer rounding cannot be guaranteed. Try using the GCD option! "
             << DEB_os_str( gmm::mat_const_row(_constraints, i)) )
            DEB_warning_if(  ( noisy_ > 0) && do_gcd_, 1, 
             "GCD of non-integer cannot be computed! " 
             << DEB_os_str( gmm::mat_const_row(_constraints, i)) )
Henrik Zimmer's avatar
   
Henrik Zimmer committed
536
537
538
        }
      }

539
540
541
542
543

    // is this condition dependent?
    if( elim_j != -1 )
    {
      // get elim variable value
544
      double elim_val_cur = _constraints(i, elim_j);
545
546
547
548
549
550
551
552
553
554
555

      // copy col
      CVector col = constraints_c.col(elim_j);

      // iterate over column
      typename gmm::linalg_traits<CVector>::const_iterator c_it   = gmm::vect_const_begin(col);
      typename gmm::linalg_traits<CVector>::const_iterator c_end  = gmm::vect_const_end(col);

      for(; c_it != c_end; ++c_it)
        if( c_it.index() > i)
        {
David Bommes's avatar
David Bommes committed
556
	  //          sw.start();
557
558
          double val = -(*c_it)/elim_val_cur;

Max Lyon's avatar
Max Lyon committed
559
          add_row_simultaneously((int)c_it.index(), val, gmm::mat_row(_constraints, i), _constraints, constraints_c);
David Bommes's avatar
David Bommes committed
560
561
562
          // 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;
563
564
565
566

          // 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()));
David Bommes's avatar
David Bommes committed
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
        }
    }
  }
}



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


template<class RMatrixT, class VectorIT >
void 
ConstrainedSolver::
make_constraints_independent_reordering(
    RMatrixT&         _constraints,
		VectorIT&         _idx_to_round,
		std::vector<int>& _c_elim)
{
585
  DEB_enter_func;
586
  // setup linear transformation for rhs, start with identity
Andreas Fabri's avatar
Andreas Fabri committed
587
  gmm::size_type nr = gmm::mat_nrows(_constraints);
588
589
  gmm::resize(rhs_update_table_.D_, nr, nr);
  gmm::clear(rhs_update_table_.D_);
Max Lyon's avatar
Max Lyon committed
590
591
592

  for(gmm::size_type i=0; i<nr; ++i)
    rhs_update_table_.D_(i,i) = 1.0;
593

594
  //  Base::StopWatch sw;
David Bommes's avatar
David Bommes committed
595
  // number of variables
Andreas Fabri's avatar
Andreas Fabri committed
596
597
  // AF: Why was n_vars signed? Can it be zero? Later we subtract 1
  const gmm::size_type n_vars = gmm::mat_ncols(_constraints);
David Bommes's avatar
David Bommes committed
598
599
600
601
602
603
604

  // TODO Check: HZ added 14.08.09 
  _c_elim.clear();
  _c_elim.resize( gmm::mat_nrows(_constraints), -1);

  // build round map
  std::vector<bool> roundmap( n_vars, false);
Max Lyon's avatar
Max Lyon committed
605
  for(size_t i=0; i<_idx_to_round.size(); ++i)
David Bommes's avatar
David Bommes committed
606
607
608
609
610
611
612
613
614
615
616
    roundmap[_idx_to_round[i]] = true;

  // copy constraints into column matrix (for faster update via iterators)
  typedef gmm::wsvector<double>      CVector;
  typedef gmm::col_matrix< CVector > CMatrix;
  CMatrix constraints_c;
  gmm::resize(constraints_c, gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
  gmm::copy(_constraints, constraints_c);

  // init priority queue
  MutablePriorityQueueT<unsigned int, unsigned int> queue;
Andreas Fabri's avatar
Andreas Fabri committed
617
  queue.clear( static_cast<int>(nr) );
David Bommes's avatar
David Bommes committed
618
619
  for(unsigned int i=0; i<nr; ++i)
  {
Andreas Fabri's avatar
Andreas Fabri committed
620
    gmm::size_type cur_nnz = gmm::nnz( gmm::mat_row(_constraints,i));
Max Lyon's avatar
Max Lyon committed
621
622
    if( _constraints(i,n_vars-1) != 0.0)
      --cur_nnz;
David Bommes's avatar
David Bommes committed
623

Andreas Fabri's avatar
Andreas Fabri committed
624
    queue.update(i, static_cast<int>(cur_nnz));
David Bommes's avatar
David Bommes committed
625
626
627
  }
  
  std::vector<bool> row_visited(nr, false);
Max Lyon's avatar
Max Lyon committed
628
629
  std::vector<gmm::size_type> row_ordering;
  row_ordering.reserve(nr);
David Bommes's avatar
David Bommes committed
630
631
632
633
634
635
636


  // for all conditions
  //  for(unsigned int i=0; i<gmm::mat_nrows(_constraints); ++i)
  while(!queue.empty())
  {
    // get next row
Max Lyon's avatar
Max Lyon committed
637
    auto i = queue.get_next();
David Bommes's avatar
David Bommes committed
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
    row_ordering.push_back(i);
    row_visited[i] = true;

    // get elimination variable
    int elim_j = -1;
    int elim_int_j = -1;

    // iterate over current row, until variable found
    // first search for real valued variable
    // if not found for integers with value +-1
    // and finally take the smallest integer variable

    typedef typename gmm::linalg_traits<RMatrixT>::const_sub_row_type CRowT;
    typedef typename gmm::linalg_traits<RMatrixT>::sub_row_type       RowT;
    typedef typename gmm::linalg_traits<CRowT>::const_iterator        RIter;

    // get current condition row
    CRowT row       = gmm::mat_const_row( _constraints, i);
    RIter row_it    = gmm::vect_const_begin( row);
    RIter row_end   = gmm::vect_const_end( row);
    double elim_val = FLT_MAX;
    double max_elim_val = -FLT_MAX;

    // new: gcd
    std::vector<int> v_gcd;
    v_gcd.resize(gmm::nnz(row),-1);
    int n_ints(0);
    bool gcd_update_valid(true);

    for(; row_it != row_end; ++row_it)
    {
Andreas Fabri's avatar
Andreas Fabri committed
669
      int cur_j = static_cast<int>(row_it.index());
David Bommes's avatar
David Bommes committed
670
      // do not use the constant part
Max Lyon's avatar
Max Lyon committed
671
      if (cur_j != n_vars - 1)
David Bommes's avatar
David Bommes committed
672
673
      {
        // found real valued var? -> finished (UPDATE: no not any more, find biggest real value to avoid x/1e-13)
Max Lyon's avatar
Max Lyon committed
674
        if (!roundmap[ cur_j ])
David Bommes's avatar
David Bommes committed
675
        {
Max Lyon's avatar
Max Lyon committed
676
          if (fabs(*row_it) > max_elim_val)
David Bommes's avatar
David Bommes committed
677
          {
Max Lyon's avatar
Max Lyon committed
678
            elim_j = (int)cur_j;
David Bommes's avatar
David Bommes committed
679
680
681
682
683
684
685
686
687
688
            max_elim_val = fabs(*row_it);
          }
          //break;
        }
        else
        {
          double cur_row_val(fabs(*row_it));
          // gcd
          // if the coefficient of an integer variable is not an integer, then
          // the variable most problably will not be (expect if all coeffs are the same, e.g. 0.5)
Max Lyon's avatar
Max Lyon committed
689
690
691
692
693
          if ((double(int(cur_row_val))- cur_row_val) != 0.0)
          {
            DEB_warning(2, "coefficient of integer variable is NOT integer : " << cur_row_val);
            gcd_update_valid = false;
          }
David Bommes's avatar
David Bommes committed
694

Andreas Fabri's avatar
Andreas Fabri committed
695
          v_gcd[n_ints] = static_cast<int>(cur_row_val);
David Bommes's avatar
David Bommes committed
696
697
698
699
700
          ++n_ints;

          // store integer closest to 1, must be greater than epsilon_
          if( fabs(cur_row_val-1.0) < elim_val && cur_row_val > epsilon_)
          {
Max Lyon's avatar
Max Lyon committed
701
            elim_int_j   = (int)cur_j;
David Bommes's avatar
David Bommes committed
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
            elim_val     = fabs(cur_row_val-1.0);
          }
        }
      }
    }

    // first try to eliminate a valid (>epsilon_) real valued variable (safer)
    if( max_elim_val > epsilon_)
    {}
    else // use the best found integer
      elim_j = elim_int_j;

    // if no integer or real valued variable greater than epsilon_ existed, then
    // elim_j is now -1 and this row is not considered as a valid constraint




    // store result
    _c_elim[i] = elim_j;
    // error check result
    if( elim_j == -1)
    {
      // redundant or incompatible?
      if( noisy_ > 0)
727
728
729
        DEB_warning_if( (fabs(gmm::mat_const_row(_constraints, i)[n_vars-1]) > epsilon_ ), 1,
          "incompatible condition: "
          << fabs(gmm::mat_const_row(_constraints, i)[n_vars-1]) )
David Bommes's avatar
David Bommes committed
730
731
732
733
734
735
736
737
738
      //         else
      //           std::cerr << "Warning: redundant condition:\n";
    }
    else
      if(roundmap[elim_j] && elim_val > 1e-6) 
      {
        if( do_gcd_ && gcd_update_valid)
        {
          // perform gcd update
Max Lyon's avatar
Max Lyon committed
739
          bool gcd_ok = update_constraint_gcd( _constraints, (int)i, elim_j, v_gcd, n_ints);
740
741
          DEB_warning_if( !gcd_ok && (noisy_ > 0), 1, " GCD update failed! "
              << DEB_os_str(gmm::mat_const_row(_constraints, i)) )
David Bommes's avatar
David Bommes committed
742
743
744
745
746
747
        }
        else
        {
          if( noisy_ > 0)
	  {
	    if( !do_gcd_)
748
749
              DEB_warning(1, "NO +-1 coefficient found, integer rounding cannot be guaranteed. Try using the GCD option! " 
          << DEB_os_str(gmm::mat_const_row(_constraints, i)) )
David Bommes's avatar
David Bommes committed
750
	    else
751
752
              DEB_warning(1, "GCD of non-integer cannot be computed! "
          << DEB_os_str(gmm::mat_const_row(_constraints, i)) )
David Bommes's avatar
David Bommes committed
753
754
755
756
757
758
759

	  }
        }
      }


    // is this condition dependent?
Max Lyon's avatar
Max Lyon committed
760
    if (elim_j != -1)
David Bommes's avatar
David Bommes committed
761
762
763
764
765
766
767
768
    {
      // get elim variable value
      double elim_val_cur = _constraints(i, elim_j);

      // copy col
      CVector col = constraints_c.col(elim_j);

      // iterate over column
Max Lyon's avatar
Max Lyon committed
769
770
      typename gmm::linalg_traits<CVector>::const_iterator c_it = gmm::vect_const_begin(col);
      typename gmm::linalg_traits<CVector>::const_iterator c_end = gmm::vect_const_end(col);
David Bommes's avatar
David Bommes committed
771

Max Lyon's avatar
Max Lyon committed
772
773
774
775
      for (; c_it != c_end; ++c_it)
      {
//        if( c_it.index() > i)
        if (!row_visited[c_it.index()])
David Bommes's avatar
David Bommes committed
776
        {
Max Lyon's avatar
Max Lyon committed
777
778
779
//          sw.start();
          double val = -(*c_it) / elim_val_cur;
          add_row_simultaneously((int)c_it.index(), val, gmm::mat_row(_constraints, i), _constraints, constraints_c);
780
          // make sure the eliminated entry is 0 on all other rows and not 1e-17
Max Lyon's avatar
Max Lyon committed
781
          _constraints(c_it.index(), elim_j) = 0;
782
          constraints_c(c_it.index(), elim_j) = 0;
David Bommes's avatar
David Bommes committed
783

Andreas Fabri's avatar
Andreas Fabri committed
784
          gmm::size_type cur_idx = c_it.index();
Max Lyon's avatar
Max Lyon committed
785
786
787
          gmm::size_type cur_nnz = gmm::nnz( gmm::mat_row(_constraints,cur_idx));
          if( _constraints(cur_idx,n_vars-1) != 0.0)
            --cur_nnz;
David Bommes's avatar
David Bommes committed
788

Max Lyon's avatar
Max Lyon committed
789
          queue.update(static_cast<int>(cur_idx),
Andreas Fabri's avatar
Andreas Fabri committed
790
                       static_cast<int>(cur_nnz));
791
792
793

          // update linear transition of rhs
          gmm::add(gmm::scaled(gmm::mat_row(rhs_update_table_.D_, i), val),
Max Lyon's avatar
Max Lyon committed
794
            gmm::mat_row(rhs_update_table_.D_, c_it.index()));
795
        }
Max Lyon's avatar
Max Lyon committed
796
      }
797
798
    }
  }
David Bommes's avatar
David Bommes committed
799
800
801
802
803
804
805
806
807
808
  // // check result
  // for(unsigned int i=0; i<row_visited.size(); ++i)
  //   if( !row_visited[i])
  //     std::cerr <<"FAT ERROR: row " << i << " not visited...\n";
  


  // correct ordering
  RMatrixT c_tmp(gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
  gmm::copy(_constraints,c_tmp);
809
810
  RowMatrix d_tmp(gmm::mat_nrows(rhs_update_table_.D_),gmm::mat_ncols(rhs_update_table_.D_));
  gmm::copy(rhs_update_table_.D_,d_tmp);
David Bommes's avatar
David Bommes committed
811
812
813
814
815
816
817
818
819
820
821
822

  // std::vector<int> elim_temp2(_c_elim);
  // std::sort(elim_temp2.begin(), elim_temp2.end());
  // std::cerr << elim_temp2 << std::endl;


  std::vector<int> elim_temp(_c_elim);
  _c_elim.resize(0); _c_elim.resize( elim_temp.size(),-1);

  for(unsigned int i=0; i<nr; ++i)
  {
    gmm::copy(gmm::mat_row(c_tmp,row_ordering[i]), gmm::mat_row(_constraints,i));
823
824
    gmm::copy(gmm::mat_row(d_tmp,row_ordering[i]), gmm::mat_row(rhs_update_table_.D_,i));

David Bommes's avatar
David Bommes committed
825
826
827
828
829
830
831
832
833
834
    _c_elim[i] = elim_temp[row_ordering[i]];
  }

  // // hack
  // elim_temp = _c_elim;
  // std::sort(elim_temp.begin(), elim_temp.end());
  // std::cerr << elim_temp << std::endl;

  // std::sort(row_ordering.begin(), row_ordering.end());
  // std::cerr << "row ordering: " << row_ordering << std::endl;
835
836
}

David Bommes's avatar
David Bommes committed
837

838
839
//-----------------------------------------------------------------------------

Henrik Zimmer's avatar
   
Henrik Zimmer committed
840
841
842
843
844
845
846
847
848
template<class RMatrixT>
bool
ConstrainedSolver::
update_constraint_gcd( RMatrixT& _constraints, 
                       int _row_i, 
                       int& _elim_j,
                       std::vector<int>& _v_gcd,
                       int& _n_ints)
{
849
  DEB_enter_func;
Henrik Zimmer's avatar
   
Henrik Zimmer committed
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
  // find gcd
  double i_gcd = find_gcd(_v_gcd, _n_ints);

  if( fabs(i_gcd) == 1.0)
    return false;
  
  // divide by gcd
  typedef typename gmm::linalg_traits<RMatrixT>::const_sub_row_type CRowT;
  typedef typename gmm::linalg_traits<CRowT>::const_iterator        RIter;

  // get current constraint row
  RIter row_it    = gmm::vect_const_begin( gmm::mat_const_row( _constraints, _row_i));
  RIter row_end   = gmm::vect_const_end( gmm::mat_const_row( _constraints, _row_i));

  for( ; row_it!=row_end; ++row_it)
  {
Andreas Fabri's avatar
Andreas Fabri committed
866
    gmm::size_type cur_j = row_it.index();
Henrik Zimmer's avatar
   
Henrik Zimmer committed
867
868
    _constraints(_row_i, cur_j) = (*row_it)/i_gcd;
  }
Max Lyon's avatar
Max Lyon committed
869
  gmm::size_type elim_coeff = static_cast<gmm::size_type>(std::abs(_constraints(_row_i, _elim_j)));
870
871
  DEB_error_if( elim_coeff != 1, "elimination coefficient " << elim_coeff 
    << " will (most probably) NOT lead to an integer solution!")
Henrik Zimmer's avatar
   
Henrik Zimmer committed
872
873
874
875
  return true;
}

//-----------------------------------------------------------------------------
876
877
878
879
880
881
882
883
884
885
886
887

template<class SVector1T, class SVector2T, class VectorIT, class SVector3T>
void 
ConstrainedSolver::
eliminate_constraints( 
    gmm::row_matrix<SVector1T>& _constraints,
    gmm::row_matrix<SVector2T>& _B, 
    VectorIT&                   _idx_to_round,
    std::vector<int>&           _c_elim,
    std::vector<int>&           _new_idx,
    gmm::col_matrix<SVector3T>& _Bcol)
{
888
  DEB_enter_func;
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
  // copy into column matrix
  gmm::resize(_Bcol, gmm::mat_nrows(_B), gmm::mat_ncols(_B));
  gmm::copy( _B, _Bcol);

  // store columns which should be eliminated
  std::vector<int> elim_cols;
  elim_cols.reserve( _c_elim.size());

  for(unsigned int i=0; i<_c_elim.size(); ++i)
  {
    int cur_j = _c_elim[i];

    if( cur_j != -1)
    {
      double cur_val = _constraints(i,cur_j);

      // store index
      elim_cols.push_back(_c_elim[i]);

      // copy col
      SVector3T col = _Bcol.col(cur_j);

      // iterate over column
      typename gmm::linalg_traits<SVector3T>::const_iterator c_it   = gmm::vect_const_begin(col);
      typename gmm::linalg_traits<SVector3T>::const_iterator c_end  = gmm::vect_const_end(col);

      for(; c_it != c_end; ++c_it)
        add_row( c_it.index(), -(*c_it)/cur_val, gmm::mat_row(_constraints, i), _Bcol);
    }
  }

  // eliminate columns
  eliminate_columns( _Bcol, elim_cols);

  // TODO FIXME Size -1 ?!?!
  // init _new_idx vector
  _new_idx.resize(gmm::mat_ncols(_constraints));
  for(unsigned int i=0; i<_new_idx.size(); ++i)
    _new_idx[i] = i;

  // update _new_idx w.r.t. eliminated cols
David Bommes's avatar
David Bommes committed
930
  COMISO_GMM::eliminate_vars_idx( elim_cols, _new_idx, -1);
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949

  // update _idx_to_round (in place)
  std::vector<int> round_old(_idx_to_round);
  unsigned int wi = 0;
  for(unsigned int i=0; i<_idx_to_round.size(); ++i)
  {
    if(_new_idx[ _idx_to_round[i]] != -1)
    {
      _idx_to_round[wi] = _new_idx[_idx_to_round[i]];
      ++wi;
    }
  }
  
  // resize, sort and make unique
  _idx_to_round.resize(wi);

  std::sort(_idx_to_round.begin(), _idx_to_round.end());
  _idx_to_round.resize( std::unique(_idx_to_round.begin(), _idx_to_round.end()) -_idx_to_round.begin());

950

951
952
  DEB_out_if((noisy_ >2), 2, "remaining         variables: " << gmm::mat_ncols(_Bcol) << "\n");
  DEB_out_if((noisy_ >2), 2,  "remaining integer variables: " << _idx_to_round.size() << "\n");
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
}


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


template<class SVector1T, class SVector2T, class VectorIT, class CSCMatrixT>
void 
ConstrainedSolver::
eliminate_constraints(
    gmm::row_matrix<SVector1T>& _constraints,
    gmm::col_matrix<SVector2T>& _A, 
    std::vector<double>&        _x, 
    std::vector<double>&        _rhs, 
    VectorIT&                   _idx_to_round,
    std::vector<int>&           _v_elim,
    std::vector<int>&           _new_idx,
    CSCMatrixT&                 _Acsc)
{
972
973
  DEB_enter_func;
  Base::StopWatch sw;
974
975
976
977
978
979
980
981
982
  sw.start();
  // define iterator on matrix A and on constraints C
  typedef typename gmm::linalg_traits<SVector2T>::const_iterator  AIter;
  typedef typename gmm::linalg_traits<SVector1T>::const_iterator  CIter;

  // store variable indices to be eliminated
  std::vector<int> elim_varids;
  elim_varids.reserve( _v_elim.size());

983
  rhs_update_table_.clear();
984
  std::vector<double> constraint_rhs_vec(_constraints.nrows());
985

986
987
988
989
990
991
992
993
994
  for( unsigned int i=0; i < _v_elim.size(); ++i)
  {
    int cur_j = _v_elim[i];

    if( cur_j != -1)
    {
      double cur_val = _constraints(i, cur_j);

      // store index
995
996
      elim_varids.push_back(cur_j);
      rhs_update_table_.add_elim_id(cur_j);
997
998
999
1000

      // copy col
      SVector2T col ( _A.col( cur_j));

For faster browsing, not all history is shown. View entire blame