iCub-main
methods.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 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 
20 #include <yarp/math/Math.h>
21 #include <yarp/math/SVD.h>
22 
23 #include <iCub/ctrl/math.h>
28 
29 #include "methods.h"
30 
31 using namespace std;
32 using namespace yarp::os;
33 using namespace yarp::sig;
34 using namespace yarp::math;
35 using namespace iCub::ctrl;
36 using namespace iCub::optimization;
37 using namespace iCub::learningmachine;
38 
39 
40 /************************************************************************/
42 {
43  int m=(int)spatialCompetence.A.rows();
44  int n=(int)spatialCompetence.A.cols();
45  Matrix U(m,n),V(n,n); Vector S(n);
46  SVD(spatialCompetence.A,U,S,V);
47 
48  spatialCompetence.radii.resize(3);
49  spatialCompetence.radii[0]=(S[0]!=0.0)?(1.0/sqrt(fabs(S[0]))):0.0;
50  spatialCompetence.radii[1]=(S[1]!=0.0)?(1.0/sqrt(fabs(S[1]))):0.0;
51  spatialCompetence.radii[2]=(S[2]!=0.0)?(1.0/sqrt(fabs(S[2]))):0.0;
52 
53  // check if we have a right-hand basis
54  Vector x=V.getCol(0);
55  Vector y=V.getCol(1);
56  Vector z=V.getCol(2);
57  if (dot(cross(x,y),z)<0.0)
58  V.setCol(2,-1.0*z);
59 
60  Matrix R(4,4); R.zero(); R(3,3)=1.0;
61  R.setSubmatrix(V,0,0);
62  spatialCompetence.R=SE3inv(R);
63 
64  return true;
65 }
66 
67 
68 /************************************************************************/
69 bool Calibrator::computeSpatialCompetence(const deque<Vector> &points)
70 {
71  if (minVolumeEllipsoid(points,0.001,spatialCompetence.A,spatialCompetence.c))
72  if (computeSpatialTransformation())
73  return true;
74 
75  return false;
76 }
77 
78 
79 /************************************************************************/
81 {
82  type=src.type;
83  in=src.in;
84  out=src.out;
85  spatialCompetence=src.spatialCompetence;
86 }
87 
88 
89 /************************************************************************/
90 double Calibrator::getSpatialCompetence(const Vector &point)
91 {
92  if (point.length()<3)
93  return 0.0;
94 
95  Vector x=(point.subVector(0,2)-spatialCompetence.c)/spatialCompetence.scale;
96  if (dot(x,spatialCompetence.A*x)<=1.0)
97  return 1.0;
98  else if (spatialCompetence.extrapolation)
99  {
100  // getting cartesian coordinates wrt (A,c) frame
101  x.push_back(1.0);
102  x=spatialCompetence.R*x;
103  x.pop_back();
104 
105  // distance approximated as ||x-xp||,
106  // where xp is the projection of x over
107  // the ellipsoid along the line connecting
108  // x with the origin
109 
110  // switch to spherical coordinates
111  double cos_theta=x[2]/norm(x);
112  double theta=acos(cos_theta);
113  double phi=atan2(x[1],x[0]);
114 
115  Vector xp(3);
116  double cos_phi=cos(phi);
117  xp[0]=spatialCompetence.radii[0]*cos_theta*cos_phi;
118  xp[1]=spatialCompetence.radii[1]*sin(theta)*cos_phi;
119  xp[2]=spatialCompetence.radii[2]*sin(phi);
120 
121  double d=norm(x-xp);
122  return exp(-40.0*d*d); // competence(0.2 [m])=0.2
123  }
124  else
125  return 0.0;
126 }
127 
128 
129 /************************************************************************/
130 bool Calibrator::toProperty(Property &info) const
131 {
132  Bottle data;
133  Bottle &values=data.addList();
134  values.addString(spatialCompetence.extrapolation?"true":"false");
135  values.addFloat64(spatialCompetence.scale);
136  values.addFloat64(spatialCompetence.c[0]);
137  values.addFloat64(spatialCompetence.c[1]);
138  values.addFloat64(spatialCompetence.c[2]);
139  for (int r=0; r<spatialCompetence.A.rows(); r++)
140  for (int c=0; c<spatialCompetence.A.cols(); c++)
141  values.addFloat64(spatialCompetence.A(r,c));
142 
143  info.put("spatial_competence",data.get(0));
144  return true;
145 }
146 
147 
148 /************************************************************************/
149 bool Calibrator::fromProperty(const Property &info)
150 {
151  if (!info.check("spatial_competence"))
152  return false;
153 
154  spatialCompetence.A=eye(3,3);
155  spatialCompetence.c=Vector(3,0.0);
156  spatialCompetence.scale=1.0;
157  spatialCompetence.extrapolation=true;
158  if (Bottle *values=info.find("spatial_competence").asList())
159  {
160  spatialCompetence.extrapolation=(values->get(0).asString()=="true");
161  spatialCompetence.scale=values->get(1).asFloat64();
162  spatialCompetence.c[0]=values->get(2).asFloat64();
163  spatialCompetence.c[1]=values->get(3).asFloat64();
164  spatialCompetence.c[2]=values->get(4).asFloat64();
165  int r=0; int c=0;
166  for (int i=5; i<values->size(); i++)
167  {
168  spatialCompetence.A(r,c)=values->get(i).asFloat64();
169  if (++c>=spatialCompetence.A.cols())
170  {
171  c=0;
172  if (++r>=spatialCompetence.A.rows())
173  break;
174  }
175  }
176  }
177 
178  computeSpatialTransformation();
179  return true;
180 }
181 
182 
183 /************************************************************************/
185 {
186  H=eye(4,4);
187  scale=1.0;
188  if (type=="affine")
189  {
190  impl=new AffinityWithMatchedPoints;
191  this->type="affine";
192  }
193  else
194  {
196  this->type=type;
197  if ((this->type!="se3") && (this->type!="se3+scale"))
198  this->type="se3";
199  }
200 }
201 
202 
203 /************************************************************************/
205 {
206  copySuperClassData(calibrator);
207  H=calibrator.H;
208  scale=calibrator.scale;
209  if (type=="affine")
210  impl=new AffinityWithMatchedPoints(*dynamic_cast<AffinityWithMatchedPoints*>(calibrator.impl));
211  else
212  impl=new CalibReferenceWithMatchedPoints(*dynamic_cast<CalibReferenceWithMatchedPoints*>(calibrator.impl));
213 }
214 
215 
216 /************************************************************************/
218 {
219  delete impl;
220 }
221 
222 
223 /************************************************************************/
224 bool MatrixCalibrator::addPoints(const Vector &in, const Vector &out)
225 {
226  if ((in.length()>=3) && (out.length()>=3))
227  {
228  Vector _in=in.subVector(0,2);
229  Vector _out=out.subVector(0,2);
230 
231  this->in.push_back(_in);
232  this->out.push_back(_out);
233  return impl->addPoints(_in,_out);
234  }
235  else
236  return false;
237 }
238 
239 
240 /************************************************************************/
242 {
243  impl->clearPoints();
244  in.clear();
245  out.clear();
246  return true;
247 }
248 
249 
250 /************************************************************************/
251 bool MatrixCalibrator::getPoints(deque<Vector> &in, deque<Vector> &out) const
252 {
253  in=this->in;
254  out=this->out;
255  return true;
256 }
257 
258 
259 /************************************************************************/
261 {
262  (type=="se3+scale")?dynamic_cast<CalibReferenceWithMatchedPoints*>(impl)->calibrate(H,scale,error):
263  impl->calibrate(H,error);
264 
265  if (computeSpatialCompetence(in))
266  {
267  spatialCompetence.scale=1.2;
268  return true;
269  }
270  else
271  return false;
272 }
273 
274 
275 /************************************************************************/
276 bool MatrixCalibrator::retrieve(const Vector &in, Vector &out)
277 {
278  if (in.length()>=3)
279  {
280  Vector _in=in.subVector(0,2);
281  _in.push_back(1.0);
282  out=H*_in;
283  out.pop_back();
284  out*=scale;
285  return true;
286  }
287  else
288  return false;
289 }
290 
291 
292 /************************************************************************/
293 bool MatrixCalibrator::toProperty(Property &info) const
294 {
295  Bottle data;
296  Bottle &values=data.addList();
297  values.addFloat64(scale);
298  for (int r=0; r<H.rows(); r++)
299  for (int c=0; c<H.cols(); c++)
300  values.addFloat64(H(r,c));
301 
302  info.clear();
303  info.put("type",type);
304  info.put("calibration_data",data.get(0));
306 }
307 
308 
309 /************************************************************************/
311 {
312  if (!info.check("type"))
313  return false;
314 
315  string type=info.find("type").asString();
316  if ((type=="se3") || (type=="se3+scale"))
317  {
318  delete impl;
320  }
321  else if (type=="affine")
322  {
323  delete impl;
324  impl=new AffinityWithMatchedPoints;
325  }
326  else
327  return false;
328 
329  H=eye(4,4);
330  scale=1.0;
331  if (Bottle *values=info.find("calibration_data").asList())
332  {
333  scale=values->get(0).asFloat64();
334 
335  int r=0; int c=0;
336  for (int i=1; i<values->size(); i++)
337  {
338  H(r,c)=values->get(i).asFloat64();
339  if (++c>=H.cols())
340  {
341  c=0;
342  if (++r>=H.rows())
343  break;
344  }
345  }
346  }
347 
349 }
350 
351 
352 /************************************************************************/
354 {
355  impl=new LSSVMLearner;
356  LSSVMLearner *lssvm=dynamic_cast<LSSVMLearner*>(impl);
357  lssvm->setDomainSize(3);
358  lssvm->setCoDomainSize(3);
359  lssvm->setC(100.0);
360  lssvm->getKernel()->setGamma(10.0);
361 
362  this->type="lssvm";
363 }
364 
365 
366 /************************************************************************/
368 {
369  copySuperClassData(calibrator);
370  impl=new LSSVMLearner(*dynamic_cast<LSSVMLearner*>(calibrator.impl));
371 }
372 
373 
374 /************************************************************************/
375 bool LSSVMCalibrator::addPoints(const Vector &in, const Vector &out)
376 {
377  if ((in.length()>=3) && (out.length()>=3))
378  {
379  Vector _in=in.subVector(0,2);
380  Vector _out=out.subVector(0,2);
381  impl->feedSample(in,out);
382 
383  this->in.push_back(_in);
384  this->out.push_back(_out);
385  return true;
386  }
387  else
388  return false;
389 }
390 
391 
392 /************************************************************************/
394 {
395  impl->reset();
396  in.clear();
397  out.clear();
398  return true;
399 }
400 
401 
402 /************************************************************************/
403 bool LSSVMCalibrator::getPoints(deque<Vector> &in, deque<Vector> &out) const
404 {
405  in=this->in;
406  out=this->out;
407  return true;
408 }
409 
410 
411 /************************************************************************/
413 {
414  if (getNumPoints()==0)
415  return false;
416 
417  impl->train();
418 
419  error=0.0;
420  for (size_t i=0; i<getNumPoints(); i++)
421  {
422  Vector p;
423  retrieve(in[i],p);
424  error+=norm(out[i].subVector(0,2)-p);
425  }
426  error/=getNumPoints();
427 
428  if (computeSpatialCompetence(in))
429  {
430  spatialCompetence.scale=1.0;
431  return true;
432  }
433  else
434  return false;
435 }
436 
437 
438 /************************************************************************/
439 bool LSSVMCalibrator::retrieve(const Vector &in, Vector &out)
440 {
441  if (in.length()>=3)
442  {
443  Prediction prediction=impl->predict(in.subVector(0,2));
444  out=prediction.getPrediction();
445  return true;
446  }
447  else
448  return false;
449 }
450 
451 
452 /************************************************************************/
453 bool LSSVMCalibrator::toProperty(Property &info) const
454 {
455  Bottle data;
456  Bottle &values=data.addList();
457  values.addString(impl->toString());
458 
459  info.clear();
460  info.put("type",type);
461  info.put("calibration_data",data.get(0));
463 }
464 
465 
466 /************************************************************************/
467 bool LSSVMCalibrator::fromProperty(const Property &info)
468 {
469  if (!info.check("type"))
470  return false;
471 
472  string type=info.find("type").asString();
473  if (type=="lssvm")
474  clearPoints();
475  else
476  return false;
477 
478  bool ret=false;
479  if (Bottle *values=info.find("calibration_data").asList())
480  if (values->size()>=1)
481  ret=impl->fromString(values->toString());
482 
484  return ret;
485 }
486 
487 
488 /************************************************************************/
490 {
491  delete impl;
492  in.clear();
493  out.clear();
494 }
495 
496 
497 /************************************************************************/
499 {
500  if (i<models.size())
501  return models[i];
502  else
503  return NULL;
504 }
505 
506 
507 /************************************************************************/
509 {
510  Calibrator *calibrator;
511  if (c.getType()=="lssvm")
512  calibrator=new LSSVMCalibrator(dynamic_cast<LSSVMCalibrator&>(c));
513  else
514  calibrator=new MatrixCalibrator(dynamic_cast<MatrixCalibrator&>(c));
515 
516  models.push_back(calibrator);
517  return *this;
518 }
519 
520 
521 /************************************************************************/
522 bool LocallyWeightedExperts::retrieve(const Vector &in, Vector &out)
523 {
524  if (in.length()<3)
525  return false;
526 
527  Vector _in=in.subVector(0,2);
528  Vector outExperts(_in.length(),0.0);
529  Vector outExtrapolators(_in.length(),0.0);
530  double sumExperts=0.0;
531  double sumExtrapolators=0.0;
532 
533  // collect information over available models
534  for (size_t i=0; i<models.size(); i++)
535  {
536  double competence=models[i]->getSpatialCompetence(_in);
537  if (competence>0.0)
538  {
539  Vector pred;
540  models[i]->retrieve(_in,pred);
541 
542  outExtrapolators+=competence*pred;
543  sumExtrapolators+=competence;
544  if (competence>=1.0)
545  {
546  outExperts+=competence*pred;
547  sumExperts+=competence;
548  }
549  }
550  }
551 
552  // consider experts first
553  if (sumExperts!=0.0)
554  {
555  out=outExperts/sumExperts;
556  return true;
557  }
558  // then extrapolators
559  else if (sumExtrapolators!=0.0)
560  {
561  out=outExtrapolators/sumExtrapolators;
562  return true;
563  }
564  // no models found
565  else
566  return false;
567 }
568 
569 
570 /************************************************************************/
572 {
573  for (size_t i=0; i<models.size(); i++)
574  delete models[i];
575 
576  models.clear();
577 }
578 
579 
580 /************************************************************************/
582 {
583  clear();
584 }
585 
586 
@ data
std::deque< yarp::sig::Vector > out
Definition: methods.h:36
virtual bool toProperty(yarp::os::Property &info) const
Definition: methods.cpp:130
virtual bool computeSpatialCompetence(const std::deque< yarp::sig::Vector > &points)
Definition: methods.cpp:69
virtual bool fromProperty(const yarp::os::Property &info)
Definition: methods.cpp:149
std::string type
Definition: methods.h:35
void copySuperClassData(const Calibrator &src)
Definition: methods.cpp:80
virtual bool computeSpatialTransformation()
Definition: methods.cpp:41
std::deque< yarp::sig::Vector > in
Definition: methods.h:36
struct Calibrator::SpatialCompetence spatialCompetence
virtual std::string getType() const
Definition: methods.h:60
virtual double getSpatialCompetence(const yarp::sig::Vector &point)
Definition: methods.cpp:90
virtual ~LSSVMCalibrator()
Definition: methods.cpp:489
virtual bool addPoints(const yarp::sig::Vector &in, const yarp::sig::Vector &out)
Definition: methods.cpp:375
virtual bool getPoints(std::deque< yarp::sig::Vector > &in, std::deque< yarp::sig::Vector > &out) const
Definition: methods.cpp:403
LSSVMCalibrator(const std::string &type="lssvm")
Definition: methods.cpp:353
virtual bool clearPoints()
Definition: methods.cpp:393
iCub::learningmachine::IMachineLearner * impl
Definition: methods.h:102
virtual bool toProperty(yarp::os::Property &info) const
Definition: methods.cpp:453
virtual bool calibrate(double &error)
Definition: methods.cpp:412
virtual bool fromProperty(const yarp::os::Property &info)
Definition: methods.cpp:467
virtual bool retrieve(const yarp::sig::Vector &in, yarp::sig::Vector &out)
Definition: methods.cpp:439
virtual LocallyWeightedExperts & operator<<(Calibrator &c)
Definition: methods.cpp:508
virtual Calibrator * operator[](const size_t i)
Definition: methods.cpp:498
virtual bool retrieve(const yarp::sig::Vector &in, yarp::sig::Vector &out)
Definition: methods.cpp:522
virtual void clear()
Definition: methods.cpp:571
virtual ~LocallyWeightedExperts()
Definition: methods.cpp:581
virtual bool clearPoints()
Definition: methods.cpp:241
virtual bool fromProperty(const yarp::os::Property &info)
Definition: methods.cpp:310
virtual bool calibrate(double &error)
Definition: methods.cpp:260
MatrixCalibrator(const std::string &type="se3")
Definition: methods.cpp:184
virtual bool retrieve(const yarp::sig::Vector &in, yarp::sig::Vector &out)
Definition: methods.cpp:276
double scale
Definition: methods.h:82
yarp::sig::Matrix H
Definition: methods.h:81
virtual bool addPoints(const yarp::sig::Vector &in, const yarp::sig::Vector &out)
Definition: methods.cpp:224
virtual bool toProperty(yarp::os::Property &info) const
Definition: methods.cpp:293
virtual bool getPoints(std::deque< yarp::sig::Vector > &in, std::deque< yarp::sig::Vector > &out) const
Definition: methods.cpp:251
iCub::optimization::MatrixTransformationWithMatchedPoints * impl
Definition: methods.h:80
virtual ~MatrixCalibrator()
Definition: methods.cpp:217
This is basic implementation of the LSSVM algorithms.
Definition: LSSVMLearner.h:121
void setCoDomainSize(unsigned int size)
Mutator for the codomain size.
void setDomainSize(unsigned int size)
Mutator for the domain size.
virtual void setC(double C)
Mutator for the regularization parameter C.
Definition: LSSVMLearner.h:249
virtual RBFKernel * getKernel()
Accessor for the kernel.
Definition: LSSVMLearner.h:267
A class that represents a prediction result.
Definition: Prediction.h:44
yarp::sig::Vector getPrediction()
Accessor for the expected value of the prediction.
Definition: Prediction.h:106
virtual void setGamma(double g)
Definition: LSSVMLearner.h:71
A class that deals with the problem of determining the affine transformation matrix A between two set...
Definition: affinity.h:51
A class that deals with the problem of determining the roto-translation matrix H and scaling factors ...
virtual bool calibrate(yarp::sig::Matrix &H, double &error)
Perform reference calibration to determine the matrix H.
exp(-x3 *T)]
int n
bool minVolumeEllipsoid(const std::deque< yarp::sig::Vector > &points, const double tol, yarp::sig::Matrix &A, yarp::sig::Vector &c)
Find the minimum volume ellipsoide (MVEE) of a set of N d-dimensional data points.
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).
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
yarp::sig::Vector cross(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
Returns the cross product between two vectors given in the form: matrix(:,col).
out
Definition: sine.m:8