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 "cameraparameters.h"
29 #include <fstream>
30 #include <iostream>
31 #include <opencv2/core/core.hpp>
32 #include <opencv2/calib3d/calib3d.hpp>
33 using namespace std;
34 namespace aruco {
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  if (cameraMatrix.rows != 3 || cameraMatrix.cols != 3)
70  throw cv::Exception(9000, "invalid input cameraMatrix", "CameraParameters::setParams", __FILE__, __LINE__);
71  cameraMatrix.convertTo(CameraMatrix, CV_32FC1);
72  if (distorsionCoeff.total() < 4 || distorsionCoeff.total() >= 7)
73  throw cv::Exception(9000, "invalid input distorsionCoeff", "CameraParameters::setParams", __FILE__, __LINE__);
74  cv::Mat auxD;
75 
76  distorsionCoeff.convertTo(Distorsion, CV_32FC1);
77 
78  // Distorsion.create(1,4,CV_32FC1);
79  // for (int i=0;i<4;i++)
80  // Distorsion.ptr<float>(0)[i]=auxD.ptr<float>(0)[i];
81 
82  CamSize = size;
83 }
84 
87 cv::Point3f CameraParameters::getCameraLocation(cv::Mat Rvec, cv::Mat Tvec) {
88  cv::Mat m33(3, 3, CV_32FC1);
89  cv::Rodrigues(Rvec, m33);
90 
91  cv::Mat m44 = cv::Mat::eye(4, 4, CV_32FC1);
92  for (int i = 0; i < 3; i++)
93  for (int j = 0; j < 3; j++)
94  m44.at< float >(i, j) = m33.at< float >(i, j);
95 
96  // now, add translation information
97  for (int i = 0; i < 3; i++)
98  m44.at< float >(i, 3) = Tvec.at< float >(0, i);
99  // invert the matrix
100  m44.inv();
101  return cv::Point3f(m44.at< float >(0, 0), m44.at< float >(0, 1), m44.at< float >(0, 2));
102 }
103 
106 void CameraParameters::readFromFile(string path) throw(cv::Exception) {
107 
108  ifstream file(path.c_str());
109  if (!file)
110  throw cv::Exception(9005, "could not open file:" + path, "CameraParameters::readFromFile", __FILE__, __LINE__);
111  // Create the matrices
112  Distorsion = cv::Mat::zeros(4, 1, CV_32FC1);
113  CameraMatrix = cv::Mat::eye(3, 3, CV_32FC1);
114  char line[1024];
115  while (!file.eof()) {
116  file.getline(line, 1024);
117  char cmd[20];
118  float fval;
119  if (sscanf(line, "%s = %f", cmd, &fval) == 2) {
120  string scmd(cmd);
121  if (scmd == "fx")
122  CameraMatrix.at< float >(0, 0) = fval;
123  else if (scmd == "cx")
124  CameraMatrix.at< float >(0, 2) = fval;
125  else if (scmd == "fy")
126  CameraMatrix.at< float >(1, 1) = fval;
127  else if (scmd == "cy")
128  CameraMatrix.at< float >(1, 2) = fval;
129  else if (scmd == "k1")
130  Distorsion.at< float >(0, 0) = fval;
131  else if (scmd == "k2")
132  Distorsion.at< float >(1, 0) = fval;
133  else if (scmd == "p1")
134  Distorsion.at< float >(2, 0) = fval;
135  else if (scmd == "p2")
136  Distorsion.at< float >(3, 0) = fval;
137  else if (scmd == "width")
138  CamSize.width = fval;
139  else if (scmd == "height")
140  CamSize.height = fval;
141  }
142  }
143 }
146 void CameraParameters::saveToFile(string path, bool inXML) throw(cv::Exception) {
147  if (!isValid())
148  throw cv::Exception(9006, "invalid object", "CameraParameters::saveToFile", __FILE__, __LINE__);
149  if (!inXML) {
150  ofstream file(path.c_str());
151  if (!file)
152  throw cv::Exception(9006, "could not open file:" + path, "CameraParameters::saveToFile", __FILE__, __LINE__);
153  file << "# Aruco 1.0 CameraParameters" << endl;
154  file << "fx = " << CameraMatrix.at< float >(0, 0) << endl;
155  file << "cx = " << CameraMatrix.at< float >(0, 2) << endl;
156  file << "fy = " << CameraMatrix.at< float >(1, 1) << endl;
157  file << "cy = " << CameraMatrix.at< float >(1, 2) << endl;
158  file << "k1 = " << Distorsion.at< float >(0, 0) << endl;
159  file << "k2 = " << Distorsion.at< float >(1, 0) << endl;
160  file << "p1 = " << Distorsion.at< float >(2, 0) << endl;
161  file << "p2 = " << Distorsion.at< float >(3, 0) << endl;
162  file << "width = " << CamSize.width << endl;
163  file << "height = " << CamSize.height << endl;
164  } else {
165  cv::FileStorage fs(path, cv::FileStorage::WRITE);
166  fs << "image_width" << CamSize.width;
167  fs << "image_height" << CamSize.height;
168  fs << "camera_matrix" << CameraMatrix;
169  fs << "distortion_coefficients" << Distorsion;
170  }
171 }
172 
175 void CameraParameters::resize(cv::Size size) throw(cv::Exception) {
176  if (!isValid())
177  throw cv::Exception(9007, "invalid object", "CameraParameters::resize", __FILE__, __LINE__);
178  if (size == CamSize)
179  return;
180  // now, read the camera size
181  // resize the camera parameters to fit this image size
182  float AxFactor = float(size.width) / float(CamSize.width);
183  float AyFactor = float(size.height) / float(CamSize.height);
184  CameraMatrix.at< float >(0, 0) *= AxFactor;
185  CameraMatrix.at< float >(0, 2) *= AxFactor;
186  CameraMatrix.at< float >(1, 1) *= AyFactor;
187  CameraMatrix.at< float >(1, 2) *= AyFactor;
188 }
189 
190 /****
191  *
192  *
193  *
194  *
195  */
196 void CameraParameters::readFromXMLFile(string filePath) throw(cv::Exception) {
197  cv::FileStorage fs(filePath, cv::FileStorage::READ);
198  int w = -1, h = -1;
199  cv::Mat MCamera, MDist;
200 
201  fs["image_width"] >> w;
202  fs["image_height"] >> h;
203  fs["distortion_coefficients"] >> MDist;
204  fs["camera_matrix"] >> MCamera;
205 
206  if (MCamera.cols == 0 || MCamera.rows == 0)
207  throw cv::Exception(9007, "File :" + filePath + " does not contains valid camera matrix", "CameraParameters::readFromXML", __FILE__, __LINE__);
208  if (w == -1 || h == 0)
209  throw cv::Exception(9007, "File :" + filePath + " does not contains valid camera dimensions", "CameraParameters::readFromXML", __FILE__, __LINE__);
210 
211  if (MCamera.type() != CV_32FC1)
212  MCamera.convertTo(CameraMatrix, CV_32FC1);
213  else
214  CameraMatrix = MCamera;
215 
216  if (MDist.total() < 4)
217  throw cv::Exception(9007, "File :" + filePath + " does not contains valid distortion_coefficients", "CameraParameters::readFromXML", __FILE__,
218  __LINE__);
219  // convert to 32 and get the 4 first elements only
220  cv::Mat mdist32;
221  MDist.convertTo(mdist32, CV_32FC1);
222  // Distorsion.create(1,4,CV_32FC1);
223  // for (int i=0;i<4;i++)
224  // Distorsion.ptr<float>(0)[i]=mdist32.ptr<float>(0)[i];
225 
226  Distorsion.create(1, 5, CV_32FC1);
227  for (int i = 0; i < 5; i++)
228  Distorsion.ptr< float >(0)[i] = mdist32.ptr< float >(0)[i];
229  CamSize.width = w;
230  CamSize.height = h;
231 }
232 /****
233  *
234  */
235 void CameraParameters::glGetProjectionMatrix(cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar,
236  bool invert) throw(cv::Exception) {
237 
238  if (cv::countNonZero(Distorsion) != 0)
239  std::cerr << "CameraParameters::glGetProjectionMatrix :: The camera has distortion coefficients " << __FILE__ << " " << __LINE__ << endl;
240  if (isValid() == false)
241  throw cv::Exception(9100, "invalid camera parameters", "CameraParameters::glGetProjectionMatrix", __FILE__, __LINE__);
242 
243  // Deterime the rsized info
244  double Ax = double(size.width) / double(orgImgSize.width);
245  double Ay = double(size.height) / double(orgImgSize.height);
246  double _fx = CameraMatrix.at< float >(0, 0) * Ax;
247  double _cx = CameraMatrix.at< float >(0, 2) * Ax;
248  double _fy = CameraMatrix.at< float >(1, 1) * Ay;
249  double _cy = CameraMatrix.at< float >(1, 2) * Ay;
250  double cparam[3][4] = {{_fx, 0, _cx, 0}, {0, _fy, _cy, 0}, {0, 0, 1, 0}};
251 
252  argConvGLcpara2(cparam, size.width, size.height, gnear, gfar, proj_matrix, invert);
253 }
254 
255 /*******************
256  *
257  *
258  *******************/
259 double CameraParameters::norm(double a, double b, double c) { return (sqrt(a * a + b * b + c * c)); }
260 
261 /*******************
262  *
263  *
264  *******************/
265 
266 double CameraParameters::dot(double a1, double a2, double a3, double b1, double b2, double b3) { return (a1 * b1 + a2 * b2 + a3 * b3); }
267 
268 /*******************
269  *
270  *
271  *******************/
272 
273 void CameraParameters::argConvGLcpara2(double cparam[3][4], int width, int height, double gnear, double gfar, double m[16], bool invert) throw(cv::Exception) {
274 
275  double icpara[3][4];
276  double trans[3][4];
277  double p[3][3], q[4][4];
278  int i, j;
279 
280  cparam[0][2] *= -1.0;
281  cparam[1][2] *= -1.0;
282  cparam[2][2] *= -1.0;
283 
284  if (arParamDecompMat(cparam, icpara, trans) < 0)
285  throw cv::Exception(9002, "parameter error", "MarkerDetector::argConvGLcpara2", __FILE__, __LINE__);
286 
287  for (i = 0; i < 3; i++) {
288  for (j = 0; j < 3; j++) {
289  p[i][j] = icpara[i][j] / icpara[2][2];
290  }
291  }
292  q[0][0] = (2.0 * p[0][0] / width);
293  q[0][1] = (2.0 * p[0][1] / width);
294  q[0][2] = ((2.0 * p[0][2] / width) - 1.0);
295  q[0][3] = 0.0;
296 
297  q[1][0] = 0.0;
298  q[1][1] = (2.0 * p[1][1] / height);
299  q[1][2] = ((2.0 * p[1][2] / height) - 1.0);
300  q[1][3] = 0.0;
301 
302  q[2][0] = 0.0;
303  q[2][1] = 0.0;
304  q[2][2] = (gfar + gnear) / (gfar - gnear);
305  q[2][3] = -2.0 * gfar * gnear / (gfar - gnear);
306 
307  q[3][0] = 0.0;
308  q[3][1] = 0.0;
309  q[3][2] = 1.0;
310  q[3][3] = 0.0;
311 
312  for (i = 0; i < 4; i++) {
313  for (j = 0; j < 3; j++) {
314  m[i + j * 4] = q[i][0] * trans[0][j] + q[i][1] * trans[1][j] + q[i][2] * trans[2][j];
315  }
316  m[i + 3 * 4] = q[i][0] * trans[0][3] + q[i][1] * trans[1][3] + q[i][2] * trans[2][3] + q[i][3];
317  }
318 
319  if (!invert) {
320  m[13] = -m[13];
321  m[1] = -m[1];
322  m[5] = -m[5];
323  m[9] = -m[9];
324  }
325 }
326 /*******************
327  *
328  *
329  *******************/
330 
331 int CameraParameters::arParamDecompMat(double source[3][4], double cpara[3][4], double trans[3][4]) throw(cv::Exception) {
332  int r, c;
333  double Cpara[3][4];
334  double rem1, rem2, rem3;
335 
336  if (source[2][3] >= 0) {
337  for (r = 0; r < 3; r++) {
338  for (c = 0; c < 4; c++) {
339  Cpara[r][c] = source[r][c];
340  }
341  }
342  } else {
343  for (r = 0; r < 3; r++) {
344  for (c = 0; c < 4; c++) {
345  Cpara[r][c] = -(source[r][c]);
346  }
347  }
348  }
349 
350  for (r = 0; r < 3; r++) {
351  for (c = 0; c < 4; c++) {
352  cpara[r][c] = 0.0;
353  }
354  }
355  cpara[2][2] = norm(Cpara[2][0], Cpara[2][1], Cpara[2][2]);
356  trans[2][0] = Cpara[2][0] / cpara[2][2];
357  trans[2][1] = Cpara[2][1] / cpara[2][2];
358  trans[2][2] = Cpara[2][2] / cpara[2][2];
359  trans[2][3] = Cpara[2][3] / cpara[2][2];
360 
361  cpara[1][2] = dot(trans[2][0], trans[2][1], trans[2][2], Cpara[1][0], Cpara[1][1], Cpara[1][2]);
362  rem1 = Cpara[1][0] - cpara[1][2] * trans[2][0];
363  rem2 = Cpara[1][1] - cpara[1][2] * trans[2][1];
364  rem3 = Cpara[1][2] - cpara[1][2] * trans[2][2];
365  cpara[1][1] = norm(rem1, rem2, rem3);
366  trans[1][0] = rem1 / cpara[1][1];
367  trans[1][1] = rem2 / cpara[1][1];
368  trans[1][2] = rem3 / cpara[1][1];
369 
370  cpara[0][2] = dot(trans[2][0], trans[2][1], trans[2][2], Cpara[0][0], Cpara[0][1], Cpara[0][2]);
371  cpara[0][1] = dot(trans[1][0], trans[1][1], trans[1][2], Cpara[0][0], Cpara[0][1], Cpara[0][2]);
372  rem1 = Cpara[0][0] - cpara[0][1] * trans[1][0] - cpara[0][2] * trans[2][0];
373  rem2 = Cpara[0][1] - cpara[0][1] * trans[1][1] - cpara[0][2] * trans[2][1];
374  rem3 = Cpara[0][2] - cpara[0][1] * trans[1][2] - cpara[0][2] * trans[2][2];
375  cpara[0][0] = norm(rem1, rem2, rem3);
376  trans[0][0] = rem1 / cpara[0][0];
377  trans[0][1] = rem2 / cpara[0][0];
378  trans[0][2] = rem3 / cpara[0][0];
379 
380  trans[1][3] = (Cpara[1][3] - cpara[1][2] * trans[2][3]) / cpara[1][1];
381  trans[0][3] = (Cpara[0][3] - cpara[0][1] * trans[1][3] - cpara[0][2] * trans[2][3]) / cpara[0][0];
382 
383  for (r = 0; r < 3; r++) {
384  for (c = 0; c < 3; c++) {
385  cpara[r][c] /= cpara[2][2];
386  }
387  }
388 
389  return 0;
390 }
391 
392 
393 /******
394  *
395  */
396 void CameraParameters::OgreGetProjectionMatrix(cv::Size orgImgSize, cv::Size size, double proj_matrix[16], double gnear, double gfar,
397  bool invert) throw(cv::Exception) {
398  double temp_matrix[16];
399  (*this).glGetProjectionMatrix(orgImgSize, size, temp_matrix, gnear, gfar, invert);
400  proj_matrix[0] = -temp_matrix[0];
401  proj_matrix[1] = -temp_matrix[4];
402  proj_matrix[2] = -temp_matrix[8];
403  proj_matrix[3] = temp_matrix[12];
404 
405  proj_matrix[4] = -temp_matrix[1];
406  proj_matrix[5] = -temp_matrix[5];
407  proj_matrix[6] = -temp_matrix[9];
408  proj_matrix[7] = temp_matrix[13];
409 
410  proj_matrix[8] = -temp_matrix[2];
411  proj_matrix[9] = -temp_matrix[6];
412  proj_matrix[10] = -temp_matrix[10];
413  proj_matrix[11] = temp_matrix[14];
414 
415  proj_matrix[12] = -temp_matrix[3];
416  proj_matrix[13] = -temp_matrix[7];
417  proj_matrix[14] = -temp_matrix[11];
418  proj_matrix[15] = temp_matrix[15];
419 }
420 /******
421  *
422  */
423 
424 cv::Mat CameraParameters::getRTMatrix(const cv::Mat &R_, const cv::Mat &T_, int forceType) {
425  cv::Mat M;
426  cv::Mat R, T;
427  R_.copyTo(R);
428  T_.copyTo(T);
429  if (R.type() == CV_64F) {
430  assert(T.type() == CV_64F);
431  cv::Mat Matrix = cv::Mat::eye(4, 4, CV_64FC1);
432 
433  cv::Mat R33 = cv::Mat(Matrix, cv::Rect(0, 0, 3, 3));
434  if (R.total() == 3) {
435  cv::Rodrigues(R, R33);
436  } else if (R.total() == 9) {
437  cv::Mat R64;
438  R.convertTo(R64, CV_64F);
439  R.copyTo(R33);
440  }
441  for (int i = 0; i < 3; i++)
442  Matrix.at< double >(i, 3) = T.ptr< double >(0)[i];
443  M = Matrix;
444  } else if (R.depth() == CV_32F) {
445  cv::Mat Matrix = cv::Mat::eye(4, 4, CV_32FC1);
446  cv::Mat R33 = cv::Mat(Matrix, cv::Rect(0, 0, 3, 3));
447  if (R.total() == 3) {
448  cv::Rodrigues(R, R33);
449  } else if (R.total() == 9) {
450  cv::Mat R32;
451  R.convertTo(R32, CV_32F);
452  R.copyTo(R33);
453  }
454 
455  for (int i = 0; i < 3; i++)
456  Matrix.at< float >(i, 3) = T.ptr< float >(0)[i];
457  M = Matrix;
458  }
459 
460  if (forceType == -1)
461  return M;
462  else {
463  cv::Mat MTyped;
464  M.convertTo(MTyped, forceType);
465  return MTyped;
466  }
467 }
468 };
string cmd
Parameters of the camera.


asr_aruco_marker_recognition
Author(s): Allgeyer Tobias, Meißner Pascal, Qattan Mohamad
autogenerated on Mon Feb 28 2022 21:41:30