iCub-main
Loading...
Searching...
No Matches
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
11using namespace std;
12using namespace yarp::os;
13using namespace yarp::sig;
14using namespace yarp::cv;
15
16PinholeCalibTool::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
27PinholeCalibTool::~PinholeCalibTool(){
28
29}
30
31bool 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
44bool PinholeCalibTool::open(Searchable &config){
45 return configure(config);
46}
47
48void 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
53bool 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
138bool 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 _needInit = false;
186 return true;
187}
188
189void PinholeCalibTool::apply(const ImageOf<PixelRgb> & in, ImageOf<PixelRgb> & out){
190
191 CvSize inSize = cvSize(in.width(),in.height());
192
193 // check if reallocation required
194 if ( inSize.width != _oldImgSize.width ||
195 inSize.height != _oldImgSize.height ||
196 _needInit)
197 init(inSize,_calibImgSize);
198
199 cv::Mat outMat;
200 cv::remap( toCvMat(const_cast<ImageOf<PixelRgb>&>(in)), outMat,
201 cv::cvarrToMat(_mapUndistortX), cv::cvarrToMat(_mapUndistortY),
202 cv::INTER_LINEAR );
203 out=fromCvMat<PixelRgb>(outMat);
204
205 // painting crosshair at calibration center
206 if (_drawCenterCross){
207 yarp::sig::PixelRgb pix = yarp::sig::PixelRgb(255,255,255);
208 yarp::sig::draw::addCrossHair(out, pix, (int)CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 0, 2),
209 (int)CV_MAT_ELEM( *_intrinsic_matrix_scaled , float, 1, 2),
210 10);
211 }
212
213 // buffering old image size
214 _oldImgSize.width = inSize.width;
215 _oldImgSize.height = inSize.height;
216}
217
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::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 Fri Sep 26 2025 14:18:32 for iCub-main by doxygen 1.9.8