iCub-main
PinholeCalibTool.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2007 Lijin Aryananda, Jonas Ruesch
3  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
4  *
5  */
6 
7 #include <utility>
8 #include <yarp/cv/Cv.h>
9 #include <iCub/PinholeCalibTool.h>
10 
11 using namespace std;
12 using namespace yarp::os;
13 using namespace yarp::sig;
14 using namespace yarp::cv;
15 
16 PinholeCalibTool::PinholeCalibTool(){
17  _mapUndistortX = NULL;
18  _mapUndistortY = NULL;
19  _intrinsic_matrix = cvCreateMat(3,3, CV_32F);
20  _intrinsic_matrix_scaled = cvCreateMat(3,3, CV_32F);
21  _distortion_coeffs = cvCreateMat(1, 4, CV_32F);
22  _oldImgSize.width = -1;
23  _oldImgSize.height = -1;
24  _needInit = true;
25 }
26 
27 PinholeCalibTool::~PinholeCalibTool(){
28 
29 }
30 
31 bool PinholeCalibTool::close(){
32  if (_mapUndistortX != NULL)
33  cvReleaseImage(&_mapUndistortX);
34  _mapUndistortX = NULL;
35  if (_mapUndistortY != NULL)
36  cvReleaseImage(&_mapUndistortY);
37  _mapUndistortY = NULL;
38  cvReleaseMat(&_intrinsic_matrix);
39  cvReleaseMat(&_intrinsic_matrix_scaled);
40  cvReleaseMat(&_distortion_coeffs);
41  return true;
42 }
43 
44 bool PinholeCalibTool::open(Searchable &config){
45  return configure(config);
46 }
47 
48 void PinholeCalibTool::stopConfig( string val ){
49 
50  fprintf(stdout,"There seem to be an error loading parameters \"%s\", stopping module\n", val.c_str());
51 }
52 
53 bool PinholeCalibTool::configure (Searchable &config){
54 
55  _calibImgSize.width = config.check("w",
56  Value(320),
57  "Image width for which calibration parameters were calculated (int)").asInt32();
58 
59  _calibImgSize.height = config.check("h",
60  Value(240),
61  "Image height for which calibration parameters were calculated (int)").asInt32();
62 
63  _drawCenterCross = config.check("drawCenterCross",
64  Value(0),
65  "Draw a cross at calibration center (int [0|1]).").asInt32()!=0;
66 
67 
68  CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 0) = (float)config.check("fx",
69  Value(320.0),
70  "Focal length x (double)").asFloat64();
71 
72  CV_MAT_ELEM( *_intrinsic_matrix, float, 0, 1) = 0.0f;
73  CV_MAT_ELEM( *_intrinsic_matrix, float, 0, 2) = (float)config.check("cx",
74  Value(160.0),
75  "Principal point x (double)").asFloat64();
76  CV_MAT_ELEM( *_intrinsic_matrix, float, 1, 0) = 0.0f;
77  CV_MAT_ELEM( *_intrinsic_matrix, float, 1, 1) = (float)config.check("fy",
78  Value(320.0),
79  "Focal length y (double)").asFloat64();
80  CV_MAT_ELEM( *_intrinsic_matrix, float, 1, 2) = (float)config.check("cy",
81  Value(120.0),
82  "Principal point y (double)").asFloat64();
83  CV_MAT_ELEM( *_intrinsic_matrix, float, 2, 0) = 0.0f;
84  CV_MAT_ELEM( *_intrinsic_matrix, float, 2, 1) = 0.0f;
85  CV_MAT_ELEM( *_intrinsic_matrix, float, 2, 2) = 1.0f;
86 
87 
88  //check to see if the value is read correctly without caring about the default values.
89  if ( !config.check("drawCenterCross") ) { stopConfig("drawCenterCross"); return false; }
90  if ( !config.check("w") ) { stopConfig("w"); return false;}
91  if ( !config.check("h") ) { stopConfig("h"); return false;}
92  if ( !config.check("fx") ) { stopConfig("fx"); return false;}
93  if ( !config.check("fy") ) { stopConfig("fy"); return false;}
94  if ( !config.check("cx") ) { stopConfig("cx"); return false;}
95  if ( !config.check("cy") ) { stopConfig("cy"); return false;}
96  if ( !config.check("k1") ) { stopConfig("k1"); return false;}
97  if ( !config.check("k2") ) { stopConfig("k2"); return false;}
98  if ( !config.check("p1") ) { stopConfig("p1"); return false;}
99  if ( !config.check("p2") ) { stopConfig("p2"); return false;}
100 
101 
102  fprintf(stdout,"fx=%g\n",config.find("fx").asFloat64());
103  fprintf(stdout,"fy=%g\n",config.find("fy").asFloat64());
104  fprintf(stdout,"cx=%g\n",config.find("cx").asFloat64());
105  fprintf(stdout,"cy=%g\n",config.find("cy").asFloat64());
106 
107 
108  // copy to scaled matrix ;)
109  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 0);
110  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 1);
111  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 2);
112  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 0);
113  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 1);
114  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 2);
115  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 0);
116  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 1);
117  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 2);
118 
119  /* init the distortion coeffs */
120  CV_MAT_ELEM( *_distortion_coeffs, float, 0, 0) = (float)config.check("k1",
121  Value(0.0),
122  "Radial distortion 1(double)").asFloat64();
123  CV_MAT_ELEM( *_distortion_coeffs, float, 0, 1) = (float)config.check("k2",
124  Value(0.0),
125  "Radial distortion 2(double)").asFloat64();
126  CV_MAT_ELEM( *_distortion_coeffs, float, 0, 2) = (float)config.check("p1",
127  Value(0.0),
128  "Tangential distortion 1(double)").asFloat64();
129  CV_MAT_ELEM( *_distortion_coeffs, float, 0, 3) = (float)config.check("p2",
130  Value(0.0),
131  "Tangential distortion 2(double)").asFloat64();
132  _needInit = true;
133 
134  return true;
135 }
136 
137 
138 bool PinholeCalibTool::init(CvSize currImgSize, CvSize calibImgSize){
139 
140  if (_mapUndistortX != NULL)
141  cvReleaseImage(&_mapUndistortX);
142  _mapUndistortX = NULL;
143  if (_mapUndistortY != NULL)
144  cvReleaseImage(&_mapUndistortY);
145  _mapUndistortY = NULL;
146 
147 
148  // Scale the intrinsics if required:
149  // if current image size is not the same as the size for
150  // which calibration parameters are specified we need to
151  // scale the intrinsic matrix components.
152  if (currImgSize.width != calibImgSize.width ||
153  currImgSize.height != calibImgSize.height){
154  float scaleX = (float)currImgSize.width / (float)calibImgSize.width;
155  float scaleY = (float)currImgSize.height / (float)calibImgSize.height;
156  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 0) * scaleX;
157  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 1);
158  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 2) * scaleX;
159  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 0);
160  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 1) * scaleY;
161  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 2) * scaleY;
162  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 0);
163  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 1);
164  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 2);
165  }
166  else{
167  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 0);
168  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 1);
169  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 0, 2);
170  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 0);
171  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 1);
172  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 1, 2);
173  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 0) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 0);
174  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 1) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 1);
175  CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 2, 2) = CV_MAT_ELEM( *_intrinsic_matrix , float, 2, 2);
176  }
177 
178  _mapUndistortX = cvCreateImage(currImgSize, IPL_DEPTH_32F, 1);
179  _mapUndistortY = cvCreateImage(currImgSize, IPL_DEPTH_32F, 1);
180 
181  /* init the undistortion matrices */
182  cv::initUndistortRectifyMap(cv::cvarrToMat(_intrinsic_matrix_scaled), cv::cvarrToMat(_distortion_coeffs), cv::Mat(),
183  cv::cvarrToMat(_intrinsic_matrix_scaled), cv::Size(currImgSize.width, currImgSize.height),
184  CV_32FC1,cv::cvarrToMat(_mapUndistortX), cv::cvarrToMat(_mapUndistortY));
185 
186  _needInit = false;
187  return true;
188 }
189 
190 void PinholeCalibTool::apply(const ImageOf<PixelRgb> & in, ImageOf<PixelRgb> & out){
191 
192  CvSize inSize = cvSize(in.width(),in.height());
193 
194  // check if reallocation required
195  if ( inSize.width != _oldImgSize.width ||
196  inSize.height != _oldImgSize.height ||
197  _needInit)
198  init(inSize,_calibImgSize);
199 
200  cv::Mat outMat;
201  cv::remap( toCvMat(const_cast<ImageOf<PixelRgb>&>(in)), outMat,
202  cv::cvarrToMat(_mapUndistortX), cv::cvarrToMat(_mapUndistortY),
203  cv::INTER_LINEAR );
204  out=fromCvMat<PixelRgb>(outMat);
205 
206  // painting crosshair at calibration center
207  if (_drawCenterCross){
208  yarp::sig::PixelRgb pix = yarp::sig::PixelRgb(255,255,255);
209  yarp::sig::draw::addCrossHair(out, pix, (int)CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 2),
210  (int)CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 2),
211  10);
212  }
213 
214  // buffering old image size
215  _oldImgSize.width = inSize.width;
216  _oldImgSize.height = inSize.height;
217 }
218 
PinholeCalibTool::configure
virtual bool configure(yarp::os::Searchable &config)
The PinholeCalibTool expects a configuration file as show below.
PinholeCalibTool::close
virtual bool close()
Definition: PinholeCalibTool.cpp:31
PinholeCalibTool::apply
void apply(const yarp::sig::ImageOf< yarp::sig::PixelRgb > &in, yarp::sig::ImageOf< yarp::sig::PixelRgb > &out)
Apply calibration, in = rgb image, out = calibrated rgb image.
PinholeCalibTool::~PinholeCalibTool
virtual ~PinholeCalibTool()
Definition: PinholeCalibTool.cpp:27
PinholeCalibTool::open
virtual bool open(yarp::os::Searchable &config)
open() passes on to configure()
PinholeCalibTool::stopConfig
void stopConfig(std::string val)
Stop module if there is a wrong value.
Definition: PinholeCalibTool.cpp:48
fprintf
fprintf(fid,'\n')
out
out
Definition: sine.m:8
Generated on Wed Nov 13 2024 21:11:41 for iCub-main by doxygen 1.9.1