iCub-main
CamCalibModule.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 /*
4  * Copyright (C) 2007 Jonas Ruesch
5  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
6  *
7  */
8 
9 #include <cmath>
10 
11 #include <iCub/CamCalibModule.h>
12 #include <yarp/math/Math.h>
13 #include <yarp/os/Stamp.h>
14 
15 #define AERONAUTIC_CONVENTION 0
16 
17 
18 using namespace std;
19 using namespace yarp::os;
20 using namespace yarp::math;
21 using namespace yarp::sig;
22 
24  portImgOut(NULL),
25  calibTool(NULL),
26  verbose(false),
27  filter_enable(0),
28  cf1(10.514),
29  cf2(0.809),
30  t0(Time::now()),
31  useIMU(false),
32  useTorso(false),
33  useEyes(false),
34  useLast(false)
35 {
36  r_xv[0]=r_xv[1]=0;
37  p_xv[0]=p_xv[1]=0;
38  y_xv[0]=y_xv[1]=0;
39  r_yv[0]=r_yv[1]=0;
40  p_yv[0]=p_yv[1]=0;
41  y_yv[0]=y_yv[1]=0;
42 
43 }
44 
45 void CamCalibPort::setPointers(yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > *_portImgOut, ICalibTool *_calibTool)
46 {
47  portImgOut=_portImgOut;
48  calibTool=_calibTool;
49 }
50 
51 void CamCalibPort::setSaturation(double satVal)
52 {
53  currSat = satVal;
54 }
55 
56 void CamCalibPort::onRead(ImageOf<PixelRgb> &yrpImgIn)
57 {
58  double t=Time::now();
59 
60  yarp::os::Stamp s;
61  this->getEnvelope(s);
62  double time = s.getTime();
63 
64  if (!updatePose(time)) {
65  return;
66  }
67 
68  unsigned char *pixel = yrpImgIn.getPixelAddress(0, 0);
69  double *stamp = reinterpret_cast<double*>(pixel);
70  double backdoorTime = stamp[0];
71  double backdoorRoll = stamp[1];
72  double backdoorPitch = stamp[2];
73  double backdoorYaw = stamp[3];
74 
75  if (time != backdoorTime) {
76  yWarning() << "Backdoor time:" << backdoorTime << "Imu time:" << time << "diff:" << (backdoorTime - time);
77  }
78 
79  Bottle& b = rpyPort.prepare();
80  b.clear();
81  b.addFloat64(roll);
82  b.addFloat64(pitch);
83  b.addFloat64(yaw);
84  b.addFloat64(backdoorRoll);
85  b.addFloat64(backdoorPitch);
86  b.addFloat64(backdoorYaw);
87  b.addFloat64(backdoorRoll - roll);
88  b.addFloat64(backdoorPitch - pitch);
89  b.addFloat64(backdoorYaw - yaw);
90  rpyPort.write();
91 
92 
93 
94  // execute calibration
95  if (portImgOut!=NULL) {
96  yarp::sig::ImageOf<PixelRgb> &yrpImgOut=portImgOut->prepare();
97 
98  if (verbose) {
99  yDebug("received input image after %g [s] ... ",t-t0);
100  }
101 
102  double t1=Time::now();
103 
104  if (calibTool!=NULL) {
105  calibTool->apply(yrpImgIn,yrpImgOut);
106 
107  for (int r =0; r <yrpImgOut.height(); r++) {
108  for (int c=0; c<yrpImgOut.width(); c++) {
109  unsigned char *pixel = yrpImgOut.getPixelAddress(c,r);
110  double mean = (1.0/3.0)*(pixel[0]+pixel[1]+pixel[2]);
111 
112  for(int i=0; i<3; i++) {
113  double s=pixel[i]-mean;
114  double sn=currSat*s;
115  sn+=mean;
116 
117  if(sn<0.0)
118  sn=0.0;
119  else if(sn>255.0)
120  sn=255.0;
121 
122  pixel[i]=(unsigned char)sn;
123  }
124  }
125  }
126 
127  if (verbose)
128  yDebug("calibrated in %g [s]\n",Time::now()-t1);
129  } else {
130  yrpImgOut=yrpImgIn;
131 
132  if (verbose)
133  yDebug("just copied in %g [s]\n",Time::now()-t1);
134  }
135 
136  lock_guard<mutex> lck(m);
137 
138  //timestamp propagation
139  //yarp::os::Stamp stamp;
140  //BufferedPort<ImageOf<PixelRgb> >::getEnvelope(stamp);
141  //portImgOut->setEnvelope(stamp);
142 
143  Bottle pose;
144  char buf[512];
145  std::snprintf(buf, 512, "%d %.*g %.*g %.*g %.*g", s.getCount(), DBL_DIG, s.getTime(), DBL_DIG, roll, DBL_DIG, pitch, DBL_DIG, yaw);
146  pose.fromString(buf);
147  portImgOut->setEnvelope(pose);
148 
149  portImgOut->writeStrict();
150  }
151 
152  t0=t;
153 }
154 
155 
156 
157 bool CamCalibPort::selectBottleFromMap(double time,
158  std::map<double, yarp::os::Bottle> *datamap,
159  yarp::os::Bottle *bottle,
160  bool verbose)
161 {
162  if (datamap->size() == 0) {
163  return false;
164  }
165 
166  if (useLast)
167  {
168  lock_guard<mutex> lck(m);
169  bottle->clear();
170  if (useIMU)
171  {
172  bottle->addFloat64(m_last_imu.get(6).asFloat64());
173  bottle->addFloat64(m_last_imu.get(7).asFloat64());
174  bottle->addFloat64(m_last_imu.get(8).asFloat64());
175  }
176  else
177  {
178  bottle->addFloat64(m_last_h_encs.get(0).asFloat64());
179  bottle->addFloat64(m_last_h_encs.get(1).asFloat64());
180  bottle->addFloat64(m_last_h_encs.get(2).asFloat64());
181  //or torso? @@@ TO BE COMPLETED
182  }
183  datamap->clear();
184  return true;
185  }
186 
187  std::map<double, yarp::os::Bottle>::iterator it_prev, it_next;
188 
189  m.lock();
190  it_next = datamap->lower_bound(time);
191 
192  // wait until we receive a sample with a time greater than image time
193  int count = 0;
194  while (it_next == datamap->end() || it_next->first < time)
195  {
196  count++;
197  m.unlock();
198  yarp::os::Time::delay(0.001);
199  if (count >= 1000)
200  {
201  yWarning() << "Clock out of sync, check your NTPD settings";
202  return false;
203  }
204  m.lock();
205  it_next = datamap->lower_bound(time);
206  }
207 
208  it_prev = it_next;
209  if(it_prev != datamap->begin()) {
210  --it_prev;
211  }
212  if(it_next == datamap->end() && it_next != datamap->begin()) {
213  --it_next;
214  }
215 
216  double diff_prev = time - it_prev->first;
217  double diff_next = it_next->first - time;
218  double diff = (diff_prev >= diff_next) ? diff_next : diff_prev;
219 
220  if (verbose) {
221  std::map<double, yarp::os::Bottle>::iterator it_begin, it_end;
222 
223  it_end = datamap->end();
224  if(it_end != datamap->begin()) {
225  --it_end;
226  }
227  it_begin = datamap->begin();
228 
229  int count_less = 0;
230  int count_more = 0;
231  for(std::map<double, yarp::os::Bottle>::iterator it = datamap->begin(); it != datamap->end(); ++it) {
232  if (it->first > time) {
233  ++count_more;
234  } else {
235  ++count_less;
236  }
237  }
238 
239  double diff_end = it_end->first - time;
240  double diff_begin = time - it_begin->first;
241  bool err_prev = ((diff_prev >= 0.0025) || (diff_prev <= -0.0025));
242  bool warn_prev = ((diff_prev >= 0.0015) || (diff_prev <= -0.0015));
243  bool err_next = ((diff_next >= 0.0025) || (diff_next <= -0.0025));
244  bool warn_next = ((diff_next >= 0.0015) || (diff_next <= -0.0015));
245  bool err_end = ((diff_end >= 0.0025) || (diff_end <= -0.0025));
246  bool warn_end = ((diff_end >= 0.0015) || (diff_end <= -0.0015));
247  bool err_begin = ((diff_begin >= 0.0025) || (diff_begin <= -0.0025));
248  bool warn_begin = ((diff_begin >= 0.0015) || (diff_begin <= -0.0015));
249  bool err = ((diff >= 0.0025) || (diff <= -0.0025));
250  bool warn = ((diff >= 0.0015) || (diff <= -0.0015));
251 
252  printf("%f, %f, %s%f%s, %d, %f, %s%f%s, %f, %s%f%s, %f, %s%f%s, %d, %s%f%s, %s%zd%s %s\n",
253  time,
254  it_begin->first,
255  (err_begin ? "\033[0;31m" : (warn_begin ? "\033[0;33m" : "")), diff_begin, ((err_begin||warn_begin) ? "\033[0m" : ""),
256  count_less,
257  it_prev->first,
258  (err_prev ? "\033[0;31m" : (warn_prev ? "\033[0;33m" : "")), diff_prev, ((err_prev||warn_prev) ? "\033[0m" : ""),
259  it_next->first,
260  (err_next ? "\033[0;31m" : (warn_next ? "\033[0;33m" : "")), diff_next, ((err_next||warn_next) ? "\033[0m" : ""),
261  it_end->first,
262  (err_end ? "\033[0;31m" : (warn_end ? "\033[0;33m" : "")), diff_end, ((err_end||warn_end) ? "\033[0m" : ""),
263  count_more,
264  (err ? "\033[0;31m" : (warn ? "\033[0;33m" : "")), diff, ((err||warn) ? "\033[0m" : ""),
265  ((datamap->size() <= 10) ? "\033[0;31m" : ((datamap->size() <= 15) ? "\033[0;33m" : "")), datamap->size(), ((datamap->size() <= 15) ? "\033[0m" : ""),
266  ((diff > maxDelay) ? "\033[0;31mSKIPPED\033[0m" : "OK"));
267  }
268 
269  if (diff > maxDelay)
270  {
271  if (verbose)
272  {
273  yDebug() << "maxDelay (" << maxDelay << ") exceeded";
274  }
275  m.unlock();
276  return false;
277  }
278 
279  bottle->clear();
280  for(int i = 0; i < it_prev->second.size(); ++i) {
281  if(i < 3) {
282  double x0 = it_prev->second.get(i).asFloat64();
283  double x1 = it_next->second.get(i).asFloat64();
284  double t0 = it_prev->first;
285  double t1 = it_next->first;
286  double tx = time;
287  double xx = 0;
288 
289  if (!useIMU) {
290  // Linear interpolation
291  xx = x0 + (tx - t0)*(x1 - x0)/(t1 - t0);
292  } else {
293 
294  double v0 = it_prev->second.get(i+6).asFloat64();
295  double v1 = it_next->second.get(i+6).asFloat64();
296  double a = (v1 - v0) / (t1 - t0);
297 
298  if (fabs(v1 - v0) > 30) {
299  m.unlock();
300  return false;
301  } else {
302 
303  // x + vt
304  xx = x0 + (tx - t0) * v0;
305 
306  // best
307  // xx = (diff_prev >= diff_next) ? (x1 - (t1 - tx) * v1)
308  // : (x0 + (tx - t0) * v0);
309 
310  // mean
311  // xx = ((x0 + (tx - t0) * v0) + (x1 - (t1 - tx) * v1)) / 2;
312 
313 
314 
315  // x + vt + 1/2at^2
316  // xx = x0 + (tx - t0) * v0 + a / 2 * pow((tx - t0), 2);
317  // xx = x1 - (t1 - tx) * v1 - a / 2 * pow((t1 - tx), 2);
318  // xx = (diff_prev >= diff_next) ? x1 - (t1 - tx) * v1 - a / 2 * pow((t1 - tx), 2)
319  // : x0 + (tx - t0) * v0 + a / 2 * pow((tx - t0), 2);
320  }
321  }
322 
323  bottle->addFloat64(xx);
324  } else {
325  bottle->add(it_prev->second.get(i));
326  }
327  }
328 
329 
330  if (it_prev != datamap->begin()) {
331  datamap->erase(datamap->begin(), it_prev);
332  }
333  m.unlock();
334 
335  return true;
336 }
337 
338 
339 bool CamCalibPort::updatePose(double time)
340 {
341  // update head encoders bottle
342  if (!selectBottleFromMap(time, &m_h_encs_map, &m_curr_h_encs, verbose && !useIMU)) {
343  if (!useIMU) {
344  return false;
345  }
346  }
347 
348  // update torso encoders bottle
349  if (!selectBottleFromMap(time, &m_t_encs_map, &m_curr_t_encs, verbose && !useIMU)) {
350  if (!useIMU && useTorso) {
351  return false;
352  }
353  }
354 
355  // update IMU bottle
356  if (!selectBottleFromMap(time, &m_imu_map, &m_curr_imu, verbose && useIMU)) {
357  if (useIMU) {
358  return false;
359  }
360  }
361 
362  double tix = useTorso ? m_curr_t_encs.get(1).asFloat64()/180.0*M_PI : 0; // torso roll
363  double tiy = useTorso ? -m_curr_t_encs.get(2).asFloat64()/180.0*M_PI : 0; // torso pitch
364  double tiz = useTorso ? -m_curr_t_encs.get(0).asFloat64()/180.0*M_PI : 0; // torso yaw
365 
366  double nix = -m_curr_h_encs.get(1).asFloat64()/180.0*M_PI; // neck roll
367  double niy = m_curr_h_encs.get(0).asFloat64()/180.0*M_PI; // neck pitch
368  double niz = m_curr_h_encs.get(2).asFloat64()/180.0*M_PI; // neck yaw
369 
370  double t = useEyes ? m_curr_h_encs.get(3).asFloat64()/180.0*M_PI : 0; // eye tilt
371  double vs = useEyes ? m_curr_h_encs.get(4).asFloat64()/180.0*M_PI : 0; // eye version
372  double vg = useEyes ? m_curr_h_encs.get(5).asFloat64()/180.0*M_PI : 0; // eye vergence
373 
374  double imu_x = useIMU ? m_curr_imu.get(0).asFloat64()/180.0*M_PI : 0; // imu roll
375  double imu_y = useIMU ? m_curr_imu.get(1).asFloat64()/180.0*M_PI : 0; // imu pitch
376  double imu_z = useIMU ? m_curr_imu.get(2).asFloat64()/180.0*M_PI : 0; // imu yaw
377 
378 
379  // Torso rotation matrix
380  yarp::sig::Vector torso_roll_vector(3);
381  torso_roll_vector(0) = tix;
382  torso_roll_vector(1) = 0;
383  torso_roll_vector(2) = 0;
384  yarp::sig::Matrix torso_roll_dcm = yarp::math::rpy2dcm(torso_roll_vector);
385 
386  yarp::sig::Vector torso_pitch_vector(3);
387  torso_pitch_vector(0) = 0;
388  torso_pitch_vector(1) = tiy;
389  torso_pitch_vector(2) = 0;
390  yarp::sig::Matrix torso_pitch_dcm = yarp::math::rpy2dcm(torso_pitch_vector);
391 
392  yarp::sig::Vector torso_yaw_vector(3);
393  torso_yaw_vector(0) = 0;
394  torso_yaw_vector(1) = 0;
395  torso_yaw_vector(2) = tiz;
396  yarp::sig::Matrix torso_yaw_dcm = yarp::math::rpy2dcm(torso_yaw_vector);
397 
398  yarp::sig::Matrix torso_dcm = (torso_pitch_dcm * torso_roll_dcm) * torso_yaw_dcm;
399 
400 
401  // Neck rotation matrix
402  yarp::sig::Vector neck_roll_vector(3);
403  neck_roll_vector(0) = nix;
404  neck_roll_vector(1) = 0;
405  neck_roll_vector(2) = 0;
406  yarp::sig::Matrix neck_roll_dcm = yarp::math::rpy2dcm(neck_roll_vector);
407 
408  yarp::sig::Vector neck_pitch_vector(3);
409  neck_pitch_vector(0) = 0;
410  neck_pitch_vector(1) = niy;
411  neck_pitch_vector(2) = 0;
412  yarp::sig::Matrix neck_pitch_dcm = yarp::math::rpy2dcm(neck_pitch_vector);
413 
414  yarp::sig::Vector neck_yaw_vector(3);
415  neck_yaw_vector(0) = 0;
416  neck_yaw_vector(1) = 0;
417  neck_yaw_vector(2) = niz;
418  yarp::sig::Matrix neck_yaw_dcm = yarp::math::rpy2dcm(neck_yaw_vector);
419 
420  yarp::sig::Matrix neck_dcm = (neck_pitch_dcm * neck_roll_dcm) * neck_yaw_dcm;
421 
422 
423  // Eye rotation matrix
424  yarp::sig::Vector eye_pitch_vector(3);
425  eye_pitch_vector(0) = 0;
426  eye_pitch_vector(1) = t;
427  eye_pitch_vector(2) = 0;
428  yarp::sig::Matrix eye_pitch_dcm = yarp::math::rpy2dcm(eye_pitch_vector);
429 
430  yarp::sig::Vector eye_yaw_vector(3);
431  eye_yaw_vector(0) = 0;
432  eye_yaw_vector(1) = 0;
433  eye_yaw_vector(2) = 0;
434  if (leftEye) {
435  eye_yaw_vector(2) = -(vs + vg/2);
436  } else {
437  eye_yaw_vector(2) = -(vs - vg / 2);
438  }
439  yarp::sig::Matrix eye_yaw_dcm = yarp::math::rpy2dcm(eye_yaw_vector);
440 
441  yarp::sig::Matrix eye_dcm = eye_pitch_dcm * eye_yaw_dcm;
442 
443 
444  // Final encoder rotation matrix
445  yarp::sig::Matrix encoders_dcm = (torso_dcm * neck_dcm) * eye_dcm;
446  yarp::sig::Vector encoders_rpy = yarp::math::dcm2rpy(encoders_dcm);
447 
448 
449  // Inertial rotation matrix
450  yarp::sig::Vector imu_roll_vector(3);
451  imu_roll_vector(0) = imu_x;
452  imu_roll_vector(1) = 0;
453  imu_roll_vector(2) = 0;
454  yarp::sig::Matrix imu_roll_dcm = yarp::math::rpy2dcm(imu_roll_vector);
455 
456  yarp::sig::Vector imu_pitch_vector(3);
457  imu_pitch_vector(0) = 0;
458  imu_pitch_vector(1) = imu_y;
459  imu_pitch_vector(2) = 0;
460  yarp::sig::Matrix imu_pitch_dcm = yarp::math::rpy2dcm(imu_pitch_vector);
461 
462  yarp::sig::Vector imu_yaw_vector(3);
463  imu_yaw_vector(0) = 0;
464  imu_yaw_vector(1) = 0;
465  imu_yaw_vector(2) = imu_z;
466  yarp::sig::Matrix imu_yaw_dcm = yarp::math::rpy2dcm(imu_yaw_vector);
467 
468 #if AERONAUTIC_CONVENTION
469 // Aeronautic convention (iCub)
470  yarp::sig::Matrix imu_dcm = (imu_yaw_dcm * imu_roll_dcm) * imu_pitch_dcm;
471 #else
472 // Robotic convention (gazebo)
473  yarp::sig::Matrix imu_dcm = (imu_yaw_dcm * imu_pitch_dcm) * imu_roll_dcm;
474 #endif
475 
476  yarp::sig::Vector imu_rpy = yarp::math::dcm2rpy(imu_dcm);
477 
478  if (!useIMU) {
479  roll = encoders_rpy(0) * 180.0/M_PI;
480  pitch = encoders_rpy(1) * 180.0/M_PI;
481  yaw = encoders_rpy(2) * 180.0/M_PI;
482  } else {
483  roll = imu_rpy(0) * 180.0/M_PI;
484  pitch = imu_rpy(1) * 180.0/M_PI;
485  yaw = imu_rpy(2) * 180.0/M_PI;
486  }
487 
488  if (filter_enable) {
489  r_xv[0] = r_xv[1];
490  r_xv[1] = roll / cf1;
491  r_yv[0] = r_yv[1];
492  r_yv[1] = (r_xv[0] + r_xv[1]) + ( cf2 * r_yv[0]);
493  roll = r_yv[1];
494 
495  p_xv[0] = p_xv[1];
496  p_xv[1] = pitch / cf1;
497  p_yv[0] = p_yv[1];
498  p_yv[1] = (p_xv[0] + p_xv[1]) + ( cf2 * p_yv[0]);
499  pitch = p_yv[1];
500 
501  y_xv[0] = y_xv[1];
502  y_xv[1] = yaw / cf1;
503  y_yv[0] = y_yv[1];
504  y_yv[1] = (y_xv[0] + y_xv[1]) + ( cf2 * y_yv[0]);
505  yaw = y_yv[1];
506  } else {
507  r_xv[0]=r_xv[1]=0;
508  p_xv[0]=p_xv[1]=0;
509  y_xv[0]=y_xv[1]=0;
510  r_yv[0]=r_yv[1]=0;
511  p_yv[0]=p_yv[1]=0;
512  y_yv[0]=y_yv[1]=0;
513  }
514 
515  return true;
516 }
517 
518 
520 {
521  _calibTool = NULL;
522 }
523 
525 {
526 }
527 
528 bool CamCalibModule::configure(yarp::os::ResourceFinder &rf)
529 {
530  string str = rf.check("name", Value("/camCalib"), "module name (string)").asString();
531  setName(str.c_str()); // modulePortName
532 
533  double maxDelay = rf.check("maxDelay", Value(0.010), "Max delay between image and encoders").asFloat64();
534 
535  // pass configuration over to bottle
536  Bottle botConfig(rf.toString());
537  botConfig.setMonitor(rf.getMonitor());
538  // Load from configuration group ([<group_name>]), if group option present
539  Value *valGroup; // check assigns pointer to reference
540  if(botConfig.check("group", valGroup, "Configuration group to load module options from (string).")) {
541  strGroup = valGroup->asString();
542  // is group a valid bottle?
543  if (botConfig.check(strGroup)){
544  Bottle &group=botConfig.findGroup(strGroup,"Loading configuration from group " + strGroup);
545  botConfig.fromString(group.toString());
546  } else {
547  yError() << "Group " << strGroup << " not found.";
548  return false;
549  }
550  } else {
551  yError ("There seem to be an error loading parameters (group section missing), stopping module");
552  return false;
553  }
554 
555  string calibToolName = botConfig.check("projection",
556  Value("pinhole"),
557  "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString();
558 
559  _calibTool = CalibToolFactories::getPool().get(calibToolName.c_str());
560  if (_calibTool!=NULL) {
561  bool ok = _calibTool->open(botConfig);
562  if (!ok) {
563  delete _calibTool;
564  _calibTool = NULL;
565  return false;
566  }
567  }
568 
569  if (yarp::os::Network::exists(getName("/in"))) {
570  yWarning() << "port " << getName("/in") << " already in use";
571  }
572  if (yarp::os::Network::exists(getName("/out"))) {
573  yWarning() << "port " << getName("/out") << " already in use";
574  }
575  if (yarp::os::Network::exists(getName("/conf"))) {
576  yWarning() << "port " << getName("/conf") << " already in use";
577  }
578  _prtImgIn.setSaturation(rf.check("saturation",Value(1.0)).asFloat64());
579  _prtImgIn.open(getName("/in"));
580  _prtImgIn.setPointers(&_prtImgOut,_calibTool);
581  _prtImgIn.setVerbose(rf.check("verbose"));
582  _prtImgIn.setLeftEye((strGroup == "CAMERA_CALIBRATION_LEFT") ? true : false);
583  _prtImgIn.setMaxDelay(maxDelay);
584  _prtImgIn.setUseIMU(rf.check("useIMU"));
585  _prtImgIn.setUseTorso(rf.check("useTorso"));
586  _prtImgIn.setUseEyes(rf.check("useEyes"));
587  _prtImgIn.setUseLast(rf.check("useLast"));
588  _prtImgIn.useCallback();
589  _prtImgOut.open(getName("/out"));
590  _configPort.open(getName("/conf"));
591 
592  _prtTEncsIn.open(getName("/torso_encs/in"));
593  _prtTEncsIn._prtImgIn = &_prtImgIn;
594 // _prtTEncsIn.setStrict();
595  _prtTEncsIn.useCallback();
596 
597  _prtHEncsIn.open(getName("/head_encs/in"));
598  _prtHEncsIn._prtImgIn = &_prtImgIn;
599 // _prtHEncsIn.setStrict();
600  _prtHEncsIn.useCallback();
601 
602  _prtImuIn.open(getName("/imu/in"));
603  _prtImuIn._prtImgIn = &_prtImgIn;
604 // _prtImuIn.setStrict();
605  _prtImuIn.useCallback();
606 
607  attach(_configPort);
608  fflush(stdout);
609 
610  _prtImgIn.rpyPort.open(getName("/rpy"));
611 
612  return true;
613 }
614 
616 {
617  _prtImgIn.close();
618  _prtImgOut.close();
619  _prtTEncsIn.close();
620  _prtHEncsIn.close();
621  _prtImuIn.close();
622  _configPort.close();
623  if (_calibTool != NULL){
624  _calibTool->close();
625  delete _calibTool;
626  _calibTool = NULL;
627  }
628  return true;
629 }
630 
632 {
633  _prtImgIn.interrupt();
634  _prtImgOut.interrupt();
635  _configPort.interrupt();
636  _prtTEncsIn.interrupt();
637  _prtHEncsIn.interrupt();
638  _prtImuIn.interrupt();
639  return true;
640 }
641 
642 void TorsoEncoderPort::onRead(yarp::os::Bottle &t_encs)
643 {
644  yarp::os::Stamp s;
645  this->getEnvelope(s);
646  double time = s.getTime();
647  if(time !=0) {
648  _prtImgIn->setTorsoEncoders(time, t_encs);
649  }
650 }
651 
652 void HeadEncoderPort::onRead(yarp::os::Bottle &h_encs)
653 {
654  yarp::os::Stamp s;
655  this->getEnvelope(s);
656  double time = s.getTime();
657  if(time !=0) {
658  _prtImgIn->setHeadEncoders(time, h_encs);
659  }
660 }
661 
662 void ImuPort::onRead(yarp::os::Bottle &imu)
663 {
664  yarp::os::Stamp s;
665  this->getEnvelope(s);
666  double time = s.getTime();
667  if(time !=0) {
668  _prtImgIn->setImuData(time, imu);
669  }
670 }
671 
673 {
674  return true;
675 }
676 
678 {
679  return 0.001;
680 }
681 
682 bool CamCalibModule::respond(const Bottle& command, Bottle& reply)
683 {
684  reply.clear();
685 
686  if (command.get(0).asString()=="quit") {
687  reply.addString("quitting");
688  return false;
689  } else if (command.get(0).asString()=="sat" || command.get(0).asString()=="saturation") {
690  double satVal = command.get(1).asFloat64();
691  _prtImgIn.setSaturation(satVal);
692 
693  reply.addString("ok");
694  } else if (command.get(0).asString()=="filt") {
695  _prtImgIn.filter_enable = command.get(1).asInt32();
696  } else if (command.get(0).asString()=="cf1") {
697  _prtImgIn.cf1 = command.get(1).asFloat64();
698  } else if (command.get(0).asString()=="cf2") {
699  _prtImgIn.cf2 = command.get(1).asFloat64();
700  } else {
701  yError() << "command not known - type help for more info";
702  }
703  return true;
704 }
CamCalibPort::y_yv
double y_yv[2]
Definition: CamCalibModule.h:80
CamCalibPort::filter_enable
int filter_enable
Definition: CamCalibModule.h:72
CamCalibPort::setImuData
void setImuData(double time, const yarp::os::Bottle &imu)
Definition: CamCalibModule.h:93
CalibToolFactories::get
ICalibTool * get(const char *name)
Definition: CalibToolFactory.h:58
CamCalibModule::~CamCalibModule
~CamCalibModule()
Definition: CamCalibModule.cpp:102
CamCalibModule::respond
virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
CamCalibPort::setTorsoEncoders
void setTorsoEncoders(double time, const yarp::os::Bottle &t_encs)
Definition: CamCalibModule.h:91
CamCalibPort::setHeadEncoders
void setHeadEncoders(double time, const yarp::os::Bottle &h_encs)
Definition: CamCalibModule.h:92
CamCalibPort::cf1
double cf1
Definition: CamCalibModule.h:73
CamCalibPort::setSaturation
void setSaturation(double satVal)
Definition: CamCalibModule.cpp:30
CamCalibPort::CamCalibPort
CamCalibPort()
Definition: CamCalibModule.cpp:15
CamCalibPort::r_xv
double r_xv[2]
Definition: CamCalibModule.h:75
HeadEncoderPort::_prtImgIn
CamCalibPort * _prtImgIn
Definition: CamCalibModule.h:114
CamCalibPort::y_xv
double y_xv[2]
Definition: CamCalibModule.h:77
CamCalibModule::updateModule
virtual bool updateModule()
Definition: CamCalibModule.cpp:194
CamCalibPort::r_yv
double r_yv[2]
Definition: CamCalibModule.h:78
TorsoEncoderPort::_prtImgIn
CamCalibPort * _prtImgIn
Definition: CamCalibModule.h:107
CamCalibPort::setLeftEye
void setLeftEye(bool eye)
Definition: CamCalibModule.h:88
CamCalibPort::setMaxDelay
void setMaxDelay(double delay)
Definition: CamCalibModule.h:89
CamCalibModule::close
virtual bool close()
Definition: CamCalibModule.cpp:175
CamCalibPort::cf2
double cf2
Definition: CamCalibModule.h:74
CamCalibPort::setUseEyes
void setUseEyes(bool useEyes)
Definition: CamCalibModule.h:96
state::ok
@ ok
ICalibTool
Interface to calibrate and project input image based on camera's internal parameters and projection m...
Definition: ICalibTool.h:19
ICalibTool::close
virtual bool close()=0
CamCalibPort::rpyPort
yarp::os::BufferedPort< yarp::os::Bottle > rpyPort
Definition: CamCalibModule.h:99
ICalibTool::apply
virtual void apply(const yarp::sig::ImageOf< yarp::sig::PixelRgb > &in, yarp::sig::ImageOf< yarp::sig::PixelRgb > &out)=0
CamCalibModule::interruptModule
virtual bool interruptModule()
Definition: CamCalibModule.cpp:187
x1
x1
Definition: compute_ekf_sym.m:28
CamCalibPort::setVerbose
void setVerbose(const bool sw)
Definition: CamCalibModule.h:50
CamCalibPort::p_xv
double p_xv[2]
Definition: CamCalibModule.h:76
CamCalibPort::p_yv
double p_yv[2]
Definition: CamCalibModule.h:79
ImuPort::_prtImgIn
CamCalibPort * _prtImgIn
Definition: CamCalibModule.h:121
CamCalibPort::setPointers
void setPointers(yarp::os::BufferedPort< yarp::sig::ImageOf< yarp::sig::PixelRgb > > *_portImgOut, ICalibTool *_calibTool)
Definition: CamCalibModule.cpp:24
CamCalibModule::getPeriod
virtual double getPeriod()
Definition: CamCalibModule.cpp:198
CamCalibPort::setUseTorso
void setUseTorso(bool useTorso)
Definition: CamCalibModule.h:95
M_PI
#define M_PI
Definition: XSensMTx.cpp:24
ICalibTool::open
virtual bool open(yarp::os::Searchable &config)=0
CamCalibModule::CamCalibModule
CamCalibModule()
Definition: CamCalibModule.cpp:97
CalibToolFactories::getPool
static CalibToolFactories & getPool()
Definition: CalibToolFactory.h:54
time
degrees time
Definition: sine.m:5
CamCalibPort::setUseLast
void setUseLast(bool useLast)
Definition: CamCalibModule.h:97
CamCalibModule::configure
virtual bool configure(yarp::os::ResourceFinder &rf)
Passes config on to CalibTool.
Definition: CamCalibModule.cpp:106
CamCalibPort::setUseIMU
void setUseIMU(bool useIMU)
Definition: CamCalibModule.h:94