aruco/src/aruco/cameraparameters.cpp
Go to the documentation of this file.
1 /*****************************
2 Copyright 2011 Rafael Muñoz Salinas. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without modification, are
5 permitted provided that the following conditions are met:
6 
7  1. Redistributions of source code must retain the above copyright notice, this list of
8  conditions and the following disclaimer.
9 
10  2. Redistributions in binary form must reproduce the above copyright notice, this list
11  of conditions and the following disclaimer in the documentation and/or other materials
12  provided with the distribution.
13 
14 THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED
15 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
16 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR
17 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
18 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
19 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
20 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
21 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
22 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
23 
24 The views and conclusions contained in the software and documentation are those of the
25 authors and should not be interpreted as representing official policies, either expressed
26 or implied, of Rafael Muñoz Salinas.
27 ********************************/
28 #include <aruco/cameraparameters.h>
29 #include <fstream>
30 #include <iostream>
31 #include <opencv2/opencv.hpp>
32 using namespace std;
33 namespace aruco
34 {
35 
36 
37 CameraParameters::CameraParameters() {
38  CameraMatrix=cv::Mat();
39  Distorsion=cv::Mat();
40  CamSize=cv::Size(-1,-1);
41 }
47 CameraParameters::CameraParameters(cv::Mat cameraMatrix,cv::Mat distorsionCoeff,cv::Size size) throw(cv::Exception) {
48  setParams(cameraMatrix,distorsionCoeff,size);
49 }
52 CameraParameters::CameraParameters(const CameraParameters &CI) {
53  CI.CameraMatrix.copyTo(CameraMatrix);
54  CI.Distorsion.copyTo(Distorsion);
55  CamSize=CI.CamSize;
56 }
57 
60 CameraParameters & CameraParameters::operator=(const CameraParameters &CI) {
61  CI.CameraMatrix.copyTo(CameraMatrix);
62  CI.Distorsion.copyTo(Distorsion);
63  CamSize=CI.CamSize;
64  return *this;
65 }
68 void CameraParameters::setParams(cv::Mat cameraMatrix,cv::Mat distorsionCoeff,cv::Size size) throw(cv::Exception)
69 {
70  if (cameraMatrix.rows!=3 || cameraMatrix.cols!=3)
71  throw cv::Exception(9000,"invalid input cameraMatrix","CameraParameters::setParams",__FILE__,__LINE__);
72  cameraMatrix.convertTo(CameraMatrix,CV_32FC1);
73  if ( distorsionCoeff.total()<4 || distorsionCoeff.total()>=7 )
74  throw cv::Exception(9000,"invalid input distorsionCoeff","CameraParameters::setParams",__FILE__,__LINE__);
75  cv::Mat auxD;
76 
77  distorsionCoeff.convertTo( Distorsion,CV_32FC1);
78 
79 // Distorsion.create(1,4,CV_32FC1);
80 // for (int i=0;i<4;i++)
81 // Distorsion.ptr<float>(0)[i]=auxD.ptr<float>(0)[i];
82 
83  CamSize=size;
84 
85 }
86 
89 cv::Point3f CameraParameters::getCameraLocation(cv::Mat Rvec,cv::Mat Tvec)
90 {
91  cv::Mat m33(3,3,CV_32FC1);
92  cv::Rodrigues(Rvec, m33) ;
93 
94  cv::Mat m44=cv::Mat::eye(4,4,CV_32FC1);
95  for (int i=0;i<3;i++)
96  for (int j=0;j<3;j++)
97  m44.at<float>(i,j)=m33.at<float>(i,j);
98 
99  //now, add translation information
100  for (int i=0;i<3;i++)
101  m44.at<float>(i,3)=Tvec.at<float>(0,i);
102  //invert the matrix
103  m44.inv();
104  return cv::Point3f( m44.at<float>(0,0),m44.at<float>(0,1),m44.at<float>(0,2));
105 
106 }
107 
110 void CameraParameters::readFromFile(string path)throw(cv::Exception)
111 {
112 
113  ifstream file(path.c_str());
114  if (!file) throw cv::Exception(9005,"could not open file:"+path,"CameraParameters::readFromFile",__FILE__,__LINE__);
115 //Create the matrices
116  Distorsion=cv::Mat::zeros(4,1,CV_32FC1);
117  CameraMatrix=cv::Mat::eye(3,3,CV_32FC1);
118  char line[1024];
119  while (!file.eof()) {
120  file.getline(line,1024);
121  char cmd[20];
122  float fval;
123  if ( sscanf(line,"%s = %f",cmd,&fval)==2) {
124  string scmd(cmd);
125  if (scmd=="fx") CameraMatrix.at<float>(0,0)=fval;
126  else if (scmd=="cx") CameraMatrix.at<float>(0,2)=fval;
127  else if (scmd=="fy") CameraMatrix.at<float>(1,1)=fval;
128  else if (scmd=="cy") CameraMatrix.at<float>(1,2)=fval;
129  else if (scmd=="k1") Distorsion.at<float>(0,0)=fval;
130  else if (scmd=="k2") Distorsion.at<float>(1,0)=fval;
131  else if (scmd=="p1") Distorsion.at<float>(2,0)=fval;
132  else if (scmd=="p2") Distorsion.at<float>(3,0)=fval;
133  else if (scmd=="width") CamSize.width=fval;
134  else if (scmd=="height") CamSize.height=fval;
135  }
136  }
137 }
140 void CameraParameters::saveToFile(string path,bool inXML)throw(cv::Exception)
141 {
142  if (!isValid()) throw cv::Exception(9006,"invalid object","CameraParameters::saveToFile",__FILE__,__LINE__);
143  if (!inXML) {
144  ofstream file(path.c_str());
145  if (!file) throw cv::Exception(9006,"could not open file:"+path,"CameraParameters::saveToFile",__FILE__,__LINE__);
146  file<<"# Aruco 1.0 CameraParameters"<<endl;
147  file<<"fx = "<<CameraMatrix.at<float>(0,0)<<endl;
148  file<<"cx = "<<CameraMatrix.at<float>(0,2)<<endl;
149  file<<"fy = "<<CameraMatrix.at<float>(1,1)<<endl;
150  file<<"cy = "<<CameraMatrix.at<float>(1,2)<<endl;
151  file<<"k1 = "<<Distorsion.at<float>(0,0)<<endl;
152  file<<"k2 = "<<Distorsion.at<float>(1,0)<<endl;
153  file<<"p1 = "<<Distorsion.at<float>(2,0)<<endl;
154  file<<"p2 = "<<Distorsion.at<float>(3,0)<<endl;
155  file<<"width = "<<CamSize.width<<endl;
156  file<<"height = "<<CamSize.height<<endl;
157  }
158  else {
159  cv::FileStorage fs(path,cv::FileStorage::WRITE);
160  fs<<"image_width" << CamSize.width;
161  fs<<"image_height" << CamSize.height;
162  fs<<"camera_matrix" << CameraMatrix;
163  fs<<"distortion_coefficients" <<Distorsion;
164  }
165 }
166 
169 void CameraParameters::resize(cv::Size size)throw(cv::Exception)
170 {
171  if (!isValid()) throw cv::Exception(9007,"invalid object","CameraParameters::resize",__FILE__,__LINE__);
172  if (size==CamSize) return;
173  //now, read the camera size
174  //resize the camera parameters to fit this image size
175  float AxFactor= float(size.width)/ float(CamSize.width);
176  float AyFactor= float(size.height)/ float(CamSize.height);
177  CameraMatrix.at<float>(0,0)*=AxFactor;
178  CameraMatrix.at<float>(0,2)*=AxFactor;
179  CameraMatrix.at<float>(1,1)*=AyFactor;
180  CameraMatrix.at<float>(1,2)*=AyFactor;
181 }
182 
183 /****
184  *
185  *
186  *
187  *
188  */
189 void CameraParameters::readFromXMLFile(string filePath)throw(cv::Exception)
190 {
191  cv::FileStorage fs(filePath, cv::FileStorage::READ);
192  int w=-1,h=-1;
193  cv::Mat MCamera,MDist;
194 
195  fs["image_width"] >> w;
196  fs["image_height"] >> h;
197  fs["distortion_coefficients"] >> MDist;
198  fs["camera_matrix"] >> MCamera;
199 
200  if (MCamera.cols==0 || MCamera.rows==0)throw cv::Exception(9007,"File :"+filePath+" does not contains valid camera matrix","CameraParameters::readFromXML",__FILE__,__LINE__);
201  if (w==-1 || h==0)throw cv::Exception(9007,"File :"+filePath+" does not contains valid camera dimensions","CameraParameters::readFromXML",__FILE__,__LINE__);
202 
203  if (MCamera.type()!=CV_32FC1) MCamera.convertTo(CameraMatrix,CV_32FC1);
204  else CameraMatrix=MCamera;
205 
206  if (MDist.total()<4) throw cv::Exception(9007,"File :"+filePath+" does not contains valid distortion_coefficients","CameraParameters::readFromXML",__FILE__,__LINE__);
207  //convert to 32 and get the 4 first elements only
208  cv::Mat mdist32;
209  MDist.convertTo(mdist32,CV_32FC1);
210  Distorsion.create(1,4,CV_32FC1);
211  for (int i=0;i<4;i++)
212  Distorsion.ptr<float>(0)[i]=mdist32.ptr<float>(0)[i];
213 
214  CamSize.width=w;
215  CamSize.height=h;
216 }
217 /****
218  *
219  */
220 void CameraParameters::glGetProjectionMatrix( cv::Size orgImgSize, cv::Size size,double proj_matrix[16],double gnear,double gfar,bool invert )throw(cv::Exception)
221 {
222 
223  if (cv::countNonZero(Distorsion)!=0) std::cerr<< "CameraParameters::glGetProjectionMatrix :: The camera has distortion coefficients " <<__FILE__<<" "<<__LINE__<<endl;
224  if (isValid()==false) throw cv::Exception(9100,"invalid camera parameters","CameraParameters::glGetProjectionMatrix",__FILE__,__LINE__);
225 
226  //Deterime the rsized info
227  double Ax=double(size.width)/double(orgImgSize.width);
228  double Ay=double(size.height)/double(orgImgSize.height);
229  double _fx=CameraMatrix.at<float>(0,0)*Ax;
230  double _cx=CameraMatrix.at<float>(0,2)*Ax;
231  double _fy=CameraMatrix.at<float>(1,1)*Ay;
232  double _cy=CameraMatrix.at<float>(1,2)*Ay;
233  double cparam[3][4] =
234  {
235  {
236  _fx, 0, _cx, 0
237  },
238  {0, _fy, _cy, 0},
239  {0, 0, 1, 0}
240  };
241 
242  argConvGLcpara2( cparam, size.width, size.height, gnear, gfar, proj_matrix, invert );
243 
244 }
245 
246 /*******************
247  *
248  *
249  *******************/
250 double CameraParameters::norm( double a, double b, double c )
251 {
252  return( sqrt( a*a + b*b + c*c ) );
253 }
254 
255 /*******************
256  *
257  *
258  *******************/
259 
260 double CameraParameters::dot( double a1, double a2, double a3,
261  double b1, double b2, double b3 )
262 {
263  return( a1 * b1 + a2 * b2 + a3 * b3 );
264 }
265 
266 /*******************
267  *
268  *
269  *******************/
270 
271 void CameraParameters::argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert )throw(cv::Exception)
272 {
273 
274  double icpara[3][4];
275  double trans[3][4];
276  double p[3][3], q[4][4];
277  int i, j;
278 
279  cparam[0][2] *= -1.0;
280  cparam[1][2] *= -1.0;
281  cparam[2][2] *= -1.0;
282 
283  if ( arParamDecompMat(cparam, icpara, trans) < 0 )
284  throw cv::Exception(9002,"parameter error","MarkerDetector::argConvGLcpara2",__FILE__,__LINE__);
285 
286  for ( i = 0; i < 3; i++ )
287  {
288  for ( j = 0; j < 3; j++ )
289  {
290  p[i][j] = icpara[i][j] / icpara[2][2];
291  }
292  }
293  q[0][0] = (2.0 * p[0][0] / width);
294  q[0][1] = (2.0 * p[0][1] / width);
295  q[0][2] = ((2.0 * p[0][2] / width) - 1.0);
296  q[0][3] = 0.0;
297 
298  q[1][0] = 0.0;
299  q[1][1] = (2.0 * p[1][1] / height);
300  q[1][2] = ((2.0 * p[1][2] / height) - 1.0);
301  q[1][3] = 0.0;
302 
303  q[2][0] = 0.0;
304  q[2][1] = 0.0;
305  q[2][2] = (gfar + gnear)/(gfar - gnear);
306  q[2][3] = -2.0 * gfar * gnear / (gfar - gnear);
307 
308  q[3][0] = 0.0;
309  q[3][1] = 0.0;
310  q[3][2] = 1.0;
311  q[3][3] = 0.0;
312 
313  for ( i = 0; i < 4; i++ )
314  {
315  for ( j = 0; j < 3; j++ )
316  {
317  m[i+j*4] = q[i][0] * trans[0][j]
318  + q[i][1] * trans[1][j]
319  + q[i][2] * trans[2][j];
320  }
321  m[i+3*4] = q[i][0] * trans[0][3]
322  + q[i][1] * trans[1][3]
323  + q[i][2] * trans[2][3]
324  + q[i][3];
325  }
326 
327  if (!invert)
328  {
329  m[13]=-m[13] ;
330  m[1]=-m[1];
331  m[5]=-m[5];
332  m[9]=-m[9];
333  }
334 
335 }
336 /*******************
337  *
338  *
339  *******************/
340 
341 int CameraParameters::arParamDecompMat( double source[3][4], double cpara[3][4], double trans[3][4] )throw(cv::Exception)
342 {
343  int r, c;
344  double Cpara[3][4];
345  double rem1, rem2, rem3;
346 
347  if ( source[2][3] >= 0 )
348  {
349  for ( r = 0; r < 3; r++ )
350  {
351  for ( c = 0; c < 4; c++ )
352  {
353  Cpara[r][c] = source[r][c];
354  }
355  }
356  }
357  else
358  {
359  for ( r = 0; r < 3; r++ )
360  {
361  for ( c = 0; c < 4; c++ )
362  {
363  Cpara[r][c] = -(source[r][c]);
364  }
365  }
366  }
367 
368  for ( r = 0; r < 3; r++ )
369  {
370  for ( c = 0; c < 4; c++ )
371  {
372  cpara[r][c] = 0.0;
373  }
374  }
375  cpara[2][2] = norm( Cpara[2][0], Cpara[2][1], Cpara[2][2] );
376  trans[2][0] = Cpara[2][0] / cpara[2][2];
377  trans[2][1] = Cpara[2][1] / cpara[2][2];
378  trans[2][2] = Cpara[2][2] / cpara[2][2];
379  trans[2][3] = Cpara[2][3] / cpara[2][2];
380 
381  cpara[1][2] = dot( trans[2][0], trans[2][1], trans[2][2],
382  Cpara[1][0], Cpara[1][1], Cpara[1][2] );
383  rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0];
384  rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1];
385  rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2];
386  cpara[1][1] = norm( rem1, rem2, rem3 );
387  trans[1][0] = rem1 / cpara[1][1];
388  trans[1][1] = rem2 / cpara[1][1];
389  trans[1][2] = rem3 / cpara[1][1];
390 
391  cpara[0][2] = dot( trans[2][0], trans[2][1], trans[2][2],
392  Cpara[0][0], Cpara[0][1], Cpara[0][2] );
393  cpara[0][1] = dot( trans[1][0], trans[1][1], trans[1][2],
394  Cpara[0][0], Cpara[0][1], Cpara[0][2] );
395  rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0] - cpara[0][2]*trans[2][0];
396  rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1] - cpara[0][2]*trans[2][1];
397  rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];
398  cpara[0][0] = norm( rem1, rem2, rem3 );
399  trans[0][0] = rem1 / cpara[0][0];
400  trans[0][1] = rem2 / cpara[0][0];
401  trans[0][2] = rem3 / cpara[0][0];
402 
403  trans[1][3] = (Cpara[1][3] - cpara[1][2]*trans[2][3]) / cpara[1][1];
404  trans[0][3] = (Cpara[0][3] - cpara[0][1]*trans[1][3]
405  - cpara[0][2]*trans[2][3]) / cpara[0][0];
406 
407  for ( r = 0; r < 3; r++ )
408  {
409  for ( c = 0; c < 3; c++ )
410  {
411  cpara[r][c] /= cpara[2][2];
412  }
413  }
414 
415  return 0;
416 }
417 
418 
419 /******
420  *
421  */
422 void CameraParameters::OgreGetProjectionMatrix(cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar, bool invert) throw(cv::Exception)
423 {
424  double temp_matrix[16];
425  (*this).glGetProjectionMatrix(orgImgSize, size, temp_matrix, gnear, gfar, invert);
426  proj_matrix[0]=-temp_matrix[0];
427  proj_matrix[1]=-temp_matrix[4];
428  proj_matrix[2]=-temp_matrix[8];
429  proj_matrix[3]=temp_matrix[12];
430 
431  proj_matrix[4]=-temp_matrix[1];
432  proj_matrix[5]=-temp_matrix[5];
433  proj_matrix[6]=-temp_matrix[9];
434  proj_matrix[7]=temp_matrix[13];
435 
436  proj_matrix[8]=-temp_matrix[2];
437  proj_matrix[9]=-temp_matrix[6];
438  proj_matrix[10]=-temp_matrix[10];
439  proj_matrix[11]=temp_matrix[14];
440 
441  proj_matrix[12]=-temp_matrix[3];
442  proj_matrix[13]=-temp_matrix[7];
443  proj_matrix[14]=-temp_matrix[11];
444  proj_matrix[15]=temp_matrix[15];
445 }
446 
447 
448 
449 
450 };
cmd
string cmd
dot
TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion &q1, const Quaternion &q2)
w
TFSIMD_FORCE_INLINE const tfScalar & w() const

lidar_camera_calibration
Author(s):
autogenerated on Sat Feb 6 2021 03:39:37