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 <iCub/CamCalibModule.h>
10 
11 using namespace std;
12 using namespace yarp::os;
13 using namespace yarp::sig;
14 
16 {
17  portImgOut=NULL;
18  calibTool=NULL;
19 
20  verbose=false;
21  t0=Time::now();
22 }
23 
24 void CamCalibPort::setPointers(yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > *_portImgOut, ICalibTool *_calibTool)
25 {
26  portImgOut=_portImgOut;
27  calibTool=_calibTool;
28 }
29 
30 void CamCalibPort::setSaturation(double satVal)
31 {
32  currSat = satVal;
33 }
34 
35 void CamCalibPort::onRead(ImageOf<PixelRgb> &yrpImgIn)
36 {
37  double t=Time::now();
38  // execute calibration
39  if (portImgOut!=NULL)
40  {
41  yarp::sig::ImageOf<PixelRgb> &yrpImgOut=portImgOut->prepare();
42 
43  if (verbose)
44  yDebug("received input image after %g [s] ... ",t-t0);
45 
46  double t1=Time::now();
47 
48  if (calibTool!=NULL)
49  {
50  calibTool->apply(yrpImgIn,yrpImgOut);
51 
52  for (int r =0; r <yrpImgOut.height(); r++)
53  {
54  for (int c=0; c<yrpImgOut.width(); c++)
55  {
56  unsigned char *pixel = yrpImgOut.getPixelAddress(c,r);
57  double mean = (1.0/3.0)*(pixel[0]+pixel[1]+pixel[2]);
58 
59  for(int i=0; i<3; i++)
60  {
61  double s=pixel[i]-mean;
62  double sn=currSat*s;
63  sn+=mean;
64 
65  if(sn<0.0)
66  sn=0.0;
67  else if(sn>255.0)
68  sn=255.0;
69 
70  pixel[i]=(unsigned char)sn;
71  }
72  }
73  }
74 
75  if (verbose)
76  yDebug("calibrated in %g [s]\n",Time::now()-t1);
77  }
78  else
79  {
80  yrpImgOut=yrpImgIn;
81 
82  if (verbose)
83  yDebug("just copied in %g [s]\n",Time::now()-t1);
84  }
85 
86  //timestamp propagation
87  yarp::os::Stamp stamp;
88  BufferedPort<ImageOf<PixelRgb> >::getEnvelope(stamp);
89  portImgOut->setEnvelope(stamp);
90 
91  portImgOut->writeStrict();
92  }
93 
94  t0=t;
95 }
96 
98 
99  _calibTool = NULL;
100 }
101 
103 
104 }
105 
106 bool CamCalibModule::configure(yarp::os::ResourceFinder &rf){
107 
108  string str = rf.check("name", Value("/camCalib"), "module name (string)").asString();
109  setName(str.c_str()); // modulePortName
110 
111  // pass configuration over to bottle
112  Bottle botConfig(rf.toString());
113  botConfig.setMonitor(rf.getMonitor());
114  // Load from configuration group ([<group_name>]), if group option present
115  Value *valGroup; // check assigns pointer to reference
116  if(botConfig.check("group", valGroup, "Configuration group to load module options from (string)."))
117  {
118  string strGroup = valGroup->asString();
119  // is group a valid bottle?
120  if (botConfig.check(strGroup)){
121  Bottle &group=botConfig.findGroup(strGroup,"Loading configuration from group " + strGroup);
122  botConfig.fromString(group.toString());
123  }
124  else{
125  yError() << "Group " << strGroup << " not found.";
126  return false;
127  }
128  }
129  else
130  {
131  yError ("There seem to be an error loading parameters (group section missing), stopping module");
132  return false;
133  }
134 
135  string calibToolName = botConfig.check("projection",
136  Value("pinhole"),
137  "Projection/mapping applied to calibrated image [pinhole|spherical] (string).").asString();
138 
139  _calibTool = CalibToolFactories::getPool().get(calibToolName.c_str());
140  if (_calibTool!=NULL) {
141  bool ok = _calibTool->open(botConfig);
142  if (!ok) {
143  delete _calibTool;
144  _calibTool = NULL;
145  return false;
146  }
147  }
148 
149  if (yarp::os::Network::exists(getName("/in")))
150  {
151  yWarning() << "port " << getName("/in") << " already in use";
152  }
153  if (yarp::os::Network::exists(getName("/out")))
154  {
155  yWarning() << "port " << getName("/out") << " already in use";
156  }
157  if (yarp::os::Network::exists(getName("/conf")))
158  {
159  yWarning() << "port " << getName("/conf") << " already in use";
160  }
161  _prtImgIn.setSaturation(rf.check("saturation",Value(1.0)).asFloat64());
162  _prtImgIn.open(getName("/in"));
163  _prtImgIn.setPointers(&_prtImgOut,_calibTool);
164  _prtImgIn.setVerbose(rf.check("verbose"));
165  _prtImgIn.useCallback();
166  _prtImgOut.open(getName("/out"));
167  _configPort.open(getName("/conf"));
168 
169  attach(_configPort);
170  fflush(stdout);
171 
172  return true;
173 }
174 
176  _prtImgIn.close();
177  _prtImgOut.close();
178  _configPort.close();
179  if (_calibTool != NULL){
180  _calibTool->close();
181  delete _calibTool;
182  _calibTool = NULL;
183  }
184  return true;
185 }
186 
188  _prtImgIn.interrupt();
189  _prtImgOut.interrupt();
190  _configPort.interrupt();
191  return true;
192 }
193 
195  return true;
196 }
197 
199  return 1.0;
200 }
201 
202 bool CamCalibModule::respond(const Bottle& command, Bottle& reply)
203 {
204  reply.clear();
205 
206  if (command.get(0).asString()=="quit")
207  {
208  reply.addString("quitting");
209  return false;
210  }
211  else if (command.get(0).asString()=="sat" || command.get(0).asString()=="saturation")
212  {
213  double satVal = command.get(1).asFloat64();
214  _prtImgIn.setSaturation(satVal);
215 
216  reply.addString("ok");
217  }
218  else
219  {
220  yError() << "command not known - type help for more info";
221  }
222  return true;
223 }
224 
225 
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::setSaturation
void setSaturation(double satVal)
Definition: CamCalibModule.cpp:30
CamCalibPort::CamCalibPort
CamCalibPort()
Definition: CamCalibModule.cpp:15
CamCalibModule::updateModule
virtual bool updateModule()
Definition: CamCalibModule.cpp:194
CamCalibModule::close
virtual bool close()
Definition: CamCalibModule.cpp:175
state::ok
@ ok
ICalibTool
Interface to calibrate and project input image based on camera's internal parameters and projection m...
Definition: ICalibTool.h:19
CamCalibModule::interruptModule
virtual bool interruptModule()
Definition: CamCalibModule.cpp:187
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
CamCalibModule::CamCalibModule
CamCalibModule()
Definition: CamCalibModule.cpp:97
CalibToolFactories::getPool
static CalibToolFactories & getPool()
Definition: CalibToolFactory.h:54
CamCalibModule::configure
virtual bool configure(yarp::os::ResourceFinder &rf)
Passes config on to CalibTool.
Definition: CamCalibModule.cpp:106