iCub-main
DualCamCalibModule.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 
3 // Copyright (C) 2015 Istituto Italiano di Tecnologia - iCub Facility
4 // Author: Marco Randazzo <marco.randazzo@iit.it>
5 // CopyPolicy: Released under the terms of the GNU GPL v2.0.
6 
8 #include <yarp/os/Network.h>
9 
10 using namespace std;
11 using namespace yarp::os;
12 using namespace yarp::sig;
13 
15 {
16  calibToolLeft = NULL;
17  calibToolRight = NULL;
18  align=ALIGN_WIDTH;
19  requested_fps=0;
20  time_lastOut=yarp::os::Time::now();
21  dualImage_mode = false;
22 }
23 
25 {
26  calibToolLeft = NULL;
27  calibToolRight = NULL;
28 }
29 
30 bool CamCalibModule::configure(yarp::os::ResourceFinder &rf)
31 {
32  string str = rf.check("name", Value("/camCalib"), "module name (string)").asString();
33  verboseExecTime = rf.check("verboseExecTime");
34 
35  if (rf.check("w_align")) align=ALIGN_WIDTH;
36  else if (rf.check("h_align")) align=ALIGN_HEIGHT;
37 
38  if (rf.check("fps"))
39  {
40  requested_fps=rf.find("fps").asFloat64();
41  yInfo() << "Module will run at " << requested_fps;
42  }
43  else
44  {
45  yInfo() << "Module will run at max fps";
46  }
47 
48  setName(str.c_str()); // modulePortName
49 
50  Bottle botLeftConfig(rf.toString().c_str());
51  Bottle botRightConfig(rf.toString().c_str());
52 
53  botLeftConfig.setMonitor(rf.getMonitor());
54  botRightConfig.setMonitor(rf.getMonitor());
55 
56  string strLeftGroup = "CAMERA_CALIBRATION_LEFT";
57  if (botLeftConfig.check(strLeftGroup.c_str()))
58  {
59  Bottle &group=botLeftConfig.findGroup(strLeftGroup.c_str(),string("Loading configuration from group " + strLeftGroup).c_str());
60  botLeftConfig.fromString(group.toString());
61  }
62  else
63  {
64  yError() << "Group " << strLeftGroup << " not found.";
65  return false;
66  }
67 
68  string strRightGroup = "CAMERA_CALIBRATION_RIGHT";
69  if (botRightConfig.check(strRightGroup.c_str()))
70  {
71  Bottle &group=botRightConfig.findGroup(strRightGroup.c_str(),string("Loading configuration from group " + strRightGroup).c_str());
72  botRightConfig.fromString(group.toString());
73  }
74  else
75  {
76  yError() << "Group " << strRightGroup << " not found.";
77  return false;
78  }
79 
80  string calibToolLeftName = botLeftConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();
81  string calibToolRightName = botRightConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();
82 
83  calibToolLeft = CalibToolFactories::getPool().get(calibToolLeftName.c_str());
84  if (calibToolLeft!=NULL)
85  {
86  bool ok = calibToolLeft->open(botLeftConfig);
87  if (!ok)
88  {
89  delete calibToolLeft;
90  calibToolLeft = NULL;
91  return false;
92  }
93  }
94  calibToolRight = CalibToolFactories::getPool().get(calibToolRightName.c_str());
95  if (calibToolRight!=NULL)
96  {
97  bool ok = calibToolRight->open(botRightConfig);
98  if (!ok)
99  {
100  delete calibToolRight;
101  calibToolRight = NULL;
102  return false;
103  }
104  }
105 
106  if(rf.check("dual"))
107  {
108  dualImage_mode = true;
109  yInfo() << "Dual mode activate!!";
110  }
111 
112  if(dualImage_mode)
113  {
114  leftImage = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;
115  rightImage = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;
116 
117  // open a single port with name /dual:i
118  if (yarp::os::Network::exists(getName("/dual:i")))
119  {
120  yWarning() << "port " << getName("/dual:i") << " already in use";
121  }
122  if(! imageInLeft.open(getName("/dual:i")) )
123  return false;
124  imageInLeft.setStrict(false);
125  }
126  else
127  {
128  if (yarp::os::Network::exists(getName("/left:i")))
129  {
130  yWarning() << "port " << getName("/left:i") << " already in use";
131  }
132  if (yarp::os::Network::exists(getName("/right:i")))
133  {
134  yWarning() << "port " << getName("/right:i") << " already in use";
135  }
136  imageInLeft.open(getName("/left:i"));
137  imageInRight.open(getName("/right:i"));
138  imageInLeft.setStrict(false);
139  imageInRight.setStrict(false);
140  }
141 
142  if (yarp::os::Network::exists(getName("/out")))
143  {
144  yWarning() << "port " << getName("/out") << " already in use";
145  }
146  if (yarp::os::Network::exists(getName("/conf")))
147  {
148  yWarning() << "port " << getName("/conf") << " already in use";
149  }
150  imageOut.open(getName("/out:o"));
151  configPort.open(getName("/conf"));
152  attach(configPort);
153  return true;
154 }
155 
157 {
158  yTrace();
159  imageInLeft.close();
160  if(!dualImage_mode)
161  imageInRight.close();
162  imageOut.close();
163  configPort.close();
164  if (calibToolLeft != NULL)
165  {
166  calibToolLeft->close();
167  delete calibToolLeft;
168  calibToolLeft = NULL;
169  }
170  if (calibToolRight != NULL)
171  {
172  calibToolRight->close();
173  delete calibToolRight;
174  calibToolRight = NULL;
175  }
176  return true;
177 }
178 
180 {
181  imageInLeft.interrupt();
182  if(!dualImage_mode)
183  imageInRight.interrupt();
184  imageOut.interrupt();
185  configPort.interrupt();
186  return true;
187 }
188 
190 {
191  bool lready=false;
192  bool rready=false;
193  bool init=false;
194 
195  int dual_rowsize_pixels, dual_height_pixels, single_rowsize_pixels;
196  int dual_rowsize_bytes, single_rowsize_bytes;
197 
198  int outw = 0;
199  int outh = 0;
200 
201  unsigned char *left_raw, *right_raw, *dual_raw;
202 
203  if(dualImage_mode)
204  {
205  // read the dual image and split it up into 2 separated images for calibration
206  yarp::sig::ImageOf<yarp::sig::PixelRgb>* dual = imageInLeft.read();
207  if(dual == NULL)
208  {
209  yarp::os::Time::delay(0.001);
210  return true;
211  }
212 
213  dual_rowsize_pixels = dual->width();
214  single_rowsize_pixels = dual_rowsize_pixels/2;
215  dual_height_pixels = dual->height();
216 
217  dual_rowsize_bytes = dual->getRowSize();
218  single_rowsize_bytes = dual_rowsize_bytes/2;
219 
220  leftImage->resize(dual_rowsize_pixels/2, dual_height_pixels);
221  rightImage->resize(dual_rowsize_pixels/2, dual_height_pixels);
222 
223  leftImage->setQuantum(dual->getQuantum());
224  rightImage->setQuantum(dual->getQuantum());
225 
226  left_raw = leftImage->getRawImage();
227  right_raw = rightImage->getRawImage();
228  dual_raw = dual->getRawImage();
229 
230  for(int h=0; h<dual_height_pixels; h++)
231  {
232  memcpy(left_raw+(h*single_rowsize_bytes), dual_raw+(h*dual_rowsize_bytes), single_rowsize_bytes);
233  memcpy(right_raw+(h*single_rowsize_bytes), dual_raw+(h*dual_rowsize_bytes) + single_rowsize_bytes, single_rowsize_bytes);
234  }
235  }
236  else
237  {
238  leftImage = imageInLeft.read(false);
239  rightImage = imageInRight.read(false);
240  }
241 
242  yarp::sig::ImageOf<yarp::sig::PixelRgb> &calibratedImgOut=imageOut.prepare();
243 
244  if (calibToolLeft!=NULL && leftImage!=NULL)
245  {
246  calibToolLeft->apply(*leftImage,calibratedImgLeft);
247  lready=true;
248 
249  if (init==false)
250  {
251  if (align == ALIGN_WIDTH)
252  {
253  outw = calibratedImgLeft.width()*2;
254  outh = calibratedImgLeft.height();
255  }
256  else if (align==ALIGN_HEIGHT)
257  {
258  outw = calibratedImgLeft.width();
259  outh = calibratedImgLeft.height()*2;
260  }
261  else
262  {
263  yError() << "Invalid alignment";
264  }
265  calibratedImgOut.copy(calibratedImgLeft,outw,outh);
266  init=true;
267  }
268 
269  calibratedImgOut.resize(outw, outh);
270  for (int r=0; r<calibratedImgLeft.height(); r++)
271  for (int c=0; c<calibratedImgLeft.width(); c++)
272  {
273  unsigned char *pixel = calibratedImgOut.getPixelAddress(c,r);
274 
275  pixel[0] = *(calibratedImgLeft.getPixelAddress(c,r)+0);
276  pixel[1] = *(calibratedImgLeft.getPixelAddress(c,r)+1);
277  pixel[2] = *(calibratedImgLeft.getPixelAddress(c,r)+2);
278  }
279 
280  }
281  if (calibToolRight!=NULL && rightImage!=NULL)
282  {
283  calibToolRight->apply(*rightImage,calibratedImgRight);
284  rready=true;
285 
286  if (init==false)
287  {
288  int outw = 0;
289  int outh = 0;
290  if (align == ALIGN_WIDTH)
291  {
292  outw = calibratedImgLeft.width()*2;
293  outh = calibratedImgLeft.height();
294  }
295  else if (align==ALIGN_HEIGHT)
296  {
297  outw = calibratedImgLeft.width();
298  outh = calibratedImgLeft.height()*2;
299  }
300  else
301  {
302  yError() << "Invalid alignment";
303  }
304  calibratedImgOut.copy(calibratedImgRight,outw,outh);
305  init=true;
306  }
307 
308  for (int r=0; r<calibratedImgLeft.height(); r++)
309  for (int c=0; c<calibratedImgLeft.width(); c++)
310  {
311  int cp = 0;
312  int rp = 0;
313  if (align == ALIGN_WIDTH) {cp = c+calibratedImgLeft.width(); rp = r;}
314  else if (align == ALIGN_HEIGHT) {cp = c; rp = r+calibratedImgLeft.height();}
315  unsigned char *pixel = calibratedImgOut.getPixelAddress(cp,rp);
316  pixel[0] = *(calibratedImgRight.getPixelAddress(c,r)+0);
317  pixel[1] = *(calibratedImgRight.getPixelAddress(c,r)+1);
318  pixel[2] = *(calibratedImgRight.getPixelAddress(c,r)+2);
319  }
320  }
321 
322  if (requested_fps==0)
323  {
324  if (lready==true || rready==true)
325  {
326  imageOut.writeStrict();
327  double diffOut = yarp::os::Time::now() -time_lastOut;
328  time_lastOut = yarp::os::Time::now();
329  if (verboseExecTime) yDebug ("%f", diffOut);
330  lready=false;
331  rready=false;
332  }
333  }
334  else
335  {
336  double diffOut = yarp::os::Time::now() - time_lastOut;
337  if (diffOut>(1/requested_fps))
338  {
339  imageOut.writeStrict();
340  time_lastOut = yarp::os::Time::now();
341  if (verboseExecTime) yDebug ("%f", diffOut);
342  lready=false;
343  rready=false;
344  }
345  }
346 
347  return true;
348 }
349 
351 {
352  return 0.0;
353 }
354 
355 bool CamCalibModule::respond(const Bottle& command, Bottle& reply)
356 {
357  reply.clear();
358 
359  if (command.get(0).asString()=="quit")
360  {
361  reply.addString("quitting");
362  return false;
363  }
364  else
365  {
366  yError() << "command not known - type help for more info";
367  }
368  return true;
369 }
370 
371 
static CalibToolFactories & getPool()
ICalibTool * get(const char *name)
virtual bool interruptModule()
virtual bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
virtual bool configure(yarp::os::ResourceFinder &rf)
Passes config on to CalibTool.
virtual bool updateModule()
virtual double getPeriod()
virtual bool close()