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  string strLeftGroup = "CAMERA_CALIBRATION_LEFT";
54  if (botLeftConfig.check(strLeftGroup.c_str()))
55  {
56  Bottle &group=botLeftConfig.findGroup(strLeftGroup.c_str(),string("Loading configuration from group " + strLeftGroup).c_str());
57  botLeftConfig.fromString(group.toString());
58  }
59  else
60  {
61  yError() << "Group " << strLeftGroup << " not found.";
62  return false;
63  }
64 
65  string strRightGroup = "CAMERA_CALIBRATION_RIGHT";
66  if (botRightConfig.check(strRightGroup.c_str()))
67  {
68  Bottle &group=botRightConfig.findGroup(strRightGroup.c_str(),string("Loading configuration from group " + strRightGroup).c_str());
69  botRightConfig.fromString(group.toString());
70  }
71  else
72  {
73  yError() << "Group " << strRightGroup << " not found.";
74  return false;
75  }
76 
77  string calibToolLeftName = botLeftConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();
78  string calibToolRightName = botRightConfig.check("projection", Value("pinhole"), "Projection/mapping applied to calibrated image [projection|spherical] (string).").asString().c_str();
79 
80  calibToolLeft = CalibToolFactories::getPool().get(calibToolLeftName.c_str());
81  if (calibToolLeft!=NULL)
82  {
83  bool ok = calibToolLeft->open(botLeftConfig);
84  if (!ok)
85  {
86  delete calibToolLeft;
87  calibToolLeft = NULL;
88  return false;
89  }
90  }
91  calibToolRight = CalibToolFactories::getPool().get(calibToolRightName.c_str());
92  if (calibToolRight!=NULL)
93  {
94  bool ok = calibToolRight->open(botRightConfig);
95  if (!ok)
96  {
97  delete calibToolRight;
98  calibToolRight = NULL;
99  return false;
100  }
101  }
102 
103  if(rf.check("dual"))
104  {
105  dualImage_mode = true;
106  yInfo() << "Dual mode activate!!";
107  }
108 
109  if(dualImage_mode)
110  {
111  leftImage = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;
112  rightImage = new yarp::sig::ImageOf<yarp::sig::PixelRgb>;
113 
114  // open a single port with name /dual:i
115  if (yarp::os::Network::exists(getName("/dual:i")))
116  {
117  yWarning() << "port " << getName("/dual:i") << " already in use";
118  }
119  if(! imageInLeft.open(getName("/dual:i")) )
120  return false;
121  imageInLeft.setStrict(false);
122  }
123  else
124  {
125  if (yarp::os::Network::exists(getName("/left:i")))
126  {
127  yWarning() << "port " << getName("/left:i") << " already in use";
128  }
129  if (yarp::os::Network::exists(getName("/right:i")))
130  {
131  yWarning() << "port " << getName("/right:i") << " already in use";
132  }
133  imageInLeft.open(getName("/left:i"));
134  imageInRight.open(getName("/right:i"));
135  imageInLeft.setStrict(false);
136  imageInRight.setStrict(false);
137  }
138 
139  if (yarp::os::Network::exists(getName("/out")))
140  {
141  yWarning() << "port " << getName("/out") << " already in use";
142  }
143  if (yarp::os::Network::exists(getName("/conf")))
144  {
145  yWarning() << "port " << getName("/conf") << " already in use";
146  }
147  imageOut.open(getName("/out:o"));
148  configPort.open(getName("/conf"));
149  attach(configPort);
150  return true;
151 }
152 
154 {
155  yTrace();
156  imageInLeft.close();
157  if(!dualImage_mode)
158  imageInRight.close();
159  imageOut.close();
160  configPort.close();
161  if (calibToolLeft != NULL)
162  {
163  calibToolLeft->close();
164  delete calibToolLeft;
165  calibToolLeft = NULL;
166  }
167  if (calibToolRight != NULL)
168  {
169  calibToolRight->close();
170  delete calibToolRight;
171  calibToolRight = NULL;
172  }
173  return true;
174 }
175 
177 {
178  imageInLeft.interrupt();
179  if(!dualImage_mode)
180  imageInRight.interrupt();
181  imageOut.interrupt();
182  configPort.interrupt();
183  return true;
184 }
185 
187 {
188  bool lready=false;
189  bool rready=false;
190  bool init=false;
191 
192  int dual_rowsize_pixels, dual_height_pixels, single_rowsize_pixels;
193  int dual_rowsize_bytes, single_rowsize_bytes;
194 
195  int outw = 0;
196  int outh = 0;
197 
198  unsigned char *left_raw, *right_raw, *dual_raw;
199 
200  if(dualImage_mode)
201  {
202  // read the dual image and split it up into 2 separated images for calibration
203  yarp::sig::ImageOf<yarp::sig::PixelRgb>* dual = imageInLeft.read();
204  if(dual == NULL)
205  {
206  yarp::os::Time::delay(0.001);
207  return true;
208  }
209 
210  dual_rowsize_pixels = dual->width();
211  single_rowsize_pixels = dual_rowsize_pixels/2;
212  dual_height_pixels = dual->height();
213 
214  dual_rowsize_bytes = dual->getRowSize();
215  single_rowsize_bytes = dual_rowsize_bytes/2;
216 
217  leftImage->resize(dual_rowsize_pixels/2, dual_height_pixels);
218  rightImage->resize(dual_rowsize_pixels/2, dual_height_pixels);
219 
220  leftImage->setQuantum(dual->getQuantum());
221  rightImage->setQuantum(dual->getQuantum());
222 
223  left_raw = leftImage->getRawImage();
224  right_raw = rightImage->getRawImage();
225  dual_raw = dual->getRawImage();
226 
227  for(int h=0; h<dual_height_pixels; h++)
228  {
229  memcpy(left_raw+(h*single_rowsize_bytes), dual_raw+(h*dual_rowsize_bytes), single_rowsize_bytes);
230  memcpy(right_raw+(h*single_rowsize_bytes), dual_raw+(h*dual_rowsize_bytes) + single_rowsize_bytes, single_rowsize_bytes);
231  }
232  }
233  else
234  {
235  leftImage = imageInLeft.read(false);
236  rightImage = imageInRight.read(false);
237  }
238 
239  yarp::sig::ImageOf<yarp::sig::PixelRgb> &calibratedImgOut=imageOut.prepare();
240 
241  if (calibToolLeft!=NULL && leftImage!=NULL)
242  {
243  calibToolLeft->apply(*leftImage,calibratedImgLeft);
244  lready=true;
245 
246  if (init==false)
247  {
248  if (align == ALIGN_WIDTH)
249  {
250  outw = calibratedImgLeft.width()*2;
251  outh = calibratedImgLeft.height();
252  }
253  else if (align==ALIGN_HEIGHT)
254  {
255  outw = calibratedImgLeft.width();
256  outh = calibratedImgLeft.height()*2;
257  }
258  else
259  {
260  yError() << "Invalid alignment";
261  }
262  calibratedImgOut.copy(calibratedImgLeft,outw,outh);
263  init=true;
264  }
265 
266  calibratedImgOut.resize(outw, outh);
267  for (int r=0; r<calibratedImgLeft.height(); r++)
268  for (int c=0; c<calibratedImgLeft.width(); c++)
269  {
270  unsigned char *pixel = calibratedImgOut.getPixelAddress(c,r);
271 
272  pixel[0] = *(calibratedImgLeft.getPixelAddress(c,r)+0);
273  pixel[1] = *(calibratedImgLeft.getPixelAddress(c,r)+1);
274  pixel[2] = *(calibratedImgLeft.getPixelAddress(c,r)+2);
275  }
276 
277  }
278  if (calibToolRight!=NULL && rightImage!=NULL)
279  {
280  calibToolRight->apply(*rightImage,calibratedImgRight);
281  rready=true;
282 
283  if (init==false)
284  {
285  int outw = 0;
286  int outh = 0;
287  if (align == ALIGN_WIDTH)
288  {
289  outw = calibratedImgLeft.width()*2;
290  outh = calibratedImgLeft.height();
291  }
292  else if (align==ALIGN_HEIGHT)
293  {
294  outw = calibratedImgLeft.width();
295  outh = calibratedImgLeft.height()*2;
296  }
297  else
298  {
299  yError() << "Invalid alignment";
300  }
301  calibratedImgOut.copy(calibratedImgRight,outw,outh);
302  init=true;
303  }
304 
305  for (int r=0; r<calibratedImgLeft.height(); r++)
306  for (int c=0; c<calibratedImgLeft.width(); c++)
307  {
308  int cp = 0;
309  int rp = 0;
310  if (align == ALIGN_WIDTH) {cp = c+calibratedImgLeft.width(); rp = r;}
311  else if (align == ALIGN_HEIGHT) {cp = c; rp = r+calibratedImgLeft.height();}
312  unsigned char *pixel = calibratedImgOut.getPixelAddress(cp,rp);
313  pixel[0] = *(calibratedImgRight.getPixelAddress(c,r)+0);
314  pixel[1] = *(calibratedImgRight.getPixelAddress(c,r)+1);
315  pixel[2] = *(calibratedImgRight.getPixelAddress(c,r)+2);
316  }
317  }
318 
319  if (requested_fps==0)
320  {
321  if (lready==true || rready==true)
322  {
323  imageOut.writeStrict();
324  double diffOut = yarp::os::Time::now() -time_lastOut;
325  time_lastOut = yarp::os::Time::now();
326  if (verboseExecTime) yDebug ("%f", diffOut);
327  lready=false;
328  rready=false;
329  }
330  }
331  else
332  {
333  double diffOut = yarp::os::Time::now() - time_lastOut;
334  if (diffOut>(1/requested_fps))
335  {
336  imageOut.writeStrict();
337  time_lastOut = yarp::os::Time::now();
338  if (verboseExecTime) yDebug ("%f", diffOut);
339  lready=false;
340  rready=false;
341  }
342  }
343 
344  return true;
345 }
346 
348 {
349  return 0.0;
350 }
351 
352 bool CamCalibModule::respond(const Bottle& command, Bottle& reply)
353 {
354  reply.clear();
355 
356  if (command.get(0).asString()=="quit")
357  {
358  reply.addString("quitting");
359  return false;
360  }
361  else
362  {
363  yError() << "command not known - type help for more info";
364  }
365  return true;
366 }
367 
368 
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()