1 /*===========================================================================*\
2 * *
3 * CoMISo *
4 * Copyright (C) 2008-2009 by Computer Graphics Group, RWTH Aachen *
5 * www.rwth-graphics.de *
6 * *
7 *---------------------------------------------------------------------------*
8 * This file is part of CoMISo. *
9 * *
10 * CoMISo is free software: you can redistribute it and/or modify *
11 * it under the terms of the GNU General Public License as published by *
12 * the Free Software Foundation, either version 3 of the License, or *
13 * (at your option) any later version. *
14 * *
15 * CoMISo is distributed in the hope that it will be useful, *
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
18 * GNU General Public License for more details. *
19 * *
20 * You should have received a copy of the GNU General Public License *
21 * along with CoMISo. If not, see <http://www.gnu.org/licenses/>. *
22 * *
23 \*===========================================================================*/
24
25
26 //=============================================================================
27 //
28 // CLASS MISolver - IMPLEMENTATION
29 //
30 //=============================================================================
31
32 #define COMISO_CONSTRAINEDSOLVER_C
33 //== INCLUDES =================================================================
34
35 #include "ConstrainedSolver.hh"
36 #include <gmm/gmm.h>
37 #include "GMM_Tools.hh"
38 #include <float.h>
39 #include <CoMISo/Utils/StopWatch.hh>
40 #include <CoMISo/Utils/MutablePriorityQueueT.hh>
41
42 //== NAMESPACES ===============================================================
43
44 namespace COMISO {
45
46 //== IMPLEMENTATION ==========================================================
47
48
49 template<class RMatrixT, class CMatrixT, class VectorT, class VectorIT >
50 void
51 ConstrainedSolver::
solve_const(const RMatrixT & _constraints,const CMatrixT & _A,VectorT & _x,const VectorT & _rhs,const VectorIT & _idx_to_round,double _reg_factor,bool _show_miso_settings,bool _show_timings)52 solve_const( const RMatrixT& _constraints,
53 const CMatrixT& _A,
54 VectorT& _x,
55 const VectorT& _rhs,
56 const VectorIT& _idx_to_round,
57 double _reg_factor,
58 bool _show_miso_settings,
59 bool _show_timings )
60 {
61 // copy matrices
62 RMatrixT constraints( gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
63 gmm::copy(_constraints, constraints);
64
65 CMatrixT A( gmm::mat_nrows(_A), gmm::mat_ncols(_A));
66 gmm::copy(_A, A);
67
68 // ... and vectors
69 VectorT rhs(_rhs);
70 VectorIT idx_to_round(_idx_to_round);
71
72 // call non-const function
73 solve(constraints,
74 A,
75 _x,
76 rhs,
77 idx_to_round,
78 _reg_factor,
79 _show_miso_settings,
80 _show_timings);
81 }
82
83
84 //-----------------------------------------------------------------------------
85
86
87 template<class RMatrixT, class VectorT, class VectorIT >
88 void
89 ConstrainedSolver::
solve_const(const RMatrixT & _constraints,const RMatrixT & _B,VectorT & _x,const VectorIT & _idx_to_round,double _reg_factor,bool _show_miso_settings,bool _show_timings)90 solve_const( const RMatrixT& _constraints,
91 const RMatrixT& _B,
92 VectorT& _x,
93 const VectorIT& _idx_to_round,
94 double _reg_factor,
95 bool _show_miso_settings,
96 bool _show_timings )
97 {
98 // copy matrices
99 RMatrixT constraints( gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
100 gmm::copy(_constraints, constraints);
101
102 RMatrixT B( gmm::mat_nrows(_B), gmm::mat_ncols(_B));
103 gmm::copy(_B, B);
104
105 // ... and vectors
106 VectorIT idx_to_round(_idx_to_round);
107
108 // call non-const function
109 solve(constraints,
110 B,
111 _x,
112 idx_to_round,
113 _reg_factor,
114 _show_miso_settings,
115 _show_timings);
116 }
117
118
119 //-----------------------------------------------------------------------------
120
121
122 template<class RMatrixT, class VectorT, class VectorIT >
123 void
124 ConstrainedSolver::
solve(RMatrixT & _constraints,RMatrixT & _B,VectorT & _x,VectorIT & _idx_to_round,double _reg_factor,bool _show_miso_settings,bool _show_timings)125 solve(
126 RMatrixT& _constraints,
127 RMatrixT& _B,
128 VectorT& _x,
129 VectorIT& _idx_to_round,
130 double _reg_factor,
131 bool _show_miso_settings,
132 bool _show_timings )
133 {
134 // convert into quadratic system
135 VectorT rhs;
136 gmm::col_matrix< gmm::rsvector< double > > A;
137 COMISO_GMM::factored_to_quadratic(_B, A, rhs);
138
139 // solve
140 solve( _constraints, A, _x, rhs,
141 _idx_to_round, _reg_factor,
142 _show_miso_settings,
143 _show_timings);
144
145 // int nrows = gmm::mat_nrows(_B);
146 // int ncols = gmm::mat_ncols(_B);
147 // int ncons = gmm::mat_nrows(_constraints);
148
149 // if( _show_timings) std::cerr << __FUNCTION__ << "\n Initial dimension: " << nrows << " x " << ncols << ", number of constraints: " << ncons << std::endl;
150
151 // // StopWatch for Timings
152 // COMISO::StopWatch sw, sw2; sw.start(); sw2.start();
153
154 // // c_elim[i] = index of variable which is eliminated in condition i
155 // // or -1 if condition is invalid
156 // std::vector<int> c_elim( ncons);
157
158 // // apply sparse gauss elimination to make subsequent _constraints independent
159 // make_constraints_independent( _constraints, _idx_to_round, c_elim);
160 // double time_gauss = sw.stop()/1000.0; sw.start();
161
162 // // eliminate conditions and return column matrix Bcol
163 // gmm::col_matrix< gmm::rsvector< double > > Bcol( nrows, ncols);
164
165 // // reindexing vector
166 // std::vector<int> new_idx;
167
168 // eliminate_constraints( _constraints, _B, _idx_to_round, c_elim, new_idx, Bcol);
169 // double time_eliminate = sw.stop()/1000.0;
170
171 // if( _show_timings) std::cerr << "Eliminated dimension: " << gmm::mat_nrows(Bcol) << " x " << gmm::mat_ncols(Bcol) << std::endl;
172
173 // // setup and solve system
174 // double time_setup = setup_and_solve_system( Bcol, _x, _idx_to_round, _reg_factor, _show_miso_settings);
175 // sw.start();
176
177 // // double time_setup_solve = sw.stop()/1000.0; sw.start();
178
179 // // restore eliminated vars to fulfill the given conditions
180 // restore_eliminated_vars( _constraints, _x, c_elim, new_idx);
181
182 // double time_resubstitute = sw.stop()/1000.0; sw.start();
183
184 // // double time_total = sw2.stop()/1000.0;
185
186 // if( _show_timings) std::cerr << "Timings: \n\t" <<
187 // "Gauss Elimination " << time_gauss << " s\n\t" <<
188 // "System Elimination " << time_eliminate << " s\n\t" <<
189 // "Setup " << time_setup << " s\n\t" <<
190 // // "Setup + Mi-Solver " << time_setup_solve << " s\n\t" <<
191 // "Resubstitution " << time_resubstitute << " s\n\t" << std::endl << std::endl;
192 // //"Total " << time_total << std::endl;
193 }
194
195
196 //-----------------------------------------------------------------------------
197
198
199 template<class RMatrixT, class CMatrixT, class VectorT, class VectorIT>
200 void
201 ConstrainedSolver::
solve(RMatrixT & _constraints,CMatrixT & _A,VectorT & _x,VectorT & _rhs,VectorIT & _idx_to_round,double _reg_factor,bool _show_miso_settings,bool _show_timings)202 solve(
203 RMatrixT& _constraints,
204 CMatrixT& _A,
205 VectorT& _x,
206 VectorT& _rhs,
207 VectorIT& _idx_to_round,
208 double _reg_factor,
209 bool _show_miso_settings,
210 bool _show_timings )
211 {
212 // show options dialog
213 if( _show_miso_settings)
214 miso_.show_options_dialog();
215
216
217 int nrows = gmm::mat_nrows(_A);
218 int ncols = gmm::mat_ncols(_A);
219 int ncons = gmm::mat_nrows(_constraints);
220
221 if( _show_timings) std::cerr << __FUNCTION__ << "\n Initital dimension: " << nrows << " x " << ncols
222 << ", number of constraints: " << ncons << " use reordering: " << use_constraint_reordering() << std::endl;
223
224 // StopWatch for Timings
225 COMISO::StopWatch sw, sw2; sw.start(); sw2.start();
226
227 // c_elim[i] = index of variable which is eliminated in condition i
228 // or -1 if condition is invalid
229 std::vector<int> c_elim( ncons);
230
231 // apply sparse gauss elimination to make subsequent _conditions independent
232 if(use_constraint_reordering())
233 make_constraints_independent_reordering( _constraints, _idx_to_round, c_elim);
234 else
235 make_constraints_independent( _constraints, _idx_to_round, c_elim);
236
237 double time_gauss = sw.stop()/1000.0; sw.start();
238
239 // re-indexing vector
240 std::vector<int> new_idx;
241
242 gmm::csc_matrix< double > Acsc;
243 eliminate_constraints( _constraints, _A, _x, _rhs, _idx_to_round, c_elim, new_idx, Acsc);
244 double time_eliminate = sw.stop()/1000.0;
245
246 if( _show_timings)
247 {
248 std::cerr << "Eliminated dimension: " << Acsc.nr << " x " << Acsc.nc << std::endl;
249 std::cerr << "#nonzeros: " << gmm::nnz(Acsc) << std::endl;
250 }
251
252 sw.start();
253 miso_.solve( Acsc, _x, _rhs, _idx_to_round);
254 double time_miso = sw.stop()/1000.0; sw.start();
255
256 rhs_update_table_.store(_constraints, c_elim, new_idx);
257 // restore eliminated vars to fulfill the given conditions
258 restore_eliminated_vars( _constraints, _x, c_elim, new_idx);
259
260 double time_resubstitute = sw.stop()/1000.0; sw.start();
261 double time_total = time_gauss + time_eliminate + time_miso + time_resubstitute;
262 if( _show_timings) std::cerr << "Timings: \n\t" <<
263 "\tGauss Elimination " << time_gauss << " s\n\t" <<
264 "\tSystem Elimination " << time_eliminate << " s\n\t" <<
265 "\tMi-Solver " << time_miso << " s\n\t" <<
266 "\tResubstitution " << time_resubstitute << " s\n\t" <<
267 "\tTotal " << time_total << std::endl << std::endl;
268 }
269
270
271 //-----------------------------------------------------------------------------
272
273
274 template<class RMatrixT, class VectorT >
275 void
276 ConstrainedSolver::
resolve(const RMatrixT & _B,VectorT & _x,VectorT * _constraint_rhs,bool _show_timings)277 resolve(
278 const RMatrixT& _B,
279 VectorT& _x,
280 VectorT* _constraint_rhs,
281 bool _show_timings )
282 {
283 // extract rhs from quadratic system
284 VectorT rhs;
285 // gmm::col_matrix< gmm::rsvector< double > > A;
286 // COMISO_GMM::factored_to_quadratic(_B, A, rhs);
287 //TODO only compute rhs, not complete A for efficiency
288
289 unsigned int m = gmm::mat_nrows(_B);
290 unsigned int n = gmm::mat_ncols(_B);
291
292 typedef typename gmm::linalg_traits<RMatrixT>::const_sub_row_type CRowT;
293 typedef typename gmm::linalg_traits<RMatrixT>::sub_row_type RowT;
294 typedef typename gmm::linalg_traits<CRowT>::const_iterator RIter;
295 typedef typename gmm::linalg_traits<CRowT>::value_type VecValT;
296
297 gmm::resize(rhs, n-1);
298 gmm::clear(rhs);
299 for(unsigned int i = 0; i < m; ++i)
300 {
301 // get current condition row
302 CRowT row = gmm::mat_const_row( _B, i);
303 RIter row_it = gmm::vect_const_begin( row);
304 RIter row_end = gmm::vect_const_end( row);
305
306 if(row_end == row_it) continue;
307 --row_end;
308 if(row_end.index() != n-1) continue;
309 VecValT n_i = *row_end;
310 while(row_end != row_it)
311 {
312 --row_end;
313 rhs[row_end.index()] -= (*row_end) * n_i;
314 }
315 }
316
317 // solve
318 resolve(_x, _constraint_rhs, &rhs,
319 _show_timings);
320 }
321
322
323 //-----------------------------------------------------------------------------
324
325
326 template<class VectorT >
327 void
328 ConstrainedSolver::
resolve(VectorT & _x,VectorT * _constraint_rhs,VectorT * _rhs,bool _show_timings)329 resolve(
330 VectorT& _x,
331 VectorT* _constraint_rhs,
332 VectorT* _rhs ,
333 bool _show_timings )
334 {
335 // StopWatch for Timings
336 COMISO::StopWatch sw;
337
338 sw.start();
339 // apply stored updates and eliminations to exchanged rhs
340 if(_constraint_rhs)
341 {
342 // apply linear transformation of Gaussian elimination
343 rhs_update_table_.cur_constraint_rhs_.resize(gmm::mat_nrows(rhs_update_table_.D_));
344 gmm::mult(rhs_update_table_.D_, *_constraint_rhs, rhs_update_table_.cur_constraint_rhs_);
345
346 // update rhs of stored constraints
347 unsigned int nc = gmm::mat_ncols(rhs_update_table_.constraints_p_);
348 for(unsigned int i=0; i<rhs_update_table_.cur_constraint_rhs_.size(); ++i)
349 rhs_update_table_.constraints_p_(i,nc-1) = -rhs_update_table_.cur_constraint_rhs_[i];
350 }
351 if(_rhs)
352 rhs_update_table_.cur_rhs_ = *_rhs;
353
354 std::vector<double> rhs_red = rhs_update_table_.cur_rhs_;
355
356 rhs_update_table_.apply(rhs_update_table_.cur_constraint_rhs_, rhs_red);
357 rhs_update_table_.eliminate(rhs_red);
358
359 // std::cerr << "############### Resolve info ##############" << std::endl;
360 // std::cerr << rhs_update_table_.D_ << std::endl;
361 // std::cerr << rhs_update_table_.cur_rhs_ << std::endl;
362 // std::cerr << rhs_update_table_.cur_constraint_rhs_ << std::endl;
363 // std::cerr << rhs_update_table_.table_.size() << std::endl;
364 // std::cerr << "rhs_red: " << rhs_red << std::endl;
365
366 miso_.resolve(_x, rhs_red);
367
368 double time_miso = sw.stop()/1000.0; sw.start();
369
370 // restore eliminated vars to fulfill the given conditions
371 restore_eliminated_vars( rhs_update_table_.constraints_p_, _x, rhs_update_table_.c_elim_, rhs_update_table_.new_idx_);
372
373 double time_resubstitute = sw.stop()/1000.0; sw.start();
374 double time_total = time_miso + time_resubstitute;
375 if( _show_timings) std::cerr << "Timings: \n\t" <<
376 "\tMi-Solver " << time_miso << " s\n\t" <<
377 "\tResubstitution " << time_resubstitute << " s\n\t" <<
378 "\tTotal " << time_total << std::endl << std::endl;
379 }
380
381
382 //-----------------------------------------------------------------------------
383
384
385 template<class RMatrixT, class VectorIT >
386 void
387 ConstrainedSolver::
make_constraints_independent(RMatrixT & _constraints,VectorIT & _idx_to_round,std::vector<int> & _c_elim)388 make_constraints_independent(
389 RMatrixT& _constraints,
390 VectorIT& _idx_to_round,
391 std::vector<int>& _c_elim)
392 {
393 // setup linear transformation for rhs, start with identity
394 unsigned int nr = gmm::mat_nrows(_constraints);
395 gmm::resize(rhs_update_table_.D_, nr, nr);
396 gmm::clear(rhs_update_table_.D_);
397 for(unsigned int i=0; i<nr; ++i) rhs_update_table_.D_(i,i) = 1.0;
398
399 // COMISO::StopWatch sw;
400 // number of variables
401 int n_vars = gmm::mat_ncols(_constraints);
402
403 // TODO Check: HZ added 14.08.09
404 _c_elim.clear();
405 _c_elim.resize( gmm::mat_nrows(_constraints), -1);
406
407 // build round map
408 std::vector<bool> roundmap( n_vars, false);
409 for(unsigned int i=0; i<_idx_to_round.size(); ++i)
410 roundmap[_idx_to_round[i]] = true;
411
412 // copy constraints into column matrix (for faster update via iterators)
413 typedef gmm::wsvector<double> CVector;
414 typedef gmm::col_matrix< CVector > CMatrix;
415 CMatrix constraints_c;
416 gmm::resize(constraints_c, gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
417 gmm::copy(_constraints, constraints_c);
418
419 // for all conditions
420 for(unsigned int i=0; i<gmm::mat_nrows(_constraints); ++i)
421 {
422 // get elimination variable
423 int elim_j = -1;
424 int elim_int_j = -1;
425
426 // iterate over current row, until variable found
427 // first search for real valued variable
428 // if not found for integers with value +-1
429 // and finally take the smallest integer variable
430
431 typedef typename gmm::linalg_traits<RMatrixT>::const_sub_row_type CRowT;
432 typedef typename gmm::linalg_traits<RMatrixT>::sub_row_type RowT;
433 typedef typename gmm::linalg_traits<CRowT>::const_iterator RIter;
434
435 // get current condition row
436 CRowT row = gmm::mat_const_row( _constraints, i);
437 RIter row_it = gmm::vect_const_begin( row);
438 RIter row_end = gmm::vect_const_end( row);
439 double elim_val = FLT_MAX;
440 double max_elim_val = -FLT_MAX;
441
442 // new: gcd
443 std::vector<int> v_gcd;
444 v_gcd.resize(gmm::nnz(row),-1);
445 int n_ints(0);
446 bool gcd_update_valid(true);
447
448 for(; row_it != row_end; ++row_it)
449 {
450 int cur_j = row_it.index();
451 // do not use the constant part
452 if( cur_j != n_vars - 1 )
453 {
454 // found real valued var? -> finished (UPDATE: no not any more, find biggest real value to avoid x/1e-13)
455 if( !roundmap[ cur_j ])
456 {
457 if( fabs(*row_it) > max_elim_val)
458 {
459 elim_j = cur_j;
460 max_elim_val = fabs(*row_it);
461 }
462 //break;
463 }
464 else
465 {
466 double cur_row_val(fabs(*row_it));
467 // gcd
468 // if the coefficient of an integer variable is not an integer, then
469 // the variable most problably will not be (expect if all coeffs are the same, e.g. 0.5)
470 if( (double(int(cur_row_val))- cur_row_val) != 0.0)
471 {
472 // std::cerr << __FUNCTION__ << " Warning: coefficient of integer variable is NOT integer: "
473 // << cur_row_val << std::endl;
474 gcd_update_valid = false;
475 }
476
477 v_gcd[n_ints] = cur_row_val;
478 ++n_ints;
479
480 // store integer closest to 1, must be greater than epsilon_
481 if( fabs(cur_row_val-1.0) < elim_val && cur_row_val > epsilon_)
482 {
483 elim_int_j = cur_j;
484 elim_val = fabs(cur_row_val-1.0);
485 }
486 }
487 }
488 }
489
490 // first try to eliminate a valid (>epsilon_) real valued variable (safer)
491 if( max_elim_val > epsilon_)
492 {}
493 else // use the best found integer
494 elim_j = elim_int_j;
495
496 // if no integer or real valued variable greater than epsilon_ existed, then
497 // elim_j is now -1 and this row is not considered as a valid constraint
498
499
500
501
502 // store result
503 _c_elim[i] = elim_j;
504 // error check result
505 if( elim_j == -1)
506 {
507 // redundant or incompatible?
508 if( noisy_ > 0)
509 if( fabs(gmm::mat_const_row(_constraints, i)[n_vars-1]) > epsilon_ )
510 std::cerr << "Warning: incompatible condition: "
511 << fabs(gmm::mat_const_row(_constraints, i)[n_vars-1]) << std::endl;
512 // else
513 // std::cerr << "Warning: redundant condition:\n";
514 }
515 else
516 if(roundmap[elim_j] && elim_val > 1e-6)
517 {
518 if( do_gcd_ && gcd_update_valid)
519 {
520 // perform gcd update
521 bool gcd_ok = update_constraint_gcd( _constraints, i, elim_j, v_gcd, n_ints);
522 if( !gcd_ok)
523 if( noisy_ > 0)
524 std::cerr << __FUNCTION__ << " Warning: GCD update failed! " << gmm::mat_const_row(_constraints, i) << std::endl;
525 }
526 else
527 {
528 if( noisy_ > 0)
529 {
530 if( !do_gcd_)
531 std::cerr << __FUNCTION__ << " Warning: NO +-1 coefficient found, integer rounding cannot be guaranteed. Try using the GCD option! " << gmm::mat_const_row(_constraints, i) << std::endl;
532 else
533 std::cerr << __FUNCTION__ << " Warning: GCD of non-integer cannot be computed! " << gmm::mat_const_row(_constraints, i) << std::endl;
534
535 }
536 }
537 }
538
539
540 // is this condition dependent?
541 if( elim_j != -1 )
542 {
543 // get elim variable value
544 double elim_val_cur = _constraints(i, elim_j);
545
546 // copy col
547 CVector col = constraints_c.col(elim_j);
548
549 // iterate over column
550 typename gmm::linalg_traits<CVector>::const_iterator c_it = gmm::vect_const_begin(col);
551 typename gmm::linalg_traits<CVector>::const_iterator c_end = gmm::vect_const_end(col);
552
553 for(; c_it != c_end; ++c_it)
554 if( c_it.index() > i)
555 {
556 // sw.start();
557 double val = -(*c_it)/elim_val_cur;
558
559 add_row_simultaneously( c_it.index(), val, gmm::mat_row(_constraints, i), _constraints, constraints_c);
560 // make sure the eliminated entry is 0 on all other rows and not 1e-17
561 _constraints( c_it.index(), elim_j) = 0;
562 constraints_c(c_it.index(), elim_j) = 0;
563
564 // update linear transition of rhs
565 gmm::add(gmm::scaled(gmm::mat_row(rhs_update_table_.D_, i), val),
566 gmm::mat_row(rhs_update_table_.D_, c_it.index()));
567 }
568 }
569 }
570 }
571
572
573
574 //-----------------------------------------------------------------------------
575
576
577 template<class RMatrixT, class VectorIT >
578 void
579 ConstrainedSolver::
make_constraints_independent_reordering(RMatrixT & _constraints,VectorIT & _idx_to_round,std::vector<int> & _c_elim)580 make_constraints_independent_reordering(
581 RMatrixT& _constraints,
582 VectorIT& _idx_to_round,
583 std::vector<int>& _c_elim)
584 {
585 // setup linear transformation for rhs, start with identity
586 unsigned int nr = gmm::mat_nrows(_constraints);
587 gmm::resize(rhs_update_table_.D_, nr, nr);
588 gmm::clear(rhs_update_table_.D_);
589 for(unsigned int i=0; i<nr; ++i) rhs_update_table_.D_(i,i) = 1.0;
590
591 // COMISO::StopWatch sw;
592 // number of variables
593 int n_vars = gmm::mat_ncols(_constraints);
594
595 // TODO Check: HZ added 14.08.09
596 _c_elim.clear();
597 _c_elim.resize( gmm::mat_nrows(_constraints), -1);
598
599 // build round map
600 std::vector<bool> roundmap( n_vars, false);
601 for(unsigned int i=0; i<_idx_to_round.size(); ++i)
602 roundmap[_idx_to_round[i]] = true;
603
604 // copy constraints into column matrix (for faster update via iterators)
605 typedef gmm::wsvector<double> CVector;
606 typedef gmm::col_matrix< CVector > CMatrix;
607 CMatrix constraints_c;
608 gmm::resize(constraints_c, gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
609 gmm::copy(_constraints, constraints_c);
610
611 // init priority queue
612 MutablePriorityQueueT<unsigned int, unsigned int> queue;
613 queue.clear( nr );
614 for(unsigned int i=0; i<nr; ++i)
615 {
616 int cur_nnz = gmm::nnz( gmm::mat_row(_constraints,i));
617 if( _constraints(i,n_vars-1) != 0.0) --cur_nnz;
618
619 queue.update(i, cur_nnz);
620 }
621
622 std::vector<bool> row_visited(nr, false);
623 std::vector<unsigned int> row_ordering; row_ordering.reserve(nr);
624
625
626 // for all conditions
627 // for(unsigned int i=0; i<gmm::mat_nrows(_constraints); ++i)
628 while(!queue.empty())
629 {
630 // get next row
631 unsigned int i = queue.get_next();
632 row_ordering.push_back(i);
633 row_visited[i] = true;
634
635 // get elimination variable
636 int elim_j = -1;
637 int elim_int_j = -1;
638
639 // iterate over current row, until variable found
640 // first search for real valued variable
641 // if not found for integers with value +-1
642 // and finally take the smallest integer variable
643
644 typedef typename gmm::linalg_traits<RMatrixT>::const_sub_row_type CRowT;
645 typedef typename gmm::linalg_traits<RMatrixT>::sub_row_type RowT;
646 typedef typename gmm::linalg_traits<CRowT>::const_iterator RIter;
647
648 // get current condition row
649 CRowT row = gmm::mat_const_row( _constraints, i);
650 RIter row_it = gmm::vect_const_begin( row);
651 RIter row_end = gmm::vect_const_end( row);
652 double elim_val = FLT_MAX;
653 double max_elim_val = -FLT_MAX;
654
655 // new: gcd
656 std::vector<int> v_gcd;
657 v_gcd.resize(gmm::nnz(row),-1);
658 int n_ints(0);
659 bool gcd_update_valid(true);
660
661 for(; row_it != row_end; ++row_it)
662 {
663 int cur_j = row_it.index();
664 // do not use the constant part
665 if( cur_j != n_vars - 1 )
666 {
667 // found real valued var? -> finished (UPDATE: no not any more, find biggest real value to avoid x/1e-13)
668 if( !roundmap[ cur_j ])
669 {
670 if( fabs(*row_it) > max_elim_val)
671 {
672 elim_j = cur_j;
673 max_elim_val = fabs(*row_it);
674 }
675 //break;
676 }
677 else
678 {
679 double cur_row_val(fabs(*row_it));
680 // gcd
681 // if the coefficient of an integer variable is not an integer, then
682 // the variable most problably will not be (expect if all coeffs are the same, e.g. 0.5)
683 if( (double(int(cur_row_val))- cur_row_val) != 0.0)
684 {
685 // std::cerr << __FUNCTION__ << " Warning: coefficient of integer variable is NOT integer: "
686 // << cur_row_val << std::endl;
687 gcd_update_valid = false;
688 }
689
690 v_gcd[n_ints] = cur_row_val;
691 ++n_ints;
692
693 // store integer closest to 1, must be greater than epsilon_
694 if( fabs(cur_row_val-1.0) < elim_val && cur_row_val > epsilon_)
695 {
696 elim_int_j = cur_j;
697 elim_val = fabs(cur_row_val-1.0);
698 }
699 }
700 }
701 }
702
703 // first try to eliminate a valid (>epsilon_) real valued variable (safer)
704 if( max_elim_val > epsilon_)
705 {}
706 else // use the best found integer
707 elim_j = elim_int_j;
708
709 // if no integer or real valued variable greater than epsilon_ existed, then
710 // elim_j is now -1 and this row is not considered as a valid constraint
711
712
713
714
715 // store result
716 _c_elim[i] = elim_j;
717 // error check result
718 if( elim_j == -1)
719 {
720 // redundant or incompatible?
721 if( noisy_ > 0)
722 if( fabs(gmm::mat_const_row(_constraints, i)[n_vars-1]) > epsilon_ )
723 std::cerr << "Warning: incompatible condition: "
724 << fabs(gmm::mat_const_row(_constraints, i)[n_vars-1]) << std::endl;
725 // else
726 // std::cerr << "Warning: redundant condition:\n";
727 }
728 else
729 if(roundmap[elim_j] && elim_val > 1e-6)
730 {
731 if( do_gcd_ && gcd_update_valid)
732 {
733 // perform gcd update
734 bool gcd_ok = update_constraint_gcd( _constraints, i, elim_j, v_gcd, n_ints);
735 if( !gcd_ok)
736 if( noisy_ > 0)
737 std::cerr << __FUNCTION__ << " Warning: GCD update failed! " << gmm::mat_const_row(_constraints, i) << std::endl;
738 }
739 else
740 {
741 if( noisy_ > 0)
742 {
743 if( !do_gcd_)
744 std::cerr << __FUNCTION__ << " Warning: NO +-1 coefficient found, integer rounding cannot be guaranteed. Try using the GCD option! " << gmm::mat_const_row(_constraints, i) << std::endl;
745 else
746 std::cerr << __FUNCTION__ << " Warning: GCD of non-integer cannot be computed! " << gmm::mat_const_row(_constraints, i) << std::endl;
747
748 }
749 }
750 }
751
752
753 // is this condition dependent?
754 if( elim_j != -1 )
755 {
756 // get elim variable value
757 double elim_val_cur = _constraints(i, elim_j);
758
759 // copy col
760 CVector col = constraints_c.col(elim_j);
761
762 // iterate over column
763 typename gmm::linalg_traits<CVector>::const_iterator c_it = gmm::vect_const_begin(col);
764 typename gmm::linalg_traits<CVector>::const_iterator c_end = gmm::vect_const_end(col);
765
766 for(; c_it != c_end; ++c_it)
767 // if( c_it.index() > i)
768 if( !row_visited[c_it.index()])
769 {
770 // sw.start();
771 double val = -(*c_it)/elim_val_cur;
772 add_row_simultaneously( c_it.index(), val, gmm::mat_row(_constraints, i), _constraints, constraints_c);
773 // make sure the eliminated entry is 0 on all other rows and not 1e-17
774 _constraints( c_it.index(), elim_j) = 0;
775 constraints_c(c_it.index(), elim_j) = 0;
776
777 int cur_idx = c_it.index();
778 int cur_nnz = gmm::nnz( gmm::mat_row(_constraints,cur_idx));
779 if( _constraints(cur_idx,n_vars-1) != 0.0) --cur_nnz;
780
781 queue.update(cur_idx, cur_nnz);
782
783 // update linear transition of rhs
784 gmm::add(gmm::scaled(gmm::mat_row(rhs_update_table_.D_, i), val),
785 gmm::mat_row(rhs_update_table_.D_, c_it.index()));
786 }
787 }
788 }
789 // // check result
790 // for(unsigned int i=0; i<row_visited.size(); ++i)
791 // if( !row_visited[i])
792 // std::cerr <<"FAT ERROR: row " << i << " not visited...\n";
793
794
795
796 // correct ordering
797 RMatrixT c_tmp(gmm::mat_nrows(_constraints), gmm::mat_ncols(_constraints));
798 gmm::copy(_constraints,c_tmp);
799 RowMatrix d_tmp(gmm::mat_nrows(rhs_update_table_.D_),gmm::mat_ncols(rhs_update_table_.D_));
800 gmm::copy(rhs_update_table_.D_,d_tmp);
801
802 // std::vector<int> elim_temp2(_c_elim);
803 // std::sort(elim_temp2.begin(), elim_temp2.end());
804 // std::cerr << elim_temp2 << std::endl;
805
806
807 std::vector<int> elim_temp(_c_elim);
808 _c_elim.resize(0); _c_elim.resize( elim_temp.size(),-1);
809
810 for(unsigned int i=0; i<nr; ++i)
811 {
812 gmm::copy(gmm::mat_row(c_tmp,row_ordering[i]), gmm::mat_row(_constraints,i));
813 gmm::copy(gmm::mat_row(d_tmp,row_ordering[i]), gmm::mat_row(rhs_update_table_.D_,i));
814
815 _c_elim[i] = elim_temp[row_ordering[i]];
816 }
817
818 // // hack
819 // elim_temp = _c_elim;
820 // std::sort(elim_temp.begin(), elim_temp.end());
821 // std::cerr << elim_temp << std::endl;
822
823 // std::sort(row_ordering.begin(), row_ordering.end());
824 // std::cerr << "row ordering: " << row_ordering << std::endl;
825 }
826
827
828 //-----------------------------------------------------------------------------
829
830 template<class RMatrixT>
831 bool
832 ConstrainedSolver::
update_constraint_gcd(RMatrixT & _constraints,int _row_i,int & _elim_j,std::vector<int> & _v_gcd,int & _n_ints)833 update_constraint_gcd( RMatrixT& _constraints,
834 int _row_i,
835 int& _elim_j,
836 std::vector<int>& _v_gcd,
837 int& _n_ints)
838 {
839 // find gcd
840 double i_gcd = find_gcd(_v_gcd, _n_ints);
841
842 if( fabs(i_gcd) == 1.0)
843 return false;
844
845 // divide by gcd
846 typedef typename gmm::linalg_traits<RMatrixT>::const_sub_row_type CRowT;
847 typedef typename gmm::linalg_traits<CRowT>::const_iterator RIter;
848
849 // get current constraint row
850 RIter row_it = gmm::vect_const_begin( gmm::mat_const_row( _constraints, _row_i));
851 RIter row_end = gmm::vect_const_end( gmm::mat_const_row( _constraints, _row_i));
852
853 for( ; row_it!=row_end; ++row_it)
854 {
855 int cur_j = row_it.index();
856 _constraints(_row_i, cur_j) = (*row_it)/i_gcd;
857 }
858 int elim_coeff = abs(_constraints(_row_i, _elim_j));
859 if( elim_coeff != 1)
860 std::cerr << __FUNCTION__ << " Error: elimination coefficient " << elim_coeff << " will (most probably) NOT lead to an integer solution!" << std::endl;
861 return true;
862 }
863
864 //-----------------------------------------------------------------------------
865
866 template<class SVector1T, class SVector2T, class VectorIT, class SVector3T>
867 void
868 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)869 eliminate_constraints(
870 gmm::row_matrix<SVector1T>& _constraints,
871 gmm::row_matrix<SVector2T>& _B,
872 VectorIT& _idx_to_round,
873 std::vector<int>& _c_elim,
874 std::vector<int>& _new_idx,
875 gmm::col_matrix<SVector3T>& _Bcol)
876 {
877 // copy into column matrix
878 gmm::resize(_Bcol, gmm::mat_nrows(_B), gmm::mat_ncols(_B));
879 gmm::copy( _B, _Bcol);
880
881 // store columns which should be eliminated
882 std::vector<int> elim_cols;
883 elim_cols.reserve( _c_elim.size());
884
885 for(unsigned int i=0; i<_c_elim.size(); ++i)
886 {
887 int cur_j = _c_elim[i];
888
889 if( cur_j != -1)
890 {
891 double cur_val = _constraints(i,cur_j);
892
893 // store index
894 elim_cols.push_back(_c_elim[i]);
895
896 // copy col
897 SVector3T col = _Bcol.col(cur_j);
898
899 // iterate over column
900 typename gmm::linalg_traits<SVector3T>::const_iterator c_it = gmm::vect_const_begin(col);
901 typename gmm::linalg_traits<SVector3T>::const_iterator c_end = gmm::vect_const_end(col);
902
903 for(; c_it != c_end; ++c_it)
904 add_row( c_it.index(), -(*c_it)/cur_val, gmm::mat_row(_constraints, i), _Bcol);
905 }
906 }
907
908 // eliminate columns
909 eliminate_columns( _Bcol, elim_cols);
910
911 // TODO FIXME Size -1 ?!?!
912 // init _new_idx vector
913 _new_idx.resize(gmm::mat_ncols(_constraints));
914 for(unsigned int i=0; i<_new_idx.size(); ++i)
915 _new_idx[i] = i;
916
917 // update _new_idx w.r.t. eliminated cols
918 COMISO_GMM::eliminate_vars_idx( elim_cols, _new_idx, -1);
919
920 // update _idx_to_round (in place)
921 std::vector<int> round_old(_idx_to_round);
922 unsigned int wi = 0;
923 for(unsigned int i=0; i<_idx_to_round.size(); ++i)
924 {
925 if(_new_idx[ _idx_to_round[i]] != -1)
926 {
927 _idx_to_round[wi] = _new_idx[_idx_to_round[i]];
928 ++wi;
929 }
930 }
931
932 // resize, sort and make unique
933 _idx_to_round.resize(wi);
934
935 std::sort(_idx_to_round.begin(), _idx_to_round.end());
936 _idx_to_round.resize( std::unique(_idx_to_round.begin(), _idx_to_round.end()) -_idx_to_round.begin());
937
938
939 if( noisy_ > 2 )
940 {
941 std::cerr << __FUNCTION__ << "remaining variables: " << gmm::mat_ncols(_Bcol) << std::endl;
942 std::cerr << __FUNCTION__ << "remaining integer variables: " << _idx_to_round.size() << std::endl;
943 std::cerr << __FUNCTION__ << std::endl;
944 }
945 }
946
947
948 //-----------------------------------------------------------------------------
949
950
951 template<class SVector1T, class SVector2T, class VectorIT, class CSCMatrixT>
952 void
953 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)954 eliminate_constraints(
955 gmm::row_matrix<SVector1T>& _constraints,
956 gmm::col_matrix<SVector2T>& _A,
957 std::vector<double>& _x,
958 std::vector<double>& _rhs,
959 VectorIT& _idx_to_round,
960 std::vector<int>& _v_elim,
961 std::vector<int>& _new_idx,
962 CSCMatrixT& _Acsc)
963 {
964 COMISO::StopWatch sw;
965 sw.start();
966 // define iterator on matrix A and on constraints C
967 typedef typename gmm::linalg_traits<SVector2T>::const_iterator AIter;
968 typedef typename gmm::linalg_traits<SVector1T>::const_iterator CIter;
969
970 // store variable indices to be eliminated
971 std::vector<int> elim_varids;
972 elim_varids.reserve( _v_elim.size());
973
974 rhs_update_table_.clear();
975 std::vector<double> constraint_rhs_vec(_constraints.nrows());
976
977 for( unsigned int i=0; i < _v_elim.size(); ++i)
978 {
979 int cur_j = _v_elim[i];
980
981 if( cur_j != -1)
982 {
983 double cur_val = _constraints(i, cur_j);
984
985 // store index
986 elim_varids.push_back(cur_j);
987 rhs_update_table_.add_elim_id(cur_j);
988
989 // copy col
990 SVector2T col ( _A.col( cur_j));
991
992 // get a reference to current constraint vector
993 SVector1T& constraint( _constraints.row(i));
994
995 // add cur_j-th row multiplied with constraint[k] to each row k
996 // iterator of matrix column
997 AIter col_it, col_end;
998
999 // constraint rhs
1000 double constraint_rhs = constraint[ constraint.size()-1];
1001 constraint_rhs_vec[i] = -constraint_rhs;
1002
1003 //std::cerr << "constraint_rhs " << constraint_rhs << std::endl;
1004 // temporarliy set last element to zero (to avoid iterator finding it)
1005 constraint[ constraint.size()-1] = 0;
1006
1007 double cur_rhs = _rhs[cur_j];
1008
1009 // iterator of constraint
1010 CIter con_it = gmm::vect_const_begin( constraint);
1011 CIter con_end = gmm::vect_const_end( constraint);
1012
1013 // loop over all constraint entries over all column entries
1014 // should not hit last element (rhs) since set to zero
1015 for ( ; con_it != con_end; ++con_it )
1016 {
1017 col_it = gmm::vect_const_begin( col );
1018 col_end = gmm::vect_const_end( col );
1019 for ( ; col_it != col_end; ++col_it )
1020 _A( con_it.index(), col_it.index() ) -= ( *col_it )*(( *con_it )/cur_val);
1021
1022 //_rhs[con_it.index()] -= cur_rhs * (( *con_it )/cur_val);
1023 // rhs_update_table_.append(con_it.index(), -1.0 * (( *con_it )/cur_val), cur_j);
1024 rhs_update_table_.append(con_it.index(), -1.0 * (( *con_it )/cur_val), cur_j, false);
1025 //std::cerr << con_it.index() << " += " << -1.0*(( *con_it )/cur_val) << " * " << cur_rhs << " (["<<cur_j<<"] = "<<_rhs[cur_j]<<") " << std::endl;
1026 }
1027
1028 // TODO FIXME must use copy col (referens sometimes yields infinite loop below no col_it++?!?)
1029 SVector2T col_ref = _A.col(cur_j);
1030
1031 // add cur_j-th col multiplied with condition[k] to each col k
1032 col_it = gmm::vect_const_begin( col_ref );
1033 col_end = gmm::vect_const_end( col_ref );
1034
1035 // loop over all column entries and over all constraint entries
1036 for ( ; col_it != col_end; ++col_it )
1037 {
1038 con_it = gmm::vect_const_begin( constraint );
1039 con_end = gmm::vect_const_end( constraint );
1040
1041 for ( ; con_it != con_end; ++con_it )
1042 _A( col_it.index(), con_it.index() ) -= ( *col_it )*(( *con_it )/cur_val);
1043 //_rhs[col_it.index()] += constraint_rhs*( *col_it )/cur_val;
1044 // rhs_update_table_.append(col_it.index(), constraint_rhs*( *col_it )/cur_val);
1045 rhs_update_table_.append(col_it.index(), -( *col_it )/cur_val, i, true);
1046 //std::cerr << col_it.index() << " += " << constraint_rhs*( *col_it )/cur_val << std::endl;
1047 }
1048
1049 // reset last constraint entry to real value
1050 constraint[ constraint.size()-1] = constraint_rhs;
1051 }
1052 }
1053
1054 // cache current rhs's
1055 rhs_update_table_.cur_constraint_rhs_ = constraint_rhs_vec;
1056 rhs_update_table_.cur_rhs_ = _rhs;
1057 // apply transformation due to elimination
1058 rhs_update_table_.apply(constraint_rhs_vec, _rhs);
1059
1060 if( noisy_ > 2)
1061 std::cerr << __FUNCTION__ << " Constraints integrated " << sw.stop()/1000.0 << std::endl;
1062
1063 // eliminate vars
1064 _Acsc.init_with_good_format(_A);
1065 sw.start();
1066 std::vector< double > elim_varvals( elim_varids.size(), 0);
1067 COMISO_GMM::eliminate_csc_vars2( elim_varids, elim_varvals, _Acsc, _x, _rhs);
1068
1069 if( noisy_ > 2)
1070 std::cerr << __FUNCTION__ << " Constraints eliminated " << sw.stop()/1000.0 << std::endl;
1071 sw.start();
1072 // init _new_idx vector
1073 // _new_idx.resize( gmm::mat_ncols(_constraints));
1074 _new_idx.resize( gmm::mat_ncols(_A)+1);
1075 for( unsigned int i=0; i<_new_idx.size(); ++i)
1076 _new_idx[i] = i;
1077
1078 // update _new_idx w.r.t. eliminated cols
1079 COMISO_GMM::eliminate_vars_idx( elim_varids, _new_idx, -1);
1080
1081 // update _idx_to_round (in place)
1082 unsigned int wi = 0;
1083 for( unsigned int i=0; i<_idx_to_round.size(); ++i)
1084 {
1085 if(_new_idx[ _idx_to_round[i]] != -1)
1086 {
1087 _idx_to_round[wi] = _new_idx[_idx_to_round[i]];
1088 ++wi;
1089 }
1090 }
1091
1092 // resize, sort and make unique
1093 _idx_to_round.resize(wi);
1094
1095 std::sort(_idx_to_round.begin(), _idx_to_round.end());
1096 _idx_to_round.resize( std::unique(_idx_to_round.begin(), _idx_to_round.end()) -_idx_to_round.begin());
1097
1098 if( noisy_ > 2)
1099 std::cerr << __FUNCTION__ << "Indices reindexed " << sw.stop()/1000.0 << std::endl << std::endl;
1100 }
1101
1102
1103 //-----------------------------------------------------------------------------
1104
1105
1106 template<class RowT, class MatrixT>
1107 void
1108 ConstrainedSolver::
add_row(int _row_i,double _coeff,RowT _row,MatrixT & _mat)1109 add_row( int _row_i,
1110 double _coeff,
1111 RowT _row,
1112 MatrixT& _mat )
1113 {
1114 typedef typename gmm::linalg_traits<RowT>::const_iterator RIter;
1115 RIter r_it = gmm::vect_const_begin(_row);
1116 RIter r_end = gmm::vect_const_end(_row);
1117
1118 for(; r_it!=r_end; ++r_it)
1119 _mat(_row_i, r_it.index()) += _coeff*(*r_it);
1120 }
1121
1122
1123 //-----------------------------------------------------------------------------
1124
1125
1126 template<class RowT, class RMatrixT, class CMatrixT>
1127 void
1128 ConstrainedSolver::
add_row_simultaneously(int _row_i,double _coeff,RowT _row,RMatrixT & _rmat,CMatrixT & _cmat)1129 add_row_simultaneously( int _row_i,
1130 double _coeff,
1131 RowT _row,
1132 RMatrixT& _rmat,
1133 CMatrixT& _cmat )
1134 {
1135 typedef typename gmm::linalg_traits<RowT>::const_iterator RIter;
1136 RIter r_it = gmm::vect_const_begin(_row);
1137 RIter r_end = gmm::vect_const_end(_row);
1138
1139 for(; r_it!=r_end; ++r_it)
1140 {
1141 _rmat(_row_i, r_it.index()) += _coeff*(*r_it);
1142 _cmat(_row_i, r_it.index()) += _coeff*(*r_it);
1143 // if( _rmat(_row_i, r_it.index())*_rmat(_row_i, r_it.index()) < epsilon_squared_ )
1144 if( fabs(_rmat(_row_i, r_it.index())) < epsilon_ )
1145 {
1146 _rmat(_row_i, r_it.index()) = 0.0;
1147 _cmat(_row_i, r_it.index()) = 0.0;
1148 }
1149 }
1150 }
1151
1152
1153 //-----------------------------------------------------------------------------
1154
1155
1156 template<class CMatrixT, class VectorT, class VectorIT>
1157 double
1158 ConstrainedSolver::
setup_and_solve_system(CMatrixT & _B,VectorT & _x,VectorIT & _idx_to_round,double _reg_factor,bool _show_miso_settings)1159 setup_and_solve_system( CMatrixT& _B,
1160 VectorT& _x,
1161 VectorIT& _idx_to_round,
1162 double _reg_factor,
1163 bool _show_miso_settings)
1164 {
1165 // show options dialog
1166 if( _show_miso_settings)
1167 miso_.show_options_dialog();
1168
1169 COMISO::StopWatch s1;
1170 COMISO::StopWatch sw; sw.start();
1171 unsigned int m = gmm::mat_nrows(_B);
1172 unsigned int n = gmm::mat_ncols(_B);
1173
1174 s1.start();
1175 // set up B transposed
1176 CMatrixT Bt;
1177 gmm::resize( Bt, n, m);
1178 gmm::copy( gmm::transposed( _B), Bt);
1179 if( noisy_ > 1 )
1180 std::cerr << __FUNCTION__ << " Bt took " << s1.stop()/1000.0 << std::endl;
1181 s1.start();
1182
1183 // setup BtB
1184 CMatrixT BtB;
1185 gmm::resize( BtB, n, n);
1186 gmm::mult( Bt, _B, BtB);
1187 if( noisy_ > 1 )
1188 std::cerr << __FUNCTION__ << " BtB took " << s1.stop()/1000.0 << std::endl;
1189 s1.start();
1190
1191 // extract rhs
1192 std::vector< double > rhs( n);
1193 gmm::copy( gmm::scaled(gmm::mat_const_col( BtB, n - 1),-1.0), rhs);
1194 rhs.resize( n - 1);
1195
1196 if( noisy_ > 1)
1197 std::cerr << __FUNCTION__ << " rhs extract resize " << s1.stop()/1000.0 << std::endl;
1198 s1.start();
1199
1200 // resize BtB to only contain the actual system matrix (and not the rhs)
1201 gmm::resize( BtB, n - 1, n - 1);
1202
1203 if( noisy_ > 1)
1204 std::cerr << __FUNCTION__ << " BtB resize took " << s1.stop()/1000.0 << std::endl;
1205 s1.start();
1206 _x.resize( n - 1);
1207 if( noisy_ > 1)
1208 std::cerr << __FUNCTION__ << " x resize took " << s1.stop()/1000.0 << std::endl;
1209
1210 // regularize if necessary
1211 if(_reg_factor != 0.0)
1212 COMISO_GMM::regularize_hack(BtB, _reg_factor);
1213 s1.start();
1214
1215 // BtB -> CSC
1216 CSCMatrix BtBCSC;
1217 BtBCSC.init_with_good_format( BtB);
1218
1219 if( noisy_ > 1)
1220 std::cerr << __FUNCTION__ << " CSC init " << s1.stop()/1000.0 << std::endl;
1221 double setup_time = sw.stop()/1000.0;
1222
1223
1224 COMISO::StopWatch misw;
1225 misw.start();
1226 // miso solve
1227 miso_.solve( BtBCSC, _x, rhs, _idx_to_round);
1228 if( noisy_ > 1)
1229 std::cerr << __FUNCTION__ << " Miso Time " << misw.stop()/1000.0 << "s." << std::endl << std::endl;
1230 return setup_time;
1231 }
1232
1233
1234 //-----------------------------------------------------------------------------
1235
1236
1237 template<class RMatrixT, class VectorT >
1238 void
1239 ConstrainedSolver::
restore_eliminated_vars(RMatrixT & _constraints,VectorT & _x,std::vector<int> & _c_elim,std::vector<int> & _new_idx)1240 restore_eliminated_vars( RMatrixT& _constraints,
1241 VectorT& _x,
1242 std::vector<int>& _c_elim,
1243 std::vector<int>& _new_idx)
1244 {
1245 // restore original ordering of _x
1246 _x.resize(_new_idx.size());
1247 // last variable is the constant term 1.0
1248 _x.back() = 1.0;
1249
1250 // reverse iterate from prelast element
1251 for(int i=_new_idx.size()-2; i>= 0; --i)
1252 {
1253 if( _new_idx[i] != -1)
1254 {
1255 // error handling
1256 if( i < _new_idx[i] && noisy_ > 0) std::cerr << "Warning: UNSAFE Ordering!!!\n";
1257
1258 _x[i] = _x[_new_idx[i]];
1259 }
1260 }
1261
1262 // reverse iterate
1263 for(int i=_c_elim.size()-1; i>=0; --i)
1264 {
1265 int cur_var = _c_elim[i];
1266
1267 if( cur_var != -1)
1268 {
1269 // get variable value and set to zero
1270 double cur_val = _constraints(i, cur_var);
1271
1272 //_constraints(i, cur_var) = 0.0;
1273 //_x[cur_var] = -gmm::vect_sp(_x, _constraints.row(i))/cur_val;
1274 //reformulated to keep _constraints intact for further use:
1275 _x[cur_var] -= gmm::vect_sp(_x, _constraints.row(i))/cur_val;
1276
1277 }
1278 }
1279
1280 // resize
1281 _x.resize(_x.size()-1);
1282 }
1283
1284
1285 //-----------------------------------------------------------------------------
1286
1287
1288 template<class RMatrixT, class VectorT, class VectorIT >
1289 void
1290 ConstrainedSolver::
verify_mi_factored(const RMatrixT & _conditions,const RMatrixT & _B,const VectorT & _x,const VectorIT & _idx_to_round)1291 verify_mi_factored( const RMatrixT& _conditions,
1292 const RMatrixT& _B,
1293 const VectorT& _x,
1294 const VectorIT& _idx_to_round )
1295 {
1296 std::cerr << "######### Verify Constrained Solver Result ############\n";
1297
1298 // create extended x vector
1299 std::vector<double> x(_x);
1300 x.resize(x.size()+1);
1301 x.back() = 1.0;
1302
1303 // verify conditions
1304 std::vector<double> a(gmm::mat_nrows(_conditions));
1305
1306 gmm::mult(_conditions, x, a);
1307
1308 int conditions_not_ok = 0;
1309 for(unsigned int i=0; i<a.size(); ++i)
1310 if( a[i] > 1e-6)
1311 {
1312 ++conditions_not_ok;
1313 }
1314
1315 if( conditions_not_ok == 0)
1316 std::cerr << "all conditions are ok!\n";
1317 else
1318 std::cerr << conditions_not_ok
1319 << " conditions are not fullfilled: "
1320 << std::endl;
1321
1322 // verify rounding
1323 int roundings_not_ok = 0;
1324 for(unsigned int i=0; i<_idx_to_round.size(); ++i)
1325 {
1326 double d = _x[_idx_to_round[i]];
1327 if( fabs(d-round(d)) > 1e-6)
1328 ++roundings_not_ok;
1329 }
1330
1331 if( roundings_not_ok)
1332 std::cerr << roundings_not_ok << " Integer variables are not rounded\n";
1333 else
1334 std::cerr << "all Integer roundings are ok\n";
1335
1336 // evaluate energy
1337 VectorT Bx(x);
1338 gmm::mult(_B, x, Bx);
1339 std::cerr << "Total energy: " << gmm::vect_sp(Bx, Bx) << std::endl;
1340
1341 std::cerr << "######### FINISHED ############\n";
1342 }
1343
1344
1345
1346 //-----------------------------------------------------------------------------
1347
1348
1349 template<class RMatrixT, class CMatrixT, class VectorT>
1350 double
verify_constrained_system(const RMatrixT & _conditions,const CMatrixT & _A,const VectorT & _x,const VectorT & _rhs,double _eps)1351 ConstrainedSolver::verify_constrained_system(
1352 const RMatrixT& _conditions,
1353 const CMatrixT& _A,
1354 const VectorT& _x,
1355 const VectorT& _rhs,
1356 double _eps)
1357 {
1358 typedef typename gmm::linalg_traits<RMatrixT>::const_sub_row_type RowT;
1359 typedef typename gmm::linalg_traits<RowT>::const_iterator RIter;
1360
1361 VectorT Ax( _x.size());
1362 gmm::mult(_A, _x, Ax);
1363
1364 gmm::add(_rhs, gmm::scaled(Ax, -1.0), Ax);
1365 double norm = gmm::vect_norm2(Ax);
1366 //std::cerr << __FUNCTION__ << ": Error residual: " << norm << " vector : " << Ax << std::endl;
1367
1368 std::cerr << __FUNCTION__ << ": Checking constraints..." << std::endl;
1369
1370 unsigned int row_cond = gmm::mat_nrows( _conditions);
1371 unsigned int col_cond = gmm::mat_ncols( _conditions);
1372 bool all_conditions_ok = true;
1373 for( unsigned int r = 0; r < row_cond; ++r)
1374 {
1375 double cond_value = 0.0;
1376 RowT row = gmm::mat_const_row( _conditions, r);
1377 RIter row_it = gmm::vect_const_begin( row);
1378 RIter row_end = gmm::vect_const_end( row);
1379 //std::cerr << "\t checking row : " << row << std::endl;
1380
1381 for( ; row_it != row_end; ++row_it)
1382 {
1383 if( row_it.index() == col_cond -1)
1384 cond_value += (*row_it);
1385 else
1386 cond_value += _x[row_it.index()]*(*row_it);
1387 }
1388 //std::cerr << "\t Value is : " << cond_value << std::endl;
1389 //std::cerr << "--- --- --- --- ---\n";
1390 if( fabs(cond_value) > _eps)
1391 {
1392 std::cerr << "\t Error on row " << r << " with vector " << row << " and condition value " << cond_value << std::endl;
1393 all_conditions_ok = false;
1394 }
1395 }
1396 std::cerr << __FUNCTION__ << (all_conditions_ok? ": All conditions ok!" : ": Some conditions not ok!") << std::endl;
1397 return norm;
1398 }
1399
1400 //-----------------------------------------------------------------------------
1401
1402
1403 template<class RMatrixT, class CMatrixT, class VectorT, class VectorIT>
1404 double
1405 ConstrainedSolver::
verify_constrained_system_round(const RMatrixT & _conditions,const CMatrixT & _A,const VectorT & _x,const VectorT & _rhs,const VectorIT & _idx_to_round,double _eps)1406 verify_constrained_system_round(
1407 const RMatrixT& _conditions,
1408 const CMatrixT& _A,
1409 const VectorT& _x,
1410 const VectorT& _rhs,
1411 const VectorIT& _idx_to_round,
1412 double _eps)
1413 {
1414 // test integer roundings
1415 std::cerr << __FUNCTION__ << ": Testing integer roundings..." << std::endl;
1416 bool all_roundings_ok = true;
1417
1418 for( unsigned int i = 0; i < _idx_to_round.size(); ++i)
1419 if(fabs(ROUND(_x[_idx_to_round[i]])-_x[_idx_to_round[i]]) != 0.0)
1420 {
1421 std::cerr << "\t Warning: variable " << _idx_to_round[i] << " was not rounded!" << " Value is = " << _x[_idx_to_round[i]] << std::endl;
1422 all_roundings_ok = false;
1423 }
1424 std::cerr << __FUNCTION__ << (all_roundings_ok? ": All roundings ok!" : ": Some roundings not ok!") << std::endl;
1425
1426 // also test other stuff
1427 return verify_constrained_system(_conditions, _A, _x, _rhs, _eps);
1428 }
1429
1430 //-----------------------------------------------------------------------------
1431
1432
1433 template<class CMatrixT>
1434 void
eliminate_columns(CMatrixT & _M,const std::vector<int> & _columns)1435 ConstrainedSolver::eliminate_columns( CMatrixT& _M,
1436 const std::vector< int >& _columns)
1437 {
1438 // nothing to do?
1439 if( _columns.size() == 0) return;
1440
1441 // eliminate columns in place by first copying to the right place
1442 // and a subsequent resize
1443 std::vector< int > columns( _columns);
1444 std::sort( columns.begin(), columns.end());
1445
1446 std::vector< int >::iterator col_it = columns.begin();
1447 std::vector< int >::iterator col_end = columns.end();
1448
1449 int next_i = *col_it;
1450 for( int i = *col_it; i < (int)_M.ncols(); ++i)
1451 {
1452 if( col_it != col_end && i == *col_it)
1453 {
1454 ++col_it;
1455 }
1456 else
1457 {
1458 _M.col(next_i) = _M.col(i);
1459 ++next_i;
1460 }
1461 }
1462 gmm::resize( _M, _M.nrows(), _M.ncols() - columns.size());
1463 }
1464
1465
1466 //=============================================================================
1467 } // namespace COMISO
1468 //=============================================================================
1469