iCub-main
localizer.h
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 #ifndef __LOCALIZER_H__
20 #define __LOCALIZER_H__
21 
22 #include <mutex>
23 #include <string>
24 
25 #include <yarp/os/all.h>
26 #include <yarp/sig/all.h>
27 #include <yarp/math/Math.h>
28 
29 #include <iCub/ctrl/pids.h>
30 #include <iCub/gazeNlp.h>
31 #include <iCub/utils.h>
32 
33 using namespace std;
34 using namespace yarp::os;
35 using namespace yarp::sig;
36 using namespace yarp::math;
37 using namespace iCub::ctrl;
38 using namespace iCub::iKin;
39 
40 
41 // The thread launched by the application which is
42 // in charge of localizing target 3D position from
43 // image coordinates.
44 class Localizer : public GazeComponent, public PeriodicThread
45 {
46 protected:
47  mutex mtx;
49  BufferedPort<Bottle> port_mono;
50  BufferedPort<Bottle> port_stereo;
51  BufferedPort<Bottle> port_anglesIn;
52  BufferedPort<Vector> port_anglesOut;
53  Stamp txInfo_ang;
54 
55  unsigned int period;
56 
57  Matrix eyeCAbsFrame;
60 
61  Matrix *PrjL, *invPrjL;
62  Matrix *PrjR, *invPrjR;
63  int widthL, heightL;
64  int widthR, heightR;
65  double cxl, cyl;
66  double cxr, cyr;
67 
69  string dominantEye;
70 
71  void handleMonocularInput();
72  void handleStereoInput();
73  void handleAnglesInput();
74  void handleAnglesOutput();
75 
76 public:
77  Localizer(ExchangeData *_commData, const unsigned int _period);
78  virtual ~Localizer();
79 
80  double getDistFromVergence(const double ver);
81  void getPidOptions(Bottle &options);
82  void setPidOptions(const Bottle &options);
83  bool projectPoint(const string &type, const Vector &x, Vector &px);
84  bool projectPoint(const string &type, const double u, const double v,
85  const double z, Vector &x);
86  bool projectPoint(const string &type, const double u, const double v,
87  const Vector &plane, Vector &x);
88  bool triangulatePoint(const Vector &pxl, const Vector &pxr, Vector &x);
89  Vector getAbsAngles(const Vector &x);
90  Vector get3DPoint(const string &type, const Vector &ang);
91  bool getIntrinsicsMatrix(const string &type, Matrix &M, int &w, int &h);
92  bool setIntrinsicsMatrix(const string &type, const Matrix &M, const int w, const int h);
93  bool threadInit();
94  void threadRelease();
95  void afterStart(bool s);
96  void run();
97 };
98 
99 
100 #endif
101 
102 
double eyesHalfBaseline
Definition: localizer.h:59
Matrix eyeCAbsFrame
Definition: localizer.h:57
Matrix invEyeCAbsFrame
Definition: localizer.h:58
BufferedPort< Bottle > port_stereo
Definition: localizer.h:50
unsigned int period
Definition: localizer.h:55
ExchangeData * commData
Definition: localizer.h:48
BufferedPort< Bottle > port_mono
Definition: localizer.h:49
Matrix * invPrjL
Definition: localizer.h:61
int heightR
Definition: localizer.h:64
double cxr
Definition: localizer.h:66
double cxl
Definition: localizer.h:65
Matrix * invPrjR
Definition: localizer.h:62
int heightL
Definition: localizer.h:63
parallelPID * pid
Definition: localizer.h:68
BufferedPort< Bottle > port_anglesIn
Definition: localizer.h:51
mutex mtx
Definition: localizer.h:47
BufferedPort< Vector > port_anglesOut
Definition: localizer.h:52
string dominantEye
Definition: localizer.h:69
Stamp txInfo_ang
Definition: localizer.h:53
General structure of parallel (non-interactive) PID.
Definition: pids.h:211