iCub-main
Loading...
Searching...
No Matches
calibReference.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2013 iCub Facility - Istituto Italiano di Tecnologia
3 * Author: Ugo Pattacini
4 * email: ugo.pattacini@iit.it
5 * Permission is granted to copy, distribute, and/or modify this program
6 * under the terms of the GNU General Public License, version 2 or any
7 * later version published by the Free Software Foundation.
8 *
9 * A copy of the license can be found at
10 * http://www.robotcub.org/icub/license/gpl.txt
11 *
12 * This program is distributed in the hope that it will be useful, but
13 * WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15 * Public License for more details
16*/
17
18#include <cmath>
19#include <algorithm>
20
21#include <yarp/math/Math.h>
22#include <iCub/ctrl/math.h>
23#include <iCub/optimization/calibReference.h>
24
25#include <IpTNLP.hpp>
26#include <IpIpoptApplication.hpp>
27
28using namespace std;
29using namespace yarp::os;
30using namespace yarp::sig;
31using namespace yarp::math;
32using namespace iCub::ctrl;
33using namespace iCub::optimization;
34
35
36namespace iCub
37{
38
39namespace optimization
40{
41
42/****************************************************************/
43inline Matrix computeH(const Vector &x)
44{
45 Matrix H=euler2dcm(x.subVector(3,5));
46 H(0,3)=x[0]; H(1,3)=x[1]; H(2,3)=x[2];
47
48 return H;
49}
50
51
52/****************************************************************/
53inline Matrix computeH(const Ipopt::Number *x)
54{
55 Vector _x(6);
56 for (size_t i=0; i<_x.length(); i++)
57 _x[i]=x[i];
58
59 return computeH(_x);
60}
61
62
63/****************************************************************/
64class CalibReferenceWithMatchedPointsNLP : public Ipopt::TNLP
65{
66protected:
67 const deque<Vector> &p0;
68 const deque<Vector> &p1;
69
70 Vector min;
71 Vector max;
72 Vector x0;
73 Vector x;
74
75public:
76 /****************************************************************/
77 CalibReferenceWithMatchedPointsNLP(const deque<Vector> &_p0,
78 const deque<Vector> &_p1,
79 const Vector &_min, const Vector &_max) :
80 p0(_p0), p1(_p1)
81 {
82 min=_min;
83 max=_max;
84 x0=0.5*(min+max);
85 }
86
87 /****************************************************************/
88 virtual void set_x0(const Vector &x0)
89 {
90 size_t len=std::min(this->x0.length(),x0.length());
91 for (size_t i=0; i<len; i++)
92 this->x0[i]=x0[i];
93 }
94
95 /****************************************************************/
96 virtual Vector get_result() const
97 {
98 return x;
99 }
100
101 /****************************************************************/
102 bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g,
103 Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
104 {
105 n=6;
106 m=nnz_jac_g=nnz_h_lag=0;
107 index_style=TNLP::C_STYLE;
108
109 return true;
110 }
111
112 /****************************************************************/
113 bool get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt::Number *x_u,
114 Ipopt::Index m, Ipopt::Number *g_l, Ipopt::Number *g_u)
115 {
116 for (Ipopt::Index i=0; i<n; i++)
117 {
118 x_l[i]=min[i];
119 x_u[i]=max[i];
120 }
121
122 return true;
123 }
124
125 /****************************************************************/
126 bool get_starting_point(Ipopt::Index n, bool init_x, Ipopt::Number *x,
127 bool init_z, Ipopt::Number *z_L, Ipopt::Number *z_U,
128 Ipopt::Index m, bool init_lambda, Ipopt::Number *lambda)
129 {
130 for (Ipopt::Index i=0; i<n; i++)
131 x[i]=x0[i];
132
133 return true;
134 }
135
136 /****************************************************************/
137 bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
138 Ipopt::Number &obj_value)
139 {
140 Matrix H=computeH(x);
141
142 obj_value=0.0;
143 if (p0.size()>0)
144 {
145 for (size_t i=0; i<p0.size(); i++)
146 obj_value+=norm2(p1[i]-H*p0[i]);
147
148 obj_value/=p0.size();
149 }
150
151 return true;
152 }
153
154 /****************************************************************/
155 bool eval_grad_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
156 Ipopt::Number *grad_f)
157 {
158 double ca=cos(x[3]); double sa=sin(x[3]);
159 double cb=cos(x[4]); double sb=sin(x[4]);
160 double cg=cos(x[5]); double sg=sin(x[5]);
161
162 Matrix Rza=eye(4,4);
163 Rza(0,0)=ca; Rza(1,1)=ca; Rza(1,0)=sa; Rza(0,1)=-sa;
164 Matrix dRza=zeros(4,4);
165 dRza(0,0)=-sa; dRza(1,1)=-sa; dRza(1,0)=ca; dRza(0,1)=-ca;
166
167 Matrix Rzg=eye(4,4);
168 Rzg(0,0)=cg; Rzg(1,1)=cg; Rzg(1,0)=sg; Rzg(0,1)=-sg;
169 Matrix dRzg=zeros(4,4);
170 dRzg(0,0)=-sg; dRzg(1,1)=-sg; dRzg(1,0)=cg; dRzg(0,1)=-cg;
171
172 Matrix Ryb=eye(4,4);
173 Ryb(0,0)=cb; Ryb(2,2)=cb; Ryb(2,0)=-sb; Ryb(0,2)=sb;
174 Matrix dRyb=zeros(4,4);
175 dRyb(0,0)=-sb; dRyb(2,2)=-sb; dRyb(2,0)=-cb; dRyb(0,2)=cb;
176
177 Matrix dHdx0=zeros(4,4); dHdx0(0,3)=1.0;
178 Matrix dHdx1=zeros(4,4); dHdx1(1,3)=1.0;
179 Matrix dHdx2=zeros(4,4); dHdx2(2,3)=1.0;
180 Matrix dHdx3=dRza*Ryb*Rzg;
181 Matrix dHdx4=Rza*dRyb*Rzg;
182 Matrix dHdx5=Rza*Ryb*dRzg;
183
184 Matrix H=computeH(x);
185 grad_f[0]=grad_f[1]=grad_f[2]=0.0;
186 grad_f[3]=grad_f[4]=grad_f[5]=0.0;
187 if (p0.size()>0)
188 {
189 for (size_t i=0; i<p0.size(); i++)
190 {
191 Vector d=p1[i]-H*p0[i];
192 grad_f[0]-=2.0*dot(d,(dHdx0*p0[i]));
193 grad_f[1]-=2.0*dot(d,(dHdx1*p0[i]));
194 grad_f[2]-=2.0*dot(d,(dHdx2*p0[i]));
195 grad_f[3]-=2.0*dot(d,(dHdx3*p0[i]));
196 grad_f[4]-=2.0*dot(d,(dHdx4*p0[i]));
197 grad_f[5]-=2.0*dot(d,(dHdx5*p0[i]));
198 }
199
200 for (Ipopt::Index i=0; i<n; i++)
201 grad_f[i]/=p0.size();
202 }
203
204 return true;
205 }
206
207 /****************************************************************/
208 bool eval_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
209 Ipopt::Index m, Ipopt::Number *g)
210 {
211 return true;
212 }
213
214 /****************************************************************/
215 bool eval_jac_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
216 Ipopt::Index m, Ipopt::Index nele_jac, Ipopt::Index *iRow,
217 Ipopt::Index *jCol, Ipopt::Number *values)
218 {
219 return true;
220 }
221
222 /****************************************************************/
223 bool eval_h(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
224 Ipopt::Number obj_factor, Ipopt::Index m, const Ipopt::Number *lambda,
225 bool new_lambda, Ipopt::Index nele_hess, Ipopt::Index *iRow,
226 Ipopt::Index *jCol, Ipopt::Number *values)
227 {
228 return true;
229 }
230
231 /****************************************************************/
232 void finalize_solution(Ipopt::SolverReturn status, Ipopt::Index n,
233 const Ipopt::Number *x, const Ipopt::Number *z_L,
234 const Ipopt::Number *z_U, Ipopt::Index m,
235 const Ipopt::Number *g, const Ipopt::Number *lambda,
236 Ipopt::Number obj_value, const Ipopt::IpoptData *ip_data,
237 Ipopt::IpoptCalculatedQuantities *ip_cq)
238 {
239 this->x.resize(n);
240 for (Ipopt::Index i=0; i<n; i++)
241 this->x[i]=x[i];
242 }
243};
244
245
246/****************************************************************/
247class CalibReferenceWithScaledMatchedPointsNLP : public CalibReferenceWithMatchedPointsNLP
248{
249public:
250 /****************************************************************/
251 CalibReferenceWithScaledMatchedPointsNLP(const deque<Vector> &_p0,
252 const deque<Vector> &_p1,
253 const Vector &_min, const Vector &_max) :
254 CalibReferenceWithMatchedPointsNLP(_p0,_p1,_min,_max) { }
255
256 /****************************************************************/
257 bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g,
258 Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
259 {
260 CalibReferenceWithMatchedPointsNLP::get_nlp_info(n,m,nnz_jac_g,nnz_h_lag,index_style);
261 n=6+3;
262
263 return true;
264 }
265
266 /****************************************************************/
267 bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
268 Ipopt::Number &obj_value)
269 {
270 Matrix H=computeH(x);
271 Vector s(4);
272 s[0]=x[6];
273 s[1]=x[7];
274 s[2]=x[8];
275 s[3]=1.0;
276
277 obj_value=0.0;
278 if (p0.size()>0)
279 {
280 for (size_t i=0; i<p0.size(); i++)
281 obj_value+=norm2(p1[i]-s*(H*p0[i]));
282
283 obj_value/=p0.size();
284 }
285
286 return true;
287 }
288
289 /****************************************************************/
290 bool eval_grad_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
291 Ipopt::Number *grad_f)
292 {
293 double ca=cos(x[3]); double sa=sin(x[3]);
294 double cb=cos(x[4]); double sb=sin(x[4]);
295 double cg=cos(x[5]); double sg=sin(x[5]);
296
297 Matrix Rza=eye(4,4);
298 Rza(0,0)=ca; Rza(1,1)=ca; Rza(1,0)=sa; Rza(0,1)=-sa;
299 Matrix dRza=zeros(4,4);
300 dRza(0,0)=-sa; dRza(1,1)=-sa; dRza(1,0)=ca; dRza(0,1)=-ca;
301
302 Matrix Rzg=eye(4,4);
303 Rzg(0,0)=cg; Rzg(1,1)=cg; Rzg(1,0)=sg; Rzg(0,1)=-sg;
304 Matrix dRzg=zeros(4,4);
305 dRzg(0,0)=-sg; dRzg(1,1)=-sg; dRzg(1,0)=cg; dRzg(0,1)=-cg;
306
307 Matrix Ryb=eye(4,4);
308 Ryb(0,0)=cb; Ryb(2,2)=cb; Ryb(2,0)=-sb; Ryb(0,2)=sb;
309 Matrix dRyb=zeros(4,4);
310 dRyb(0,0)=-sb; dRyb(2,2)=-sb; dRyb(2,0)=-cb; dRyb(0,2)=cb;
311
312 Matrix dHdx0=zeros(4,4); dHdx0(0,3)=1.0;
313 Matrix dHdx1=zeros(4,4); dHdx1(1,3)=1.0;
314 Matrix dHdx2=zeros(4,4); dHdx2(2,3)=1.0;
315 Matrix dHdx3=dRza*Ryb*Rzg;
316 Matrix dHdx4=Rza*dRyb*Rzg;
317 Matrix dHdx5=Rza*Ryb*dRzg;
318
319 Matrix H=computeH(x);
320 Matrix dHdx6=zeros(4,4); dHdx6(0,0)=1.0; dHdx6*=H;
321 Matrix dHdx7=zeros(4,4); dHdx7(1,1)=1.0; dHdx7*=H;
322 Matrix dHdx8=zeros(4,4); dHdx8(2,2)=1.0; dHdx8*=H;
323
324 Vector s(4);
325 s[0]=x[6];
326 s[1]=x[7];
327 s[2]=x[8];
328 s[3]=1.0;
329
330 grad_f[0]=grad_f[1]=grad_f[2]=0.0;
331 grad_f[3]=grad_f[4]=grad_f[5]=0.0;
332 grad_f[6]=grad_f[7]=grad_f[8]=0.0;
333 if (p0.size()>0)
334 {
335 for (size_t i=0; i<p0.size(); i++)
336 {
337 Vector d=p1[i]-s*(H*p0[i]);
338 grad_f[0]-=2.0*dot(d,(dHdx0*p0[i]));
339 grad_f[1]-=2.0*dot(d,(dHdx1*p0[i]));
340 grad_f[2]-=2.0*dot(d,(dHdx2*p0[i]));
341 grad_f[3]-=2.0*dot(d,(dHdx3*p0[i]));
342 grad_f[4]-=2.0*dot(d,(dHdx4*p0[i]));
343 grad_f[5]-=2.0*dot(d,(dHdx5*p0[i]));
344 grad_f[6]-=2.0*dot(d,(dHdx6*p0[i]));
345 grad_f[7]-=2.0*dot(d,(dHdx7*p0[i]));
346 grad_f[8]-=2.0*dot(d,(dHdx8*p0[i]));
347 }
348
349 for (Ipopt::Index i=0; i<n; i++)
350 grad_f[i]/=p0.size();
351 }
352
353 return true;
354 }
355};
356
357
358/****************************************************************/
359class CalibReferenceWithScalarScaledMatchedPointsNLP : public CalibReferenceWithMatchedPointsNLP
360{
361public:
362 /****************************************************************/
363 CalibReferenceWithScalarScaledMatchedPointsNLP(const deque<Vector> &_p0,
364 const deque<Vector> &_p1,
365 const Vector &_min, const Vector &_max) :
366 CalibReferenceWithMatchedPointsNLP(_p0,_p1,_min,_max) { }
367
368 /****************************************************************/
369 bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g,
370 Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
371 {
372 CalibReferenceWithMatchedPointsNLP::get_nlp_info(n,m,nnz_jac_g,nnz_h_lag,index_style);
373 n=6+1;
374
375 return true;
376 }
377
378 /****************************************************************/
379 bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
380 Ipopt::Number &obj_value)
381 {
382 Matrix H=computeH(x);
383 Vector s(4);
384 s[0]=s[1]=s[2]=x[6];
385 s[3]=1.0;
386
387 obj_value=0.0;
388 if (p0.size()>0)
389 {
390 for (size_t i=0; i<p0.size(); i++)
391 obj_value+=norm2(p1[i]-s*(H*p0[i]));
392
393 obj_value/=p0.size();
394 }
395
396 return true;
397 }
398
399 /****************************************************************/
400 bool eval_grad_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
401 Ipopt::Number *grad_f)
402 {
403 double ca=cos(x[3]); double sa=sin(x[3]);
404 double cb=cos(x[4]); double sb=sin(x[4]);
405 double cg=cos(x[5]); double sg=sin(x[5]);
406
407 Matrix Rza=eye(4,4);
408 Rza(0,0)=ca; Rza(1,1)=ca; Rza(1,0)=sa; Rza(0,1)=-sa;
409 Matrix dRza=zeros(4,4);
410 dRza(0,0)=-sa; dRza(1,1)=-sa; dRza(1,0)=ca; dRza(0,1)=-ca;
411
412 Matrix Rzg=eye(4,4);
413 Rzg(0,0)=cg; Rzg(1,1)=cg; Rzg(1,0)=sg; Rzg(0,1)=-sg;
414 Matrix dRzg=zeros(4,4);
415 dRzg(0,0)=-sg; dRzg(1,1)=-sg; dRzg(1,0)=cg; dRzg(0,1)=-cg;
416
417 Matrix Ryb=eye(4,4);
418 Ryb(0,0)=cb; Ryb(2,2)=cb; Ryb(2,0)=-sb; Ryb(0,2)=sb;
419 Matrix dRyb=zeros(4,4);
420 dRyb(0,0)=-sb; dRyb(2,2)=-sb; dRyb(2,0)=-cb; dRyb(0,2)=cb;
421
422 Matrix dHdx0=zeros(4,4); dHdx0(0,3)=1.0;
423 Matrix dHdx1=zeros(4,4); dHdx1(1,3)=1.0;
424 Matrix dHdx2=zeros(4,4); dHdx2(2,3)=1.0;
425 Matrix dHdx3=dRza*Ryb*Rzg;
426 Matrix dHdx4=Rza*dRyb*Rzg;
427 Matrix dHdx5=Rza*Ryb*dRzg;
428
429 Matrix H=computeH(x);
430 Matrix dHdx6=zeros(4,4); dHdx6(0,0)=1.0; dHdx6*=H;
431
432 Vector s(4);
433 s[0]=s[1]=s[2]=x[6];
434 s[3]=1.0;
435
436 grad_f[0]=grad_f[1]=grad_f[2]=0.0;
437 grad_f[3]=grad_f[4]=grad_f[5]=0.0;
438 grad_f[6]=0.0;
439 if (p0.size()>0)
440 {
441 for (size_t i=0; i<p0.size(); i++)
442 {
443 Vector d=p1[i]-s*(H*p0[i]);
444 grad_f[0]-=2.0*dot(d,(dHdx0*p0[i]));
445 grad_f[1]-=2.0*dot(d,(dHdx1*p0[i]));
446 grad_f[2]-=2.0*dot(d,(dHdx2*p0[i]));
447 grad_f[3]-=2.0*dot(d,(dHdx3*p0[i]));
448 grad_f[4]-=2.0*dot(d,(dHdx4*p0[i]));
449 grad_f[5]-=2.0*dot(d,(dHdx5*p0[i]));
450 grad_f[6]-=2.0*dot(d,(dHdx6*p0[i]));
451 }
452
453 for (Ipopt::Index i=0; i<n; i++)
454 grad_f[i]/=p0.size();
455 }
456
457 return true;
458 }
459};
460
461}
462
463}
464
465
466/****************************************************************/
467CalibReferenceWithMatchedPoints::CalibReferenceWithMatchedPoints()
468{
469 max_iter=300;
470 tol=1e-8;
471
472 min.resize(6); max.resize(6);
473 min[0]=-1.0; max[0]=1.0;
474 min[1]=-1.0; max[1]=1.0;
475 min[2]=-1.0; max[2]=1.0;
476 min[3]=-M_PI; max[3]=M_PI;
477 min[4]=-M_PI; max[4]=M_PI;
478 min[5]=-M_PI; max[5]=M_PI;
479
480 min_s.resize(3); max_s.resize(3);
481 min_s[0]=0.1; max_s[0]=10.0;
482 min_s[1]=0.1; max_s[1]=10.0;
483 min_s[2]=0.1; max_s[2]=10.0;
484
485 min_s_scalar=0.1; max_s_scalar=10.0;
486
487 x0=0.5*(min+max);
488 s0.resize(3,1.0);
489 s0_scalar=1.0;
490}
491
492
493/****************************************************************/
494void CalibReferenceWithMatchedPoints::setBounds(const Matrix &min,
495 const Matrix &max)
496{
497 if ((min.rows()<3) || (min.cols()<3) ||
498 (max.rows()<3) || (max.cols()<3))
499 return;
500
501 this->min[0]=min(0,3); this->max[0]=max(0,3);
502 this->min[1]=min(1,3); this->max[1]=max(1,3);
503 this->min[2]=min(2,3); this->max[2]=max(2,3);
504
505 this->min[3]=min(0,0); this->max[3]=max(0,0);
506 this->min[4]=min(0,1); this->max[4]=max(0,1);
507 this->min[5]=min(0,2); this->max[5]=max(0,2);
508}
509
510
511/****************************************************************/
512void CalibReferenceWithMatchedPoints::setBounds(const Vector &min,
513 const Vector &max)
514{
515 size_t len_min=std::min(this->min.length(),min.length());
516 size_t len_max=std::min(this->max.length(),max.length());
517
518 for (size_t i=0; i<len_min; i++)
519 this->min[i]=min[i];
520
521 for (size_t i=0; i<len_max; i++)
522 this->max[i]=max[i];
523}
524
525
526/****************************************************************/
527void CalibReferenceWithMatchedPoints::setScalingBounds(const Vector &min,
528 const Vector &max)
529{
530 size_t len_min=std::min(this->min_s.length(),min.length());
531 size_t len_max=std::min(this->max_s.length(),max.length());
532
533 for (size_t i=0; i<len_min; i++)
534 this->min_s[i]=min[i];
535
536 for (size_t i=0; i<len_max; i++)
537 this->max_s[i]=max[i];
538}
539
540
541/****************************************************************/
542void CalibReferenceWithMatchedPoints::setScalingBounds(const double min,
543 const double max)
544{
545 min_s_scalar=min;
546 max_s_scalar=max;
547}
548
549
550/****************************************************************/
551double CalibReferenceWithMatchedPoints::evalError(const Matrix &H)
552{
553 double error=0.0;
554 if (p0.size()>0)
555 {
556 for (size_t i=0; i<p0.size(); i++)
557 error+=norm(p1[i]-H*p0[i]);
558
559 error/=p0.size();
560 }
561
562 return error;
563}
564
565
566/****************************************************************/
567bool CalibReferenceWithMatchedPoints::addPoints(const Vector &p0,
568 const Vector &p1)
569{
570 if ((p0.length()>=3) && (p1.length()>=3))
571 {
572 Vector _p0=p0.subVector(0,2); _p0.push_back(1.0);
573 Vector _p1=p1.subVector(0,2); _p1.push_back(1.0);
574
575 this->p0.push_back(_p0);
576 this->p1.push_back(_p1);
577
578 return true;
579 }
580 else
581 return false;
582}
583
584
585/****************************************************************/
586void CalibReferenceWithMatchedPoints::getPoints(deque<Vector> &p0,
587 deque<Vector> &p1) const
588{
589 p0=this->p0;
590 p1=this->p1;
591}
592
593
594/****************************************************************/
595void CalibReferenceWithMatchedPoints::clearPoints()
596{
597 p0.clear();
598 p1.clear();
599}
600
601
602/****************************************************************/
603bool CalibReferenceWithMatchedPoints::setInitialGuess(const Matrix &H)
604{
605 if ((H.rows()>=4) && (H.cols()>=4))
606 {
607 Vector euler=dcm2euler(H);
608 x0[0]=H(0,3); x0[1]=H(1,3); x0[2]=H(2,3);
609 x0[3]=euler[0]; x0[4]=euler[1]; x0[5]=euler[2];
610
611 return true;
612 }
613 else
614 return false;
615}
616
617
618/****************************************************************/
619bool CalibReferenceWithMatchedPoints::setScalingInitialGuess(const Vector &s)
620{
621 if (s.length()>=s0.length())
622 {
623 s0=s.subVector(0,(unsigned int)s0.length()-1);
624 return true;
625 }
626 else
627 return false;
628}
629
630
631/****************************************************************/
632bool CalibReferenceWithMatchedPoints::setScalingInitialGuess(const double s)
633{
634 s0_scalar=s;
635 return true;
636}
637
638
639/****************************************************************/
640bool CalibReferenceWithMatchedPoints::setCalibrationOptions(const Property &options)
641{
642 if (options.check("max_iter"))
643 max_iter=options.find("max_iter").asInt32();
644
645 if (options.check("tol"))
646 tol=options.find("tol").asFloat64();
647
648 return true;
649}
650
651
652/****************************************************************/
653bool CalibReferenceWithMatchedPoints::calibrate(Matrix &H, double &error)
654{
655 if (p0.size()>0)
656 {
657 Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
658 app->Options()->SetNumericValue("tol",tol);
659 app->Options()->SetIntegerValue("acceptable_iter",0);
660 app->Options()->SetStringValue("mu_strategy","adaptive");
661 app->Options()->SetIntegerValue("max_iter",max_iter);
662 app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
663 app->Options()->SetStringValue("hessian_approximation","limited-memory");
664 app->Options()->SetIntegerValue("print_level",0);
665 app->Options()->SetStringValue("derivative_test","none");
666 app->Initialize();
667
668 Ipopt::SmartPtr<CalibReferenceWithMatchedPointsNLP> nlp=new CalibReferenceWithMatchedPointsNLP(p0,p1,min,max);
669
670 nlp->set_x0(x0);
671 Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
672
673 Vector x=nlp->get_result();
674 H=computeH(x);
675 error=evalError(H);
676
677 return (status==Ipopt::Solve_Succeeded);
678 }
679 else
680 return false;
681}
682
683
684/****************************************************************/
685bool CalibReferenceWithMatchedPoints::calibrate(Matrix &H, Vector &s,
686 double &error)
687{
688 if (p0.size()>0)
689 {
690 Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
691 app->Options()->SetNumericValue("tol",tol);
692 app->Options()->SetIntegerValue("acceptable_iter",0);
693 app->Options()->SetStringValue("mu_strategy","adaptive");
694 app->Options()->SetIntegerValue("max_iter",max_iter);
695 app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
696 app->Options()->SetStringValue("hessian_approximation","limited-memory");
697 app->Options()->SetIntegerValue("print_level",0);
698 app->Options()->SetStringValue("derivative_test","none");
699 app->Initialize();
700
701 Ipopt::SmartPtr<CalibReferenceWithScaledMatchedPointsNLP> nlp=new CalibReferenceWithScaledMatchedPointsNLP(p0,p1,cat(min,min_s),cat(max,max_s));
702
703 nlp->set_x0(cat(x0,s0));
704 Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
705
706 Vector x=nlp->get_result();
707 H=computeH(x);
708 s=x.subVector(6,8);
709 Matrix S=eye(4,4);
710 S(0,0)=s[0]; S(1,1)=s[1]; S(2,2)=s[2];
711 error=evalError(S*H);
712
713 return (status==Ipopt::Solve_Succeeded);
714 }
715 else
716 return false;
717}
718
719
720/****************************************************************/
721bool CalibReferenceWithMatchedPoints::calibrate(Matrix &H, double &s,
722 double &error)
723{
724 if (p0.size()>0)
725 {
726 Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
727 app->Options()->SetNumericValue("tol",tol);
728 app->Options()->SetIntegerValue("acceptable_iter",0);
729 app->Options()->SetStringValue("mu_strategy","adaptive");
730 app->Options()->SetIntegerValue("max_iter",max_iter);
731 app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
732 app->Options()->SetStringValue("hessian_approximation","limited-memory");
733 app->Options()->SetIntegerValue("print_level",0);
734 app->Options()->SetStringValue("derivative_test","none");
735 app->Initialize();
736
737 Ipopt::SmartPtr<CalibReferenceWithScalarScaledMatchedPointsNLP> nlp=new CalibReferenceWithScalarScaledMatchedPointsNLP(p0,p1,cat(min,min_s_scalar),cat(max,max_s_scalar));
738
739 nlp->set_x0(cat(x0,s0_scalar));
740 Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
741
742 Vector x=nlp->get_result();
743 H=computeH(x);
744 s=x[6];
745 Matrix S=eye(4,4);
746 S(0,0)=S(1,1)=S(2,2)=s;
747 error=evalError(S*H);
748
749 return (status==Ipopt::Solve_Succeeded);
750 }
751 else
752 return false;
753}
754
755
M_PI
#define M_PI
Definition XSensMTx.cpp:24
iCub::optimization::CalibReferenceWithMatchedPointsNLP::get_starting_point
bool get_starting_point(Ipopt::Index n, bool init_x, Ipopt::Number *x, bool init_z, Ipopt::Number *z_L, Ipopt::Number *z_U, Ipopt::Index m, bool init_lambda, Ipopt::Number *lambda)
Definition calibReference.cpp:126
iCub::optimization::CalibReferenceWithMatchedPointsNLP::get_nlp_info
bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g, Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
Definition calibReference.cpp:102
iCub::optimization::CalibReferenceWithMatchedPointsNLP::CalibReferenceWithMatchedPointsNLP
CalibReferenceWithMatchedPointsNLP(const deque< Vector > &_p0, const deque< Vector > &_p1, const Vector &_min, const Vector &_max)
Definition calibReference.cpp:77
iCub::optimization::CalibReferenceWithMatchedPointsNLP::eval_g
bool eval_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Index m, Ipopt::Number *g)
Definition calibReference.cpp:208
iCub::optimization::CalibReferenceWithMatchedPointsNLP::eval_h
bool eval_h(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number obj_factor, Ipopt::Index m, const Ipopt::Number *lambda, bool new_lambda, Ipopt::Index nele_hess, Ipopt::Index *iRow, Ipopt::Index *jCol, Ipopt::Number *values)
Definition calibReference.cpp:223
iCub::optimization::CalibReferenceWithMatchedPointsNLP::finalize_solution
void finalize_solution(Ipopt::SolverReturn status, Ipopt::Index n, const Ipopt::Number *x, const Ipopt::Number *z_L, const Ipopt::Number *z_U, Ipopt::Index m, const Ipopt::Number *g, const Ipopt::Number *lambda, Ipopt::Number obj_value, const Ipopt::IpoptData *ip_data, Ipopt::IpoptCalculatedQuantities *ip_cq)
Definition calibReference.cpp:232
iCub::optimization::CalibReferenceWithMatchedPointsNLP::eval_grad_f
bool eval_grad_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number *grad_f)
Definition calibReference.cpp:155
iCub::optimization::CalibReferenceWithMatchedPointsNLP::eval_jac_g
bool eval_jac_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Index m, Ipopt::Index nele_jac, Ipopt::Index *iRow, Ipopt::Index *jCol, Ipopt::Number *values)
Definition calibReference.cpp:215
iCub::optimization::CalibReferenceWithMatchedPointsNLP::eval_f
bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number &obj_value)
Definition calibReference.cpp:137
iCub::optimization::CalibReferenceWithMatchedPointsNLP::get_bounds_info
bool get_bounds_info(Ipopt::Index n, Ipopt::Number *x_l, Ipopt::Number *x_u, Ipopt::Index m, Ipopt::Number *g_l, Ipopt::Number *g_u)
Definition calibReference.cpp:113
iCub::optimization::CalibReferenceWithMatchedPoints::getPoints
virtual void getPoints(std::deque< yarp::sig::Vector > &p0, std::deque< yarp::sig::Vector > &p1) const
Retrieve copies of the database of 3D-points pairs.
Definition calibReference.cpp:586
iCub::optimization::CalibReferenceWithMatchedPoints::calibrate
virtual bool calibrate(yarp::sig::Matrix &H, double &error)
Perform reference calibration to determine the matrix H.
iCub::optimization::CalibReferenceWithMatchedPoints::setCalibrationOptions
virtual bool setCalibrationOptions(const yarp::os::Property &options)
Allow setting further options used during calibration.
Definition calibReference.cpp:640
iCub::optimization::CalibReferenceWithMatchedPoints::clearPoints
virtual void clearPoints()
Clear the internal database of 3D points.
Definition calibReference.cpp:595
iCub::optimization::CalibReferenceWithMatchedPoints::setInitialGuess
virtual bool setInitialGuess(const yarp::sig::Matrix &H)
Allow specifiying the initial guess for the roto-translation matrix we seek for.
Definition calibReference.cpp:603
iCub::optimization::CalibReferenceWithMatchedPoints::setScalingBounds
virtual void setScalingBounds(const yarp::sig::Vector &min, const yarp::sig::Vector &max)
Allow specifying the bounds for the 3D scaling factors.
iCub::optimization::CalibReferenceWithMatchedPoints::setScalingInitialGuess
virtual bool setScalingInitialGuess(const yarp::sig::Vector &s)
Allow specifiying the initial guess for the scaling factors.
iCub::optimization::CalibReferenceWithMatchedPoints::setBounds
virtual void setBounds(const yarp::sig::Matrix &min, const yarp::sig::Matrix &max)
Allow specifying the minimum and maximum bounds of the elements belonging to the transformation.
iCub::optimization::CalibReferenceWithMatchedPoints::addPoints
virtual bool addPoints(const yarp::sig::Vector &p0, const yarp::sig::Vector &p1)
Add to the internal database the 3D-point p0 and the 3D-point p1 which is supposed to correspond to H...
Definition calibReference.cpp:567
iCub::optimization::CalibReferenceWithScalarScaledMatchedPointsNLP::eval_grad_f
bool eval_grad_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number *grad_f)
Definition calibReference.cpp:400
iCub::optimization::CalibReferenceWithScalarScaledMatchedPointsNLP::CalibReferenceWithScalarScaledMatchedPointsNLP
CalibReferenceWithScalarScaledMatchedPointsNLP(const deque< Vector > &_p0, const deque< Vector > &_p1, const Vector &_min, const Vector &_max)
Definition calibReference.cpp:363
iCub::optimization::CalibReferenceWithScalarScaledMatchedPointsNLP::get_nlp_info
bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g, Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
Definition calibReference.cpp:369
iCub::optimization::CalibReferenceWithScalarScaledMatchedPointsNLP::eval_f
bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number &obj_value)
Definition calibReference.cpp:379
iCub::optimization::CalibReferenceWithScaledMatchedPointsNLP::get_nlp_info
bool get_nlp_info(Ipopt::Index &n, Ipopt::Index &m, Ipopt::Index &nnz_jac_g, Ipopt::Index &nnz_h_lag, IndexStyleEnum &index_style)
Definition calibReference.cpp:257
iCub::optimization::CalibReferenceWithScaledMatchedPointsNLP::CalibReferenceWithScaledMatchedPointsNLP
CalibReferenceWithScaledMatchedPointsNLP(const deque< Vector > &_p0, const deque< Vector > &_p1, const Vector &_min, const Vector &_max)
Definition calibReference.cpp:251
iCub::optimization::CalibReferenceWithScaledMatchedPointsNLP::eval_grad_f
bool eval_grad_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number *grad_f)
Definition calibReference.cpp:290
iCub::optimization::CalibReferenceWithScaledMatchedPointsNLP::eval_f
bool eval_f(Ipopt::Index n, const Ipopt::Number *x, bool new_x, Ipopt::Number &obj_value)
Definition calibReference.cpp:267
e
e
Definition compute_ekf_fast.m:13
H
H
Definition compute_ekf_fast.m:27
zeros
zeros(2, 2) eye(2
x
x
Definition compute_ekf_sym.m:21
n
int n
Definition debugFunctions.cpp:58
error
bool error
Definition debugFunctions.cpp:57
iCub::ctrl::dot
double dot(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
Returns the dot product between two vectors given in the form: matrix(:,col).
iCub::ctrl::norm2
double norm2(const yarp::sig::Matrix &M, int col)
Returns the squared norm of the vector given in the form: matrix(:,col).
Definition math.h:91
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
iCub::optimization::computeH
Matrix computeH(const Vector &x)
Definition calibReference.cpp:43
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition emotionInterface.h:17
Generated on Wed Dec 18 2024 16:34:47 for iCub-main by doxygen 1.9.8