iCub-main
SphericalCalibTool.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 <utility>
10 #include <yarp/cv/Cv.h>
11 #include <iCub/SphericalCalibTool.h>
12 #include <stdio.h>
13 
14 using namespace std;
15 using namespace yarp::os;
16 using namespace yarp::sig;
17 using namespace yarp::cv;
18 
20  _mapX = NULL;
21  _mapY = NULL;
22  _oldImgSize.width = -1;
23  _oldImgSize.height = -1;
24  _needInit = true;
25 }
26 
28 
29 }
30 
31 bool SphericalCalibTool::open(Searchable &config){
32  return configure(config);
33 }
34 
36  if (_mapX != NULL)
37  cvReleaseImage(&_mapX);
38  _mapX = NULL;
39  if (_mapY != NULL)
40  cvReleaseImage(&_mapY);
41  _mapY = NULL;
42  return true;
43 }
44 
46 
47  fprintf(stdout,"There seem to be an error loading parameters \"%s\", stopping module\n", val.c_str());
48 }
49 
50 bool SphericalCalibTool::configure (Searchable &config){
51 
52  // Defaults will correspond to a view field of 90 deg.
53  _calibImgSize.width = config.check("w",
54  Value(320),
55  "Image width for which calibration parameters were calculated (int)").asInt32();
56  _calibImgSize.height = config.check("h",
57  Value(240),
58  "Image height for which calibration parameters were calculated (int)").asInt32();
59  _drawCenterCross = config.check("drawCenterCross",
60  Value(0),
61  "Draw a cross at calibration center (int [0|1]).").asInt32()!=0;
62 
63  _fx = config.check("fx", Value(320.0), "Focal distance (on horizontal pixel size units) (double)").asFloat64();
64  _fy = config.check("fy", Value(240.0), "Focal distance (on vertical pixel size units) (double)").asFloat64();
65  _cx = config.check("cx", Value(320.0), "Image center (on horizontal pixel size units) (double)").asFloat64();
66  _cy = config.check("cy", Value(240.0), "Image center (on vertical pixel size units) (double)").asFloat64();
67  _k1 = config.check("k1", Value(0.0), "Radial distortion (first parameter) (double)").asFloat64();
68  _k2 = config.check("k2", Value(0.0), "Radial distortion (second parameter) (double)").asFloat64();
69  _p1 = config.check("p1", Value(0.0), "Tangential distortion (first parameter) (double)").asFloat64();
70  _p2 = config.check("p2", Value(0.0), "Tangential distortion (second parameter) (double)").asFloat64();
71 
72  //check to see if the value is read correctly without caring about the default values.
73  if ( !config.check("drawCenterCross") ) { stopConfig("drawCenterCross"); return false; }
74  if ( !config.check("w") ) { stopConfig("w"); return false;}
75  if ( !config.check("h") ) { stopConfig("h"); return false;}
76  if ( !config.check("fx") ) { stopConfig("fx"); return false;}
77  if ( !config.check("fy") ) { stopConfig("fy"); return false;}
78  if ( !config.check("cx") ) { stopConfig("cx"); return false;}
79  if ( !config.check("cy") ) { stopConfig("cy"); return false;}
80  if ( !config.check("k1") ) { stopConfig("k1"); return false;}
81  if ( !config.check("k2") ) { stopConfig("k2"); return false;}
82  if ( !config.check("p1") ) { stopConfig("p1"); return false;}
83  if ( !config.check("p2") ) { stopConfig("p2"); return false;}
84 
85  _fx_scaled = _fx;
86  _fy_scaled = _fy;
87  _cx_scaled = _cx;
88  _cy_scaled = _cy;
89 
90  _needInit = true;
91 
92  return true;
93 }
94 
95 
96 bool SphericalCalibTool::init(CvSize currImgSize, CvSize calibImgSize){
97 
98  if (_mapX != NULL)
99  cvReleaseImage(&_mapX);
100  _mapX = NULL;
101  if (_mapY != NULL)
102  cvReleaseImage(&_mapY);
103  _mapY = NULL;
104 
105  // Scale the intrinsics if required:
106  // if current image size is not the same as the size for
107  // which calibration parameters are specified we need to
108  // scale the intrinsic matrix components.
109  if (currImgSize.width != calibImgSize.width ||
110  currImgSize.height != calibImgSize.height){
111  float scaleX = (float)currImgSize.width / (float)calibImgSize.width;
112  float scaleY = (float)currImgSize.height / (float)calibImgSize.height;
113  _fx_scaled = _fx * scaleX;
114  _fy_scaled = _fy * scaleY;
115  _cx_scaled = _cx * scaleX;
116  _cy_scaled = _cy * scaleY;
117  }
118  else{
119  _fx_scaled = _fx;
120  _fy_scaled = _fy;
121  _cx_scaled = _cx;
122  _cy_scaled = _cy;
123  }
124 
125  _mapX = cvCreateImage(currImgSize, IPL_DEPTH_32F, 1);
126  _mapY = cvCreateImage(currImgSize, IPL_DEPTH_32F, 1);
127 
128 
129 
130  if(!compute_sp_map(currImgSize.height, currImgSize.width,
131  currImgSize.height, currImgSize.width,
132  _fx_scaled, _fy_scaled, _cx_scaled, _cy_scaled,
133  _k1, _k2, _p1, _p2,
134  (float*)_mapX->imageData, (float*)_mapY->imageData))
135  return false;
136 
137  _needInit = false;
138  return true;
139 }
140 
141 void SphericalCalibTool::apply(const ImageOf<PixelRgb> & in, ImageOf<PixelRgb> & out){
142 
143  CvSize inSize = cvSize(in.width(),in.height());
144 
145  // check if reallocation required
146  if ( inSize.width != _oldImgSize.width ||
147  inSize.height != _oldImgSize.height ||
148  _needInit)
149  init(inSize,_calibImgSize);
150 
151  out.resize(inSize.width, inSize.height);
152 
153  cv::Mat outMat=toCvMat(out);
154  cv::remap( toCvMat(const_cast<ImageOf<PixelRgb>&>(in)), outMat,
155  cv::cvarrToMat(_mapX), cv::cvarrToMat(_mapY),
156  cv::INTER_LINEAR );
157  out=fromCvMat<PixelRgb>(outMat);
158 
159  // painting crosshair at calibration center
160  if (_drawCenterCross){
161  yarp::sig::PixelRgb pix = yarp::sig::PixelRgb(255,255,255);
162  yarp::sig::draw::addCrossHair(out, pix, (int)_cx_scaled, (int)_cy_scaled, 10);
163  }
164 
165  // buffering old image size
166  _oldImgSize.width = inSize.width;
167  _oldImgSize.height = inSize.height;
168 }
SphericalCalibTool::close
virtual bool close()
Definition: SphericalCalibTool.cpp:35
compute_sp_map
bool compute_sp_map(int input_lines, int input_cols, int output_lines, int output_cols, double fx, double fy, double cx, double cy, double k1, double k2, double p1, double p2, float *mapx, float *mapy)
Definition: spherical_projection.cpp:71
SphericalCalibTool::apply
void apply(const yarp::sig::ImageOf< yarp::sig::PixelRgb > &in, yarp::sig::ImageOf< yarp::sig::PixelRgb > &out)
out
out
Definition: sine.m:8
SphericalCalibTool::stopConfig
void stopConfig(std::string val)
Stop module if there is an unread value.
Definition: SphericalCalibTool.cpp:45
SphericalCalibTool::open
virtual bool open(yarp::os::Searchable &config)
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
SphericalCalibTool::~SphericalCalibTool
virtual ~SphericalCalibTool()
Definition: SphericalCalibTool.cpp:27
SphericalCalibTool::configure
virtual bool configure(yarp::os::Searchable &config)
fprintf
fprintf(fid,'\n')
SphericalCalibTool::SphericalCalibTool
SphericalCalibTool()
Definition: SphericalCalibTool.cpp:19