1 /******************************************************************************
2 * Author: Laurent Kneip *
3 * Contact: kneip.laurent@gmail.com *
4 * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
5 * *
6 * Redistribution and use in source and binary forms, with or without *
7 * modification, are permitted provided that the following conditions *
8 * are met: *
9 * * Redistributions of source code must retain the above copyright *
10 * notice, this list of conditions and the following disclaimer. *
11 * * Redistributions in binary form must reproduce the above copyright *
12 * notice, this list of conditions and the following disclaimer in the *
13 * documentation and/or other materials provided with the distribution. *
14 * * Neither the name of ANU nor the names of its contributors may be *
15 * used to endorse or promote products derived from this software without *
16 * specific prior written permission. *
17 * *
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
19 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
21 * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
22 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
23 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
24 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
25 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
26 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
27 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
28 * SUCH DAMAGE. *
29 ******************************************************************************/
30
31
32 #include <opengv/math/Sturm.hpp>
33 #include <iostream>
34
35
36
Bracket(double lowerBound,double upperBound)37 opengv::math::Bracket::Bracket( double lowerBound, double upperBound ) :
38 _lowerBound(lowerBound),
39 _upperBound(upperBound),
40 _lowerBoundChangesComputed(false),
41 _upperBoundChangesComputed(false),
42 _lowerBoundChanges(0),
43 _upperBoundChanges(0)
44 {}
45
Bracket(double lowerBound,double upperBound,size_t changes,bool setUpperBoundChanges)46 opengv::math::Bracket::Bracket(
47 double lowerBound, double upperBound, size_t changes, bool setUpperBoundChanges ) :
48 _lowerBound(lowerBound),
49 _upperBound(upperBound),
50 _lowerBoundChangesComputed(false),
51 _upperBoundChangesComputed(false),
52 _lowerBoundChanges(0),
53 _upperBoundChanges(0)
54 {
55 if( setUpperBoundChanges )
56 {
57 _upperBoundChanges = changes;
58 _upperBoundChangesComputed = true;
59 }
60 else
61 {
62 _lowerBoundChanges = changes;
63 _lowerBoundChangesComputed = true;
64 }
65 }
66
~Bracket()67 opengv::math::Bracket::~Bracket()
68 {}
69
70 bool
dividable(double eps) const71 opengv::math::Bracket::dividable( double eps ) const
72 {
73 if( numberRoots() == 1 && (_upperBound - _lowerBound ) < eps )
74 return false;
75 if( numberRoots() == 0 )
76 return false;
77 double center = (_upperBound + _lowerBound) / 2.0;
78 if( center == _upperBound || center == _lowerBound)
79 return false;
80 return true;
81 }
82
83 void
divide(std::list<Ptr> & brackets) const84 opengv::math::Bracket::divide( std::list<Ptr> & brackets ) const
85 {
86 double center = (_upperBound + _lowerBound) / 2.0;
87 Ptr lowerBracket(new Bracket(_lowerBound,center,_lowerBoundChanges,false));
88 Ptr upperBracket(new Bracket(center,_upperBound,_upperBoundChanges,true));
89 brackets.push_back(lowerBracket);
90 brackets.push_back(upperBracket);
91 }
92
93 double
lowerBound() const94 opengv::math::Bracket::lowerBound() const
95 {
96 return _lowerBound;
97 }
98
99 double
upperBound() const100 opengv::math::Bracket::upperBound() const
101 {
102 return _upperBound;
103 }
104
105 bool
lowerBoundChangesComputed() const106 opengv::math::Bracket::lowerBoundChangesComputed() const
107 {
108 return _lowerBoundChangesComputed;
109 }
110
111 bool
upperBoundChangesComputed() const112 opengv::math::Bracket::upperBoundChangesComputed() const
113 {
114 return _upperBoundChangesComputed;
115 }
116
117 void
setLowerBoundChanges(size_t changes)118 opengv::math::Bracket::setLowerBoundChanges( size_t changes )
119 {
120 _lowerBoundChanges = changes;
121 _lowerBoundChangesComputed = true;
122 }
123
124 void
setUpperBoundChanges(size_t changes)125 opengv::math::Bracket::setUpperBoundChanges( size_t changes )
126 {
127 _upperBoundChanges = changes;
128 _upperBoundChangesComputed = true;
129 }
130
131 size_t
numberRoots() const132 opengv::math::Bracket::numberRoots() const
133 {
134 if( !_lowerBoundChangesComputed || !_upperBoundChangesComputed )
135 {
136 std::cout << "Error: cannot evaluate number of roots" <<std::endl;
137 return 0;
138 }
139 return _lowerBoundChanges - _upperBoundChanges;
140 }
141
142
Sturm(const Eigen::MatrixXd & p)143 opengv::math::Sturm::Sturm( const Eigen::MatrixXd & p ) :
144 _C(Eigen::MatrixXd(p.cols(),p.cols()))
145 {
146 _dimension = (size_t) _C.cols();
147 _C = Eigen::MatrixXd(_dimension,_dimension);
148 _C.setZero();
149 _C.row(0) = p;
150
151 for( size_t i = 1; i < _dimension; i++ )
152 _C(1,i) = _C(0,i-1) * (_dimension-i);
153
154 for( size_t i = 2; i < _dimension; i++ )
155 {
156 Eigen::MatrixXd p1 = _C.block(i-2,i-2,1,_dimension-(i-2));
157 Eigen::MatrixXd p2 = _C.block(i-1,i-1,1,_dimension-(i-1));
158 Eigen::MatrixXd r;
159 computeNegatedRemainder(p1,p2,r);
160 _C.block(i,i,1,_dimension-i) = r.block(0,2,1,_dimension-i);
161 }
162 }
163
Sturm(const std::vector<double> & p)164 opengv::math::Sturm::Sturm( const std::vector<double> & p ) :
165 _C(Eigen::MatrixXd(p.size(),p.size()))
166 {
167 _dimension = (size_t) _C.cols();
168 _C = Eigen::MatrixXd(_dimension,_dimension);
169 _C.setZero();
170
171 for( size_t i = 0; i < _dimension; i++ )
172 _C(0,i) = p[i];
173
174 for( size_t i = 1; i < _dimension; i++ )
175 _C(1,i) = _C(0,i-1) * (_dimension-i);
176
177 for( size_t i = 2; i < _dimension; i++ )
178 {
179 Eigen::MatrixXd p1 = _C.block(i-2,i-2,1,_dimension-(i-2));
180 Eigen::MatrixXd p2 = _C.block(i-1,i-1,1,_dimension-(i-1));
181 Eigen::MatrixXd r;
182 computeNegatedRemainder(p1,p2,r);
183 _C.block(i,i,1,_dimension-i) = r.block(0,2,1,_dimension-i);
184 }
185 }
186
~Sturm()187 opengv::math::Sturm::~Sturm() {};
188
189 void
findRoots2(std::vector<double> & roots,double eps_x,double eps_val)190 opengv::math::Sturm::findRoots2( std::vector<double> & roots, double eps_x, double eps_val )
191 {
192 double absoluteBound = computeLagrangianBound();
193 std::list<Bracket::Ptr> stack;
194 stack.push_back(Bracket::Ptr(new Bracket(-absoluteBound,absoluteBound)));
195 stack.back()->setLowerBoundChanges( evaluateChain2(stack.back()->lowerBound()) );
196 stack.back()->setUpperBoundChanges( evaluateChain2(stack.back()->upperBound()) );
197 roots.reserve(stack.back()->numberRoots());
198
199 //some variables for pollishing
200 Eigen::MatrixXd monomials(_dimension,1);
201 monomials(_dimension-1,0) = 1.0;
202
203 while( !stack.empty() )
204 {
205 Bracket::Ptr bracket = stack.front();
206 stack.pop_front();
207
208 if( bracket->dividable(eps_x) )
209 {
210 bool divide = true;
211
212 if( bracket->numberRoots() == 1 )
213 {
214 //new part, we try immediately to do the pollishing here
215 bool converged = false;
216
217 double root = 0.5 * (bracket->lowerBound() + bracket->upperBound());
218 for(size_t i = 2; i <= _dimension; i++)
219 monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*root;
220 Eigen::MatrixXd matValue = _C.row(0) * monomials;
221
222 double value = matValue(0,0);
223
224 while( !converged )
225 {
226 Eigen::MatrixXd matDerivative = _C.row(1) * monomials;
227 double derivative = matDerivative(0,0);
228
229 double newRoot = root - (value/derivative);
230
231 if( newRoot < bracket->lowerBound() || newRoot > bracket->upperBound() )
232 break;
233
234 for(size_t i = 2; i <= _dimension; i++)
235 monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*newRoot;
236 matValue = _C.row(0) * monomials;
237
238 double newValue = matValue(0,0);
239
240 if( fabs(newValue) > fabs(value) )
241 break;
242
243 //do update
244 value = newValue;
245 root = newRoot;
246
247 //check if converged
248 if( fabs(value) < eps_val )
249 converged = true;
250 }
251
252 if( converged )
253 {
254 divide = false;
255 roots.push_back(root);
256 }
257 }
258
259 if(divide)
260 {
261 bracket->divide(stack);
262 std::list<Bracket::Ptr>::iterator it = stack.end();
263 it--;
264 size_t changes = evaluateChain2((*it)->lowerBound());
265 (*it)->setLowerBoundChanges(changes);
266 it--;
267 (*it)->setUpperBoundChanges(changes);
268 }
269 }
270 else
271 {
272 if( bracket->numberRoots() > 0 )
273 roots.push_back(0.5 * (bracket->lowerBound() + bracket->upperBound()));
274 }
275 }
276 }
277
278 std::vector<double>
findRoots()279 opengv::math::Sturm::findRoots()
280 {
281 //bracket the roots
282 std::vector<double> roots;
283 bracketRoots(roots);
284
285 //pollish
286 Eigen::MatrixXd monomials(_dimension,1);
287 monomials(_dimension-1,0) = 1.0;
288
289 for(size_t r = 0; r < roots.size(); r++ )
290 {
291 //Now do Gauss iterations here
292 //evaluate all monomials at the left bound
293 for(size_t k = 0; k < 5; k++ )
294 {
295 for(size_t i = 2; i <= _dimension; i++)
296 monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*roots[r];
297
298 Eigen::MatrixXd value = _C.row(0) * monomials;
299 Eigen::MatrixXd derivative = _C.row(1) * monomials;
300
301 //correction
302 roots[r] = roots[r] - (value(0,0)/derivative(0,0));
303 }
304 }
305
306 return roots;
307 }
308
309 void
bracketRoots(std::vector<double> & roots,double eps)310 opengv::math::Sturm::bracketRoots( std::vector<double> & roots, double eps )
311 {
312 double absoluteBound = computeLagrangianBound();
313 std::list<Bracket::Ptr> stack;
314 stack.push_back(Bracket::Ptr(new Bracket(-absoluteBound,absoluteBound)));
315 stack.back()->setLowerBoundChanges( evaluateChain2(stack.back()->lowerBound()) );
316 stack.back()->setUpperBoundChanges( evaluateChain2(stack.back()->upperBound()) );
317
318 double localEps = eps;
319 if( eps < 0.0 )
320 localEps = absoluteBound / (10.0 * stack.back()->numberRoots());
321 roots.reserve(stack.back()->numberRoots());
322
323 while( !stack.empty() )
324 {
325 Bracket::Ptr bracket = stack.front();
326 stack.pop_front();
327
328 if( bracket->dividable( localEps) )
329 {
330 bracket->divide(stack);
331 std::list<Bracket::Ptr>::iterator it = stack.end();
332 it--;
333 size_t changes = evaluateChain2((*it)->lowerBound());
334 (*it)->setLowerBoundChanges(changes);
335 it--;
336 (*it)->setUpperBoundChanges(changes);
337 }
338 else
339 {
340 if( bracket->numberRoots() > 0 )
341 roots.push_back(0.5 * (bracket->lowerBound() + bracket->upperBound()));
342 }
343 }
344 }
345
346 size_t
evaluateChain(double bound)347 opengv::math::Sturm::evaluateChain( double bound )
348 {
349 Eigen::MatrixXd monomials(_dimension,1);
350 monomials(_dimension-1,0) = 1.0;
351
352 //evaluate all monomials at the bound
353 for(size_t i = 2; i <= _dimension; i++)
354 monomials(_dimension-i,0) = monomials(_dimension-i+1,0)*bound;
355
356 Eigen::MatrixXd signs(_dimension,1);
357 for( size_t i = 0; i < _dimension; i++ )
358 signs.block(i,0,1,1) = _C.block(i,i,1,_dimension-i) * monomials.block(i,0,_dimension-i,1);
359
360 bool positive = false;
361 if( signs(0,0) > 0.0 )
362 positive = true;
363
364 int signChanges = 0;
365
366 for( size_t i = 1; i < _dimension; i++ )
367 {
368 if( positive )
369 {
370 if( signs(i,0) < 0.0 )
371 {
372 signChanges++;
373 positive = false;
374 }
375 }
376 else
377 {
378 if( signs(i,0) > 0.0 )
379 {
380 signChanges++;
381 positive = true;
382 }
383 }
384 }
385
386 return signChanges;
387 }
388
389 size_t
evaluateChain2(double bound)390 opengv::math::Sturm::evaluateChain2( double bound )
391 {
392 std::vector<double> monomials;
393 monomials.resize(_dimension);
394 monomials[_dimension-1] = 1.0;
395
396 //evaluate all monomials at the bound
397 for(size_t i = 2; i <= _dimension; i++)
398 monomials[_dimension-i] = monomials[_dimension-i+1]*bound;
399
400 std::vector<double> signs;
401 signs.reserve(_dimension);
402 for( size_t i = 0; i < _dimension; i++ )
403 {
404 signs.push_back(0.0);
405 for( size_t j = i; j < _dimension; j++ )
406 signs.back() += _C(i,j) * monomials[j];
407 }
408
409 bool positive = false;
410 if( signs[0] > 0.0 )
411 positive = true;
412
413 int signChanges = 0;
414
415 for( size_t i = 1; i < _dimension; i++ )
416 {
417 if( positive )
418 {
419 if( signs[i] < 0.0 )
420 {
421 signChanges++;
422 positive = false;
423 }
424 }
425 else
426 {
427 if( signs[i] > 0.0 )
428 {
429 signChanges++;
430 positive = true;
431 }
432 }
433 }
434
435 return signChanges;
436 }
437
438 void
computeNegatedRemainder(const Eigen::MatrixXd & p1,const Eigen::MatrixXd & p2,Eigen::MatrixXd & r)439 opengv::math::Sturm::computeNegatedRemainder(
440 const Eigen::MatrixXd & p1,
441 const Eigen::MatrixXd & p2,
442 Eigen::MatrixXd & r )
443 {
444 //we have to create 3 subtraction polynomials
445 Eigen::MatrixXd poly_1(1,p1.cols());
446 poly_1.block(0,0,1,p2.cols()) = (p1(0,0)/p2(0,0)) * p2;
447 poly_1(0,p1.cols()-1) = 0.0;
448
449 Eigen::MatrixXd poly_2(1,p1.cols());
450 poly_2.block(0,1,1,p2.cols()) = (p1(0,1)/p2(0,0)) * p2;
451 poly_2(0,0) = 0.0;
452
453 Eigen::MatrixXd poly_3(1,p1.cols());
454 poly_3.block(0,1,1,p2.cols()) = (-p2(0,1)*p1(0,0)/pow(p2(0,0),2)) * p2;
455 poly_3(0,0) = 0.0;
456
457 //compute remainder
458 r = -p1 + poly_1 + poly_2 + poly_3;
459 }
460
461 double
computeLagrangianBound()462 opengv::math::Sturm::computeLagrangianBound()
463 {
464 std::vector<double> coefficients;
465 coefficients.reserve(_dimension-1);
466
467 for(size_t i=0; i < _dimension-1; i++)
468 coefficients.push_back(pow(fabs(_C(0,i+1)/_C(0,0)),(1.0/(i+1))));
469
470 size_t j = 0;
471 double max1 = -1.0;
472 for( size_t i = 0; i < coefficients.size(); i++ )
473 {
474 if( coefficients[i] > max1 )
475 {
476 j = i;
477 max1 = coefficients[i];
478 }
479 }
480
481 coefficients[j] = -1.0;
482
483 double max2 = -1.0;
484 for( size_t i=0; i < coefficients.size(); i++ )
485 {
486 if( coefficients[i] > max2 )
487 max2 = coefficients[i];
488 }
489
490 double bound = max1 + max2;
491 return bound;
492 }
493