iCub-main
localizer.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini, Alessandro Roncone
4  * email: ugo.pattacini@iit.it, alessandro.roncone@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 #include <cmath>
20 #include <algorithm>
21 
22 #include <yarp/math/SVD.h>
23 #include <iCub/localizer.h>
24 #include <iCub/solver.h>
25 
26 
27 /************************************************************************/
28 Localizer::Localizer(ExchangeData *_commData, const unsigned int _period) :
29  PeriodicThread((double)_period/1000.0), commData(_commData),
30  period(_period)
31 {
32  iCubHeadCenter eyeC("right_"+commData->headVersion2String());
33  eyeL=new iCubEye("left_"+commData->headVersion2String());
34  eyeR=new iCubEye("right_"+commData->headVersion2String());
35 
36  // remove constraints on the links
37  // we use the chains for logging purpose
38  eyeL->setAllConstraints(false);
39  eyeR->setAllConstraints(false);
40 
41  // release links
42  eyeL->releaseLink(0); eyeC.releaseLink(0); eyeR->releaseLink(0);
43  eyeL->releaseLink(1); eyeC.releaseLink(1); eyeR->releaseLink(1);
44  eyeL->releaseLink(2); eyeC.releaseLink(2); eyeR->releaseLink(2);
45 
46  // add aligning matrices read from configuration file
47  getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
48  getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
49 
50  // overwrite aligning matrices iff specified through tweak values
52  {
53  getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
54  getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
55  }
56 
57  // get the absolute reference frame of the head
58  Vector q(eyeC.getDOF(),0.0);
59  eyeCAbsFrame=eyeC.getH(q);
60  // ... and its inverse
62 
63  // get the length of the half of the eyes baseline
64  eyesHalfBaseline=0.5*norm(eyeL->EndEffPose().subVector(0,2)-eyeR->EndEffPose().subVector(0,2));
65 
66  bool ret;
67 
68  // get camera projection matrix
69  ret=getCamParams(commData->rf_cameras,"CAMERA_CALIBRATION_LEFT",&PrjL,widthL,heightL,true);
71  {
72  Matrix *Prj;
73  if (getCamParams(commData->rf_tweak,"CAMERA_CALIBRATION_LEFT",&Prj,widthL,heightL,true))
74  {
75  delete PrjL;
76  PrjL=Prj;
77  }
78  }
79 
80  if (ret)
81  {
82  cxl=(*PrjL)(0,2);
83  cyl=(*PrjL)(1,2);
84  invPrjL=new Matrix(pinv(PrjL->transposed()).transposed());
85  }
86  else
87  PrjL=invPrjL=nullptr;
88 
89  // get camera projection matrix
90  ret=getCamParams(commData->rf_cameras,"CAMERA_CALIBRATION_RIGHT",&PrjR,widthR,heightR,true);
92  {
93  Matrix *Prj;
94  if (getCamParams(commData->rf_tweak,"CAMERA_CALIBRATION_RIGHT",&Prj,widthR,heightR,true))
95  {
96  delete PrjR;
97  PrjR=Prj;
98  }
99  }
100 
101  if (ret)
102  {
103  cxr=(*PrjR)(0,2);
104  cyr=(*PrjR)(1,2);
105  invPrjR=new Matrix(pinv(PrjR->transposed()).transposed());
106  }
107  else
108  PrjR=invPrjR=nullptr;
109 
110  Vector Kp(1,0.001), Ki(1,0.001), Kd(1,0.0);
111  Vector Wp(1,1.0), Wi(1,1.0), Wd(1,1.0);
112  Vector N(1,10.0), Tt(1,1.0);
113  Matrix satLim(1,2);
114 
115  satLim(0,0)=0.05;
116  satLim(0,1)=10.0;
117 
118  pid=new parallelPID(0.05,Kp,Ki,Kd,Wp,Wi,Wd,N,Tt,satLim);
119 
120  Vector z0(1,0.5);
121  pid->reset(z0);
122  dominantEye="left";
123 }
124 
125 
126 /************************************************************************/
128 {
129  delete eyeL;
130  delete eyeR;
131  delete pid;
132 
133  if (PrjL!=nullptr)
134  {
135  delete PrjL;
136  delete invPrjL;
137  }
138 
139  if (PrjR!=nullptr)
140  {
141  delete PrjR;
142  delete invPrjR;
143  }
144 }
145 
146 
147 /************************************************************************/
149 {
150  port_mono.open(commData->localStemName+"/mono:i");
151  port_stereo.open(commData->localStemName+"/stereo:i");
152  port_anglesIn.open(commData->localStemName+"/angles:i");
153  port_anglesOut.open(commData->localStemName+"/angles:o");
154 
155  yInfo("Starting Localizer at %d ms",period);
156  return true;
157 }
158 
159 /************************************************************************/
161 {
162  port_mono.interrupt();
163  port_stereo.interrupt();
164  port_anglesIn.interrupt();
165  port_anglesOut.interrupt();
166 
167  port_mono.close();
168  port_stereo.close();
169  port_anglesIn.close();
170  port_anglesOut.close();
171 }
172 
173 
174 /************************************************************************/
176 {
177  if (s)
178  yInfo("Localizer started successfully");
179  else
180  yError("Localizer did not start!");
181 }
182 
183 
184 /************************************************************************/
185 void Localizer::getPidOptions(Bottle &options)
186 {
187  lock_guard<mutex> lck(mtx);
188  pid->getOptions(options);
189  Bottle &bDominantEye=options.addList();
190  bDominantEye.addString("dominantEye");
191  bDominantEye.addString(dominantEye);
192 }
193 
194 
195 /************************************************************************/
196 void Localizer::setPidOptions(const Bottle &options)
197 {
198  lock_guard<mutex> lck(mtx);
199  pid->setOptions(options);
200  if (options.check("dominantEye"))
201  {
202  string domEye=options.find("dominantEye").asString();
203  if ((domEye=="left") || (domEye=="right"))
204  dominantEye=domEye;
205  }
206 }
207 
208 
209 /************************************************************************/
210 Vector Localizer::getAbsAngles(const Vector &x)
211 {
212  Vector fp=x;
213  fp.push_back(1.0); // impose homogeneous coordinates
214 
215  // get fp wrt head-centered system
216  Vector fph=invEyeCAbsFrame*fp;
217  fph.pop_back();
218 
219  Vector ang(3);
220  ang[0]=atan2(fph[0],fph[2]);
221  ang[1]=-atan2(fph[1],fabs(fph[2]));
222  ang[2]=2.0*atan2(eyesHalfBaseline,norm(fph));
223 
224  return ang;
225 }
226 
227 
228 /************************************************************************/
229 Vector Localizer::get3DPoint(const string &type, const Vector &ang)
230 {
231  lock_guard<mutex> lck(mtx);
232  double azi=ang[0];
233  double ele=ang[1];
234  double ver=ang[2];
235 
236  Vector q(8,0.0);
237  if (type=="rel")
238  {
239  Vector torso=commData->get_torso();
240  Vector head=commData->get_q();
241 
242  q[0]=torso[0];
243  q[1]=torso[1];
244  q[2]=torso[2];
245  q[3]=head[0];
246  q[4]=head[1];
247  q[5]=head[2];
248  q[6]=head[3];
249  q[7]=head[4];
250 
251  ver+=head[5];
252  }
253 
254  // impose vergence != 0.0
256 
257  q[7]+=ver/2.0;
258  eyeL->setAng(q);
259 
260  q[7]-=ver;
261  eyeR->setAng(q);
262 
263  // compute new fp due to changed vergence
264  Vector fp;
265  CartesianHelper::computeFixationPointData(*(eyeL->asChain()),*(eyeR->asChain()),fp);
266  fp.push_back(1.0); // impose homogeneous coordinates
267 
268  // compute rotational matrix to
269  // account for elevation and azimuth
270  Vector x(4), y(4);
271  x[0]=1.0; y[0]=0.0;
272  x[1]=0.0; y[1]=1.0;
273  x[2]=0.0; y[2]=0.0;
274  x[3]=ele; y[3]=azi;
275  Matrix R=axis2dcm(y)*axis2dcm(x);
276 
277  Vector fph, xd;
278  if (type=="rel")
279  {
280  Matrix frame=commData->get_fpFrame();
281  fph=SE3inv(frame)*fp; // get fp wrt relative head-centered frame
282  xd=frame*(R*fph); // apply rotation and retrieve fp wrt root frame
283  }
284  else
285  {
286  fph=invEyeCAbsFrame*fp; // get fp wrt absolute head-centered frame
287  xd=eyeCAbsFrame*(R*fph); // apply rotation and retrieve fp wrt root frame
288  }
289 
290  xd.pop_back();
291  return xd;
292 }
293 
294 
295 /************************************************************************/
296 bool Localizer::projectPoint(const string &type, const Vector &x, Vector &px)
297 {
298  lock_guard<mutex> lck(mtx);
299  if (x.length()<3)
300  {
301  yError("Not enough values given for the point!");
302  return false;
303  }
304 
305  bool isLeft=(type=="left");
306 
307  Matrix *Prj=(isLeft?PrjL:PrjR);
308  iCubEye *eye=(isLeft?eyeL:eyeR);
309 
310  if (Prj!=nullptr)
311  {
312  Vector torso=commData->get_torso();
313  Vector head=commData->get_q();
314 
315  Vector q(8);
316  q[0]=torso[0];
317  q[1]=torso[1];
318  q[2]=torso[2];
319  q[3]=head[0];
320  q[4]=head[1];
321  q[5]=head[2];
322  q[6]=head[3];
323  q[7]=head[4]+head[5]/(isLeft?2.0:-2.0);
324 
325  Vector xo=x;
326  // impose homogeneous coordinates
327  if (xo.length()<4)
328  xo.push_back(1.0);
329  else
330  {
331  xo=xo.subVector(0,3);
332  xo[3]=1.0;
333  }
334 
335  // find position wrt the camera frame
336  Vector xe=SE3inv(eye->getH(q))*xo;
337 
338  // find the 2D projection
339  px=*Prj*xe;
340  px=px/px[2];
341  px.pop_back();
342  return true;
343  }
344  else
345  {
346  yError("Unspecified projection matrix for %s camera!",type.c_str());
347  return false;
348  }
349 }
350 
351 
352 /************************************************************************/
353 bool Localizer::projectPoint(const string &type, const double u, const double v,
354  const double z, Vector &x)
355 {
356  lock_guard<mutex> lck(mtx);
357  bool isLeft=(type=="left");
358 
359  Matrix *invPrj=(isLeft?invPrjL:invPrjR);
360  iCubEye *eye=(isLeft?eyeL:eyeR);
361 
362  if (invPrj!=nullptr)
363  {
364  Vector torso=commData->get_torso();
365  Vector head=commData->get_q();
366 
367  Vector q(8);
368  q[0]=torso[0];
369  q[1]=torso[1];
370  q[2]=torso[2];
371  q[3]=head[0];
372  q[4]=head[1];
373  q[5]=head[2];
374  q[6]=head[3];
375  q[7]=head[4]+head[5]/(isLeft?2.0:-2.0);
376 
377  Vector p(3);
378  p[0]=z*u;
379  p[1]=z*v;
380  p[2]=z;
381 
382  // find the 3D position from the 2D projection,
383  // knowing the coordinate z in the camera frame
384  Vector xe=*invPrj*p;
385  xe[3]=1.0; // impose homogeneous coordinates
386 
387  // find position wrt the root frame
388  x=eye->getH(q)*xe;
389  x.pop_back();
390  return true;
391  }
392  else
393  {
394  yError("Unspecified projection matrix for %s camera!",type.c_str());
395  return false;
396  }
397 }
398 
399 
400 /************************************************************************/
401 bool Localizer::projectPoint(const string &type, const double u, const double v,
402  const Vector &plane, Vector &x)
403 {
404  if (plane.length()<4)
405  {
406  yError("Not enough values given for the projection plane!");
407  return false;
408  }
409 
410  bool isLeft=(type=="left");
411  iCubEye *eye=(isLeft?eyeL:eyeR);
412 
413  if (projectPoint(type,u,v,1.0,x))
414  {
415  lock_guard<mutex> lck(mtx);
416 
417  // pick up a point belonging to the plane
418  Vector p0(3,0.0);
419  if (plane[0]!=0.0)
420  p0[0]=-plane[3]/plane[0];
421  else if (plane[1]!=0.0)
422  p0[1]=-plane[3]/plane[1];
423  else if (plane[2]!=0.0)
424  p0[2]=-plane[3]/plane[2];
425  else
426  {
427  yError("Error while specifying projection plane!");
428  return false;
429  }
430 
431  // take a vector orthogonal to the plane
432  Vector n(3);
433  n[0]=plane[0];
434  n[1]=plane[1];
435  n[2]=plane[2];
436 
437  // compute the projection
438  Vector e=eye->EndEffPose().subVector(0,2);
439  Vector v=x-e;
440  x=e+(dot(p0-e,n)/dot(v,n))*v;
441 
442  return true;
443  }
444  else
445  return false;
446 }
447 
448 
449 /************************************************************************/
450 bool Localizer::triangulatePoint(const Vector &pxl, const Vector &pxr, Vector &x)
451 {
452  lock_guard<mutex> lck(mtx);
453  if ((pxl.length()<2) || (pxr.length()<2))
454  {
455  yError("Not enough values given for the pixels!");
456  return false;
457  }
458 
459  if (PrjL && PrjR)
460  {
461  Vector torso=commData->get_torso();
462  Vector head=commData->get_q();
463 
464  Vector qL(8);
465  qL[0]=torso[0];
466  qL[1]=torso[1];
467  qL[2]=torso[2];
468  qL[3]=head[0];
469  qL[4]=head[1];
470  qL[5]=head[2];
471  qL[6]=head[3];
472  qL[7]=head[4]+head[5]/2.0;
473 
474  Vector qR=qL;
475  qR[7]-=head[5];
476 
477  Matrix HL=SE3inv(eyeL->getH(qL));
478  Matrix HR=SE3inv(eyeR->getH(qR));
479 
480  Matrix tmp=zeros(3,4); tmp(2,2)=1.0;
481  tmp(0,2)=pxl[0]; tmp(1,2)=pxl[1];
482  Matrix AL=(*PrjL-tmp)*HL;
483 
484  tmp(0,2)=pxr[0]; tmp(1,2)=pxr[1];
485  Matrix AR=(*PrjR-tmp)*HR;
486 
487  Matrix A(4,3);
488  Vector b(4);
489  for (int i=0; i<2; i++)
490  {
491  b[i]=-AL(i,3);
492  b[i+2]=-AR(i,3);
493 
494  for (int j=0; j<3; j++)
495  {
496  A(i,j)=AL(i,j);
497  A(i+2,j)=AR(i,j);
498  }
499  }
500 
501  // solve the least-squares problem
502  x=pinv(A)*b;
503 
504  return true;
505  }
506  else
507  {
508  yError("Unspecified projection matrix for at least one camera!");
509  return false;
510  }
511 }
512 
513 
514 /************************************************************************/
515 double Localizer::getDistFromVergence(const double ver)
516 {
517  double tg=tan(CTRL_DEG2RAD*ver/2.0);
518  return eyesHalfBaseline*sqrt(1.0+1.0/(tg*tg));
519 }
520 
521 
522 /************************************************************************/
524 {
525  if (Bottle *mono=port_mono.read(false))
526  {
527  if (mono->size()>=4)
528  {
529  string type=mono->get(0).asString();
530  double u=mono->get(1).asFloat64();
531  double v=mono->get(2).asFloat64();
532  double z;
533 
534  bool ok=false;
535  if (mono->get(3).isFloat64())
536  {
537  z=mono->get(3).asFloat64();
538  ok=true;
539  }
540  else if ((mono->get(3).asString()=="ver") && (mono->size()>=5))
541  {
542  double ver=mono->get(4).asFloat64();
543  z=getDistFromVergence(ver);
544  ok=true;
545  }
546 
547  if (ok)
548  {
549  Vector fp;
550  if (projectPoint(type,u,v,z,fp))
552  return;
553  }
554  }
555 
556  yError("Got wrong monocular information!");
557  }
558 }
559 
560 
561 /************************************************************************/
563 {
564  if (Bottle *stereo=port_stereo.read(false))
565  {
566  if ((PrjL!=nullptr) || (PrjR!=nullptr))
567  {
568  if (stereo->size()>=4)
569  {
570  double ul=stereo->get(0).asFloat64();
571  double vl=stereo->get(1).asFloat64();
572  double ur=stereo->get(2).asFloat64();
573  double vr=stereo->get(3).asFloat64();
574 
575  Vector ref(1), fb(1), fp;
576  double u, v;
577 
578  ref=0.0;
579  if (dominantEye=="left")
580  {
581  u=ul;
582  v=vl;
583  fb=cxr-ur;
584  // by inverting the sign of the error (e=ref-fb=-fb)
585  // we can keep gains always positive
586  }
587  else
588  {
589  u=ur;
590  v=vr;
591  fb=ul-cxl;
592  }
593 
594  mtx.lock();
595  Vector z=pid->compute(ref,fb);
596  mtx.unlock();
597 
598  if (projectPoint(dominantEye,u,v,z[0],fp))
600  }
601  else
602  yError("Got wrong stereo information!");
603  }
604  else
605  yError("Unspecified projection matrix!");
606  }
607 }
608 
609 
610 /************************************************************************/
612 {
613  if (Bottle *angles=port_anglesIn.read(false))
614  {
615  if (angles->size()>=4)
616  {
617  Vector ang(3);
618 
619  string type=angles->get(0).asString();
620  ang[0]=CTRL_DEG2RAD*angles->get(1).asFloat64();
621  ang[1]=CTRL_DEG2RAD*angles->get(2).asFloat64();
622  ang[2]=CTRL_DEG2RAD*angles->get(3).asFloat64();
623 
624  Vector xd=get3DPoint(type,ang);
625  commData->port_xd->set_xd(xd);
626  }
627  else
628  yError("Got wrong angles information!");
629  }
630 }
631 
632 
633 /************************************************************************/
635 {
636  double x_stamp;
637  Vector x=commData->get_x(x_stamp);
638  txInfo_ang.update(x_stamp);
639 
640  if (port_anglesOut.getOutputCount()>0)
641  {
643  port_anglesOut.setEnvelope(txInfo_ang);
644  port_anglesOut.write();
645  }
646 }
647 
648 
649 /************************************************************************/
650 bool Localizer::getIntrinsicsMatrix(const string &type, Matrix &M,
651  int &w, int &h)
652 {
653  if (type=="left")
654  {
655  if (PrjL!=nullptr)
656  {
657  M=*PrjL;
658  w=widthL;
659  h=heightL;
660  return true;
661  }
662  else
663  return false;
664  }
665  else if (type=="right")
666  {
667  if (PrjR!=nullptr)
668  {
669  M=*PrjR;
670  w=widthR;
671  h=heightR;
672  return true;
673  }
674  else
675  return false;
676  }
677  else
678  return false;
679 }
680 
681 
682 /************************************************************************/
683 bool Localizer::setIntrinsicsMatrix(const string &type, const Matrix &M,
684  const int w, const int h)
685 {
686  if (type=="left")
687  {
688  if (PrjL!=nullptr)
689  {
690  *PrjL=M;
691  *invPrjL=pinv(M.transposed()).transposed();
692  }
693  else
694  {
695  PrjL=new Matrix(M);
696  invPrjL=new Matrix(pinv(M.transposed()).transposed());
697  }
698 
699  widthL=w;
700  heightL=h;
701 
702  return true;
703  }
704  else if (type=="right")
705  {
706  if (PrjR!=nullptr)
707  {
708  *PrjR=M;
709  *invPrjR=pinv(M.transposed()).transposed();
710  }
711  else
712  {
713  PrjR=new Matrix(M);
714  invPrjR=new Matrix(pinv(M.transposed()).transposed());
715  }
716 
717  widthR=w;
718  heightR=h;
719 
720  return true;
721  }
722  else
723  return false;
724 }
725 
726 
727 /************************************************************************/
729 {
734 }
735 
736 
737 
Localizer::setPidOptions
void setPidOptions(const Bottle &options)
Definition: localizer.cpp:196
Localizer::threadInit
bool threadInit()
Definition: localizer.cpp:148
Localizer::cxl
double cxl
Definition: localizer.h:65
h
h
Definition: compute_ekf_sym.m:23
python-motor-control.tmp
tmp
Definition: python-motor-control.py:43
solver.h
Localizer::pid
parallelPID * pid
Definition: localizer.h:68
ExchangeData::get_q
Vector get_q()
Definition: utils.cpp:302
Localizer::txInfo_ang
Stamp txInfo_ang
Definition: localizer.h:53
xdPort::set_xd
bool set_xd(const Vector &_xd)
Definition: utils.cpp:84
Localizer::invPrjL
Matrix * invPrjL
Definition: localizer.h:61
ExchangeData::get_fpFrame
Matrix get_fpFrame()
Definition: utils.cpp:338
ExchangeData
Definition: utils.h:93
Localizer::afterStart
void afterStart(bool s)
Definition: localizer.cpp:175
ExchangeData::tweakOverwrite
bool tweakOverwrite
Definition: utils.h:153
Localizer::commData
ExchangeData * commData
Definition: localizer.h:48
Localizer::getDistFromVergence
double getDistFromVergence(const double ver)
Definition: localizer.cpp:515
e
e
Definition: compute_ekf_fast.m:13
Localizer::PrjR
Matrix * PrjR
Definition: localizer.h:62
Localizer::triangulatePoint
bool triangulatePoint(const Vector &pxl, const Vector &pxr, Vector &x)
Definition: localizer.cpp:450
iCub::ctrl::parallelPID::getOptions
virtual void getOptions(yarp::os::Bottle &options)
Returns the current options used by the pid.
Definition: pids.cpp:264
ExchangeData::minAllowedVergence
double minAllowedVergence
Definition: utils.h:141
z
z
Definition: show_eyes_axes.m:22
Localizer::port_mono
BufferedPort< Bottle > port_mono
Definition: localizer.h:49
CTRL_DEG2RAD
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
iCub::iKin::iKinChain::releaseLink
bool releaseLink(const unsigned int i)
Releases the ith Link.
Definition: iKinFwd.cpp:471
Localizer::port_stereo
BufferedPort< Bottle > port_stereo
Definition: localizer.h:50
Localizer::handleAnglesInput
void handleAnglesInput()
Definition: localizer.cpp:611
fp
static struct bpf_program fp
Definition: pcap_wrapper_linux.cpp:89
iCub::iKin::iCubHeadCenter
Definition: iKinFwd.h:1338
Localizer::widthL
int widthL
Definition: localizer.h:63
iCub::iKin::iKinChain::getH
yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-H...
Definition: iKinFwd.cpp:740
iCub::ctrl::dot
double dot(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
ExchangeData::headVersion2String
string headVersion2String()
Definition: utils.cpp:379
iCub::iKin::iKinChain::getDOF
unsigned int getDOF() const
Returns the current number of Chain's DOF.
Definition: iKinFwd.h:557
iCub::iKin::iKinChain::setAng
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
iCub::iKin::iCubEye
Definition: iKinFwd.h:1273
p
p
Definition: show_eyes_axes.m:23
Localizer::widthR
int widthR
Definition: localizer.h:64
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
Localizer::port_anglesIn
BufferedPort< Bottle > port_anglesIn
Definition: localizer.h:51
CTRL_RAD2DEG
#define CTRL_RAD2DEG
Definition: XSensMTx.cpp:25
Localizer::PrjL
Matrix * PrjL
Definition: localizer.h:61
Localizer::dominantEye
string dominantEye
Definition: localizer.h:69
iCub::ctrl::parallelPID
Definition: pids.h:210
Localizer::run
void run()
Definition: localizer.cpp:728
Localizer::threadRelease
void threadRelease()
Definition: localizer.cpp:160
Localizer::handleMonocularInput
void handleMonocularInput()
Definition: localizer.cpp:523
n
int n
Definition: debugFunctions.cpp:58
ExchangeData::rf_cameras
ResourceFinder rf_cameras
Definition: utils.h:158
ExchangeData::rf_tweak
ResourceFinder rf_tweak
Definition: utils.h:159
Localizer::getPidOptions
void getPidOptions(Bottle &options)
Definition: localizer.cpp:185
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
y
y
Definition: show_eyes_axes.m:21
x
x
Definition: compute_ekf_sym.m:21
state::ok
@ ok
Localizer::Localizer
Localizer(ExchangeData *_commData, const unsigned int _period)
Definition: localizer.cpp:28
Localizer::heightL
int heightL
Definition: localizer.h:63
ExchangeData::localStemName
string localStemName
Definition: utils.h:139
Localizer::handleStereoInput
void handleStereoInput()
Definition: localizer.cpp:562
Localizer::eyesHalfBaseline
double eyesHalfBaseline
Definition: localizer.h:59
Localizer::invPrjR
Matrix * invPrjR
Definition: localizer.h:62
getCamParams
bool getCamParams(const ResourceFinder &rf, const string &type, Matrix **Prj, int &w, int &h, const bool verbose=false)
Definition: utils.cpp:423
Localizer::port_anglesOut
BufferedPort< Vector > port_anglesOut
Definition: localizer.h:52
Localizer::invEyeCAbsFrame
Matrix invEyeCAbsFrame
Definition: localizer.h:58
Localizer::cxr
double cxr
Definition: localizer.h:66
A
A
Definition: sine.m:16
Localizer::cyr
double cyr
Definition: localizer.h:66
Localizer::heightR
int heightR
Definition: localizer.h:64
Localizer::get3DPoint
Vector get3DPoint(const string &type, const Vector &ang)
Definition: localizer.cpp:229
iCub::ctrl::parallelPID::setOptions
virtual void setOptions(const yarp::os::Bottle &options)
Update the options used by the pid.
Definition: pids.cpp:289
v
static int v
Definition: iCub_Sim.cpp:42
GazeComponent::eyeL
iCubEye * eyeL
Definition: utils.h:174
iCub::iKin::iKinChain::EndEffPose
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
Definition: iKinFwd.cpp:858
Localizer::projectPoint
bool projectPoint(const string &type, const Vector &x, Vector &px)
Definition: localizer.cpp:296
Localizer::mtx
mutex mtx
Definition: localizer.h:47
iCub::ctrl::parallelPID::reset
virtual void reset(const yarp::sig::Vector &u0)
Resets the internal state of integral and derivative part.
Definition: pids.cpp:247
iCub::ctrl::parallelPID::compute
virtual const yarp::sig::Vector & compute(const yarp::sig::Vector &ref, const yarp::sig::Vector &fb)
Computes the PID output.
Definition: pids.cpp:213
Localizer::getIntrinsicsMatrix
bool getIntrinsicsMatrix(const string &type, Matrix &M, int &w, int &h)
Definition: localizer.cpp:650
zeros
zeros(2, 2) eye(2
iCub::iKin::iKinChain::setAllConstraints
void setAllConstraints(bool _constrained)
Sets the constraint status of all chain links.
Definition: iKinFwd.cpp:506
localizer.h
Localizer::period
unsigned int period
Definition: localizer.h:55
iCub::iKin::iKinLimb::asChain
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iKinFwd.h:1013
getAlignHN
bool getAlignHN(const ResourceFinder &rf, const string &type, iKinChain *chain, const bool verbose=false)
Definition: utils.cpp:482
ExchangeData::get_x
Vector get_x()
Definition: utils.cpp:283
Localizer::~Localizer
virtual ~Localizer()
Definition: localizer.cpp:127
Localizer::handleAnglesOutput
void handleAnglesOutput()
Definition: localizer.cpp:634
Localizer::getAbsAngles
Vector getAbsAngles(const Vector &x)
Definition: localizer.cpp:210
ExchangeData::get_torso
Vector get_torso()
Definition: utils.cpp:311
Localizer::cyl
double cyl
Definition: localizer.h:65
Localizer::eyeCAbsFrame
Matrix eyeCAbsFrame
Definition: localizer.h:57
GazeComponent::eyeR
iCubEye * eyeR
Definition: utils.h:175
ExchangeData::port_xd
xdPort * port_xd
Definition: utils.h:137
Localizer::setIntrinsicsMatrix
bool setIntrinsicsMatrix(const string &type, const Matrix &M, const int w, const int h)
Definition: localizer.cpp:683