Medical Imaging Interaction Toolkit  2018.4.99-08619e4f
Medical Imaging Interaction Toolkit
mitkCameraIntrinsics.cpp
Go to the documentation of this file.
1 /*============================================================================
2 
3 The Medical Imaging Interaction Toolkit (MITK)
4 
5 Copyright (c) German Cancer Research Center (DKFZ)
6 All rights reserved.
7 
8 Use of this source code is governed by a 3-clause BSD license that can be
9 found in the LICENSE file.
10 
11 ============================================================================*/
12 
13 #include "mitkCameraIntrinsics.h"
14 #include <itkMutexLockHolder.h>
15 #include <mitkEndoMacros.h>
16 #include <mitkEndoDebug.h>
17 
19  : m_Valid(false), m_Mutex(itk::FastMutexLock::New())
20 {
21  m_CameraMatrix = cv::Mat::zeros(3, 3, cv::DataType<double>::type);
22  m_CameraMatrix.at<double>(2,2) = 1.0;
23  m_DistorsionCoeffs = cv::Mat::zeros(1, 5, cv::DataType<double>::type);
24 }
25 
27  : itk::Object()
29  , m_Valid(false)
30  , m_Mutex(itk::FastMutexLock::New())
31 {
32  this->Copy(&other);
33 }
34 
36 {
37 
38 }
39 
41 {
42  return other->GetDistorsionCoeffsAsPoint4D()==
44  other->GetFocalPoint()==
45  this->GetFocalPoint() &&
46  other->GetPrincipalPoint()
47  == this->GetPrincipalPoint();
48 }
49 
51 {
52  this->SetIntrinsics( other->GetCameraMatrix().clone()
53  , other->GetDistorsionCoeffs().clone() );
54  this->SetValid(other->m_Valid);
55 }
56 
58 {
60  return m_Valid;
61 }
62 
63 vnl_matrix_fixed<mitk::ScalarType, 3, 3>
65 {
66  vnl_matrix_fixed<mitk::ScalarType, 3, 3> mat;
67  mat.set_identity();
68 
69  {
71  mat(0,0) = m_CameraMatrix.at<double>(0,0);
72  mat(1,1) = m_CameraMatrix.at<double>(1,1);
73 
74  mat(0,2) = m_CameraMatrix.at<double>(0,2);
75  mat(1,2) = m_CameraMatrix.at<double>(1,2);
76  }
77 
78  return mat;
79 }
80 
82  const vnl_matrix_fixed<mitk::ScalarType, 3, 3>& _CameraMatrix )
83 {
85  m_CameraMatrix.at<double>(0,0) = _CameraMatrix(0,0);
86  m_CameraMatrix.at<double>(1,1) = _CameraMatrix(1,1);
87 
88  m_CameraMatrix.at<double>(0,2) = _CameraMatrix(0,2);
89  m_CameraMatrix.at<double>(1,2) = _CameraMatrix(1,2);
90 }
91 
92 vnl_matrix_fixed<mitk::ScalarType, 3, 4>
94 {
95  vnl_matrix_fixed<mitk::ScalarType, 3, 4> mat;
96  mat.fill(0);
97  mat.update( this->GetVnlCameraMatrix().as_matrix() );
98 
99  return mat;
100 }
101 
102 void mitk::CameraIntrinsics::SetIntrinsics( const cv::Mat& _CameraMatrix
103  , const cv::Mat& _DistorsionCoeffs)
104 {
105  {
107  if( _CameraMatrix.cols != 3 || _CameraMatrix.rows != 3)
108  throw std::invalid_argument("Wrong format of camera matrix. Should be 3x3"
109  " double.");
110 
111  endoAssertMsg( (_DistorsionCoeffs.cols == 5) &&
112  _DistorsionCoeffs.rows == 1, "Wrong format of distorsion coefficients"
113  " vector. Should be 5x1 double.");
114 
115  m_CameraMatrix = _CameraMatrix.clone();
116  m_DistorsionCoeffs = _DistorsionCoeffs.clone();
117 
118  m_Valid = true;
119  }
120  this->Modified();
121 }
122 
124  const mitk::Point3D& principalPoint,
125  const mitk::Point4D& distortionCoefficients)
126 
127 {
128  {
130  m_CameraMatrix.at<double>(0,0) = focalPoint[0];
131  m_CameraMatrix.at<double>(1,1) = focalPoint[1];
132 
133  m_CameraMatrix.at<double>(0,2) = principalPoint[0];
134  m_CameraMatrix.at<double>(1,2) = principalPoint[1];
135 
136  m_DistorsionCoeffs.at<double>(0,0) = distortionCoefficients[0];
137  m_DistorsionCoeffs.at<double>(0,1) = distortionCoefficients[1];
138  m_DistorsionCoeffs.at<double>(0,2) = distortionCoefficients[2];
139  m_DistorsionCoeffs.at<double>(0,3) = distortionCoefficients[3];
140  }
141  this->Modified();
142 }
143 
144 void mitk::CameraIntrinsics::SetFocalLength( double x, double y )
145 {
146  {
148  m_CameraMatrix.at<double>(0,0) = x;
149  m_CameraMatrix.at<double>(1,1) = y;
150  }
151  this->Modified();
152 }
153 
155 {
156  {
158  m_CameraMatrix.at<double>(0,2) = x;
159  m_CameraMatrix.at<double>(1,2) = y;
160  }
161  this->Modified();
162 }
163 
165  double p1, double p2 )
166 {
167 
168  {
170 
171  m_DistorsionCoeffs.at<double>(0,0) = k1;
172  m_DistorsionCoeffs.at<double>(0,1) = k2;
173  m_DistorsionCoeffs.at<double>(0,2) = p1;
174  m_DistorsionCoeffs.at<double>(0,3) = p2;
175  }
176  this->Modified();
177 }
178 
180 {
182  return m_CameraMatrix.clone(); // return a copy of this small matrix
183 }
184 
186 {
188  return m_DistorsionCoeffs.clone(); // return a copy of this small matrix
189 }
190 
192 {
193  const CameraIntrinsics* intrinsics = this;
194  return intrinsics->GetDistorsionCoeffs();
195 }
196 
198 {
200  std::ostringstream s; s.precision(12);
201  const cv::Mat& CameraMatrix = m_CameraMatrix;
202  const cv::Mat& DistorsionCoeffs = m_DistorsionCoeffs;
203 
204  s.str(""); s << this->GetNameOfClass() << ": ";
205  s << "fx = " << CameraMatrix.at<double>(0,0);
206  s << ", fy = " << CameraMatrix.at<double>(1,1);
207  s << ", cx = " << CameraMatrix.at<double>(0,2);
208  s << ", cy = " << CameraMatrix.at<double>(1,2);
209 
210  s << ", k1 = " << DistorsionCoeffs.at<double>(0,0);
211  s << ", k2 = " << DistorsionCoeffs.at<double>(0,1);
212  s << ", p1 = " << DistorsionCoeffs.at<double>(0,2);
213  s << ", p2 = " << DistorsionCoeffs.at<double>(0,3);
214  //s << ", k3 = " << DistorsionCoeffs.at<double>(0,4);
215 
216  return s.str();
217 }
218 
219 void mitk::CameraIntrinsics::ToXML(TiXmlElement* elem) const
220 {
222  elem->SetValue(this->GetNameOfClass());
223  std::ostringstream s; s.precision(12);
224  const cv::Mat& CameraMatrix = m_CameraMatrix;
225  s.str(""); s << CameraMatrix.at<double>(0,0);
226  elem->SetAttribute( "fx", s.str() );
227  s.str(""); s << CameraMatrix.at<double>(1,1);
228  elem->SetAttribute( "fy", s.str() );
229  s.str(""); s << CameraMatrix.at<double>(0,2);
230  elem->SetAttribute( "cx", s.str() );
231  s.str(""); s << CameraMatrix.at<double>(1,2);
232  elem->SetAttribute( "cy", s.str() );
233 
234  const cv::Mat& DistorsionCoeffs = m_DistorsionCoeffs;
235  s.str(""); s << DistorsionCoeffs.at<double>(0,0);
236  elem->SetAttribute( "k1", s.str() );
237  s.str(""); s << DistorsionCoeffs.at<double>(0,1);
238  elem->SetAttribute( "k2", s.str() );
239  s.str(""); s << DistorsionCoeffs.at<double>(0,2);
240  elem->SetAttribute( "p1", s.str() );
241  s.str(""); s << DistorsionCoeffs.at<double>(0,3);
242  elem->SetAttribute( "p2", s.str() );
243  elem->SetAttribute("Valid", m_Valid);
244  //s.str(""); s << DistorsionCoeffs.at<double>(4,0);
245  //elem->SetAttribute( "k3", s.str() );
246 }
247 
249 {
250  assert( elem );
251  assert( elem->ValueStr() == "results" );
252  cv::Mat CameraMatrix = cv::Mat::zeros(3, 3, cv::DataType<double>::type);
253  CameraMatrix.at<double>(2,2) = 1.0;
254  cv::Mat DistorsionCoeffs = cv::Mat::zeros(1, 5, cv::DataType<double>::type);
255 
256  TiXmlElement* focus_lenXElem = elem->FirstChildElement("focus_lenX");
257  endoAssert( focus_lenXElem != nullptr );
258  CameraMatrix.at<double>(0,0) = atof( focus_lenXElem->GetText() );
259 
260  TiXmlElement* focus_lenYElem = elem->FirstChildElement("focus_lenY");
261  endoAssert( focus_lenYElem != nullptr );
262  CameraMatrix.at<double>(1,1) = atof( focus_lenYElem->GetText() );
263 
264  TiXmlElement* PrincipalXElem = elem->FirstChildElement("PrincipalX");
265  endoAssert( PrincipalXElem != nullptr );
266  CameraMatrix.at<double>(0,2) = atof( PrincipalXElem->GetText() );
267 
268  TiXmlElement* PrincipalYElem = elem->FirstChildElement("PrincipalY");
269  endoAssert( PrincipalYElem != nullptr );
270  CameraMatrix.at<double>(1,2) = atof( PrincipalYElem->GetText() );
271 
272  // DISTORSION COEFFS
273 
274  TiXmlElement* Dist1Elem = elem->FirstChildElement("Dist1");
275  endoAssert( Dist1Elem != nullptr );
276  DistorsionCoeffs.at<double>(0,0) = atof( Dist1Elem->GetText() );
277 
278  TiXmlElement* Dist2Elem = elem->FirstChildElement("Dist2");
279  endoAssert( Dist2Elem != nullptr );
280  DistorsionCoeffs.at<double>(0,1) = atof( Dist2Elem->GetText() );
281 
282  TiXmlElement* Dist3Elem = elem->FirstChildElement("Dist3");
283  endoAssert( Dist3Elem != nullptr );
284  DistorsionCoeffs.at<double>(0,2) = atof( Dist3Elem->GetText() );
285 
286  TiXmlElement* Dist4Elem = elem->FirstChildElement("Dist4");
287  endoAssert( Dist4Elem != nullptr );
288  DistorsionCoeffs.at<double>(0,3) = atof( Dist4Elem->GetText() );
289 
290  int valid = 0;
291  elem->QueryIntAttribute("Valid", &valid);
292 
293  {
295  m_Valid = static_cast<bool>(valid);
296  m_CameraMatrix = CameraMatrix;
297  m_DistorsionCoeffs = DistorsionCoeffs;
298  }
299 
300  this->Modified();
301 }
302 
303 void mitk::CameraIntrinsics::FromXML(TiXmlElement* elem)
304 {
305  endoAssert ( elem );
306  MITK_DEBUG << elem->Value();
307  std::string filename;
308  if(elem->QueryStringAttribute("file", &filename) == TIXML_SUCCESS)
309  {
310  this->FromXMLFile(filename);
311  return;
312  }
313  else if(strcmp(elem->Value(), "CalibrationProject") == 0)
314  {
315  this->FromGMLCalibrationXML(elem->FirstChildElement("results"));
316  return;
317  }
318 
319  assert ( elem );
320  if(strcmp(elem->Value(), this->GetNameOfClass()) != 0)
321  elem = elem->FirstChildElement(this->GetNameOfClass());
322 
323  std::ostringstream err;
324  // CAMERA MATRIX
325  cv::Mat CameraMatrix = cv::Mat::zeros(3, 3, cv::DataType<double>::type);
326  CameraMatrix.at<double>(2,2) = 1.0;
327  double val = 0.0;
328  if(elem->QueryDoubleAttribute("fx", &val) == TIXML_SUCCESS)
329  CameraMatrix.at<double>(0,0) = val;
330  else
331  err << "fx, ";
332 
333  if(elem->QueryDoubleAttribute("fy", &val) == TIXML_SUCCESS)
334  CameraMatrix.at<double>(1,1) = val;
335  else
336  err << "fy, ";
337 
338  if(elem->QueryDoubleAttribute("cx", &val) == TIXML_SUCCESS)
339  CameraMatrix.at<double>(0,2) = val;
340  else
341  err << "cx, ";
342 
343  if(elem->QueryDoubleAttribute("cy", &val) == TIXML_SUCCESS)
344  CameraMatrix.at<double>(1,2) = val;
345  else
346  err << "cy, ";
347 
348  // DISTORSION COEFFS
349  endodebug( "creating DistorsionCoeffs from XML file")
350  cv::Mat DistorsionCoeffs = cv::Mat::zeros(1, 5, cv::DataType<double>::type);
351  if(elem->QueryDoubleAttribute("k1", &val) == TIXML_SUCCESS)
352  DistorsionCoeffs.at<double>(0,0) = val;
353  else
354  err << "k1, ";
355 
356  if(elem->QueryDoubleAttribute("k2", &val) == TIXML_SUCCESS)
357  DistorsionCoeffs.at<double>(0,1) = val;
358  else
359  err << "k2, ";
360 
361  if(elem->QueryDoubleAttribute("p1", &val) == TIXML_SUCCESS)
362  DistorsionCoeffs.at<double>(0,2) = val;
363  else
364  err << "p1, ";
365 
366  if(elem->QueryDoubleAttribute("p2", &val) == TIXML_SUCCESS)
367  DistorsionCoeffs.at<double>(0,3) = val;
368  else
369  err << "p2, ";
370 
371  DistorsionCoeffs.at<double>(0,4) = 0.0;
372 
373  /*if(elem->QueryDoubleAttribute("k3", &val) == TIXML_SUCCESS)
374  DistorsionCoeffs.at<double>(4,0) = val;
375  else
376  err << "k3, ";*/
377 
378  std::string errorStr = err.str();
379  int errLength = errorStr.length();
380  if(errLength > 0)
381  {
382  errorStr = errorStr.substr(0, errLength-2);
383  errorStr.append(" not found");
384  throw std::invalid_argument(err.str());
385  }
386 
387  int valid = 0;
388  elem->QueryIntAttribute("Valid", &valid);
389 
390  {
392  m_Valid = static_cast<bool>(valid);
393  m_CameraMatrix = CameraMatrix;
394  m_DistorsionCoeffs = DistorsionCoeffs;
395  }
396 
397  this->Modified();
398 }
399 
401 {
403  double FocalLengthX = m_CameraMatrix.at<double>(0,0);
404 
405  return FocalLengthX;
406 }
408 {
410  double FocalLengthY = m_CameraMatrix.at<double>(1,1);;
411  return FocalLengthY;
412 }
414 {
416  double PrincipalPointX = m_CameraMatrix.at<double>(0,2);
417  return PrincipalPointX;
418 }
420 {
422  double PrincipalPointY = m_CameraMatrix.at<double>(1,2);
423  return PrincipalPointY;
424 }
426 {
428  mitk::Point4D coeffs;
429 
430  coeffs[0] = m_DistorsionCoeffs.at<double>(0,0);
431  coeffs[1] = m_DistorsionCoeffs.at<double>(0,1);
432  coeffs[2] = m_DistorsionCoeffs.at<double>(0,2);
433  coeffs[3] = m_DistorsionCoeffs.at<double>(0,3);
434 
435  return coeffs;
436 }
437 
439 {
440  mitk::Point3D p;
441  p[0] = this->GetFocalLengthX();
442  p[1] = this->GetFocalLengthY();
443  p[2] = 0;
444  return p;
445 }
446 
448 {
449  mitk::Point3D p;
450  p[0] = this->GetPrincipalPointX();
451  p[1] = this->GetPrincipalPointY();
452  p[2] = 0;
453  return p;
454 }
455 
456 vnl_vector_fixed<mitk::ScalarType, 2>
458 {
459  vnl_vector_fixed<mitk::ScalarType, 2> vec;
460  vec[0] = this->GetFocalLengthX();
461  vec[1] = this->GetFocalLengthY();
462  return vec;
463 }
464 
465 vnl_vector_fixed<mitk::ScalarType, 2>
467 {
468  vnl_vector_fixed<mitk::ScalarType, 2> vec;
469  vec[0] = this->GetPrincipalPointX();
470  vec[1] = this->GetPrincipalPointY();
471  return vec;
472 }
473 
474 std::ostream& operator<< (std::ostream& os, mitk::CameraIntrinsics::Pointer p)
475 {
476  os << p->ToString();
477  return os;
478 }
479 
481 {
482  return this->ToString();
483 }
484 
486  const std::string& varName)
487 {
488  std::ostringstream s;
489  s << varName << " = [" << this->GetFocalLengthX() << " 0 "
490  << this->GetPrincipalPointX() << "; 0 " <<
491  this->GetFocalLengthY() << " " << this->GetPrincipalPointY() << ";"
492  << " 0 0 1 ];";
493  return s.str();
494 }
495 
497 {
499  m_Valid = valid;
500 }
501 
502 itk::LightObject::Pointer mitk::CameraIntrinsics::InternalClone() const
503 {
504  itk::LightObject::Pointer result(new Self(*this));
505  result->UnRegister();
506  return result;
507 }
vnl_vector_fixed< mitk::ScalarType, 2 > GetFocalPointAsVnlVector() const
mitk::Point3D GetFocalPoint() const
std::string ToString() const
void FromXML(TiXmlElement *elem) override
bool Equals(const CameraIntrinsics *other) const
class representing camera intrinsics and related functions
void FromGMLCalibrationXML(TiXmlElement *elem)
std::string ToOctaveString(const std::string &varName="CameraIntrinsics")
vnl_matrix_fixed< mitk::ScalarType, 3, 3 > GetVnlCameraMatrix() const
virtual void FromXMLFile(const std::string &file, const std::string &elemName="")
#define MITK_DEBUG
Definition: mitkLogMacros.h:22
#define endoAssertMsg(a, msg)
DataCollection - Class to facilitate loading/accessing structured data.
mitk::Point3D GetPrincipalPoint() const
void SetFocalLength(double x, double y)
void SetIntrinsics(const cv::Mat &_CameraMatrix, const cv::Mat &_DistorsionCoeffs)
#define endodebug(msg)
virtual const char * GetNameOfClass() const =0
itk::FastMutexLock::Pointer m_Mutex
vnl_matrix_fixed< mitk::ScalarType, 3, 4 > GetVnlCameraMatrix3x4() const
void SetDistorsionCoeffs(double k1, double k2, double p1, double p2)
void SetPrincipalPoint(double x, double y)
#define endoAssert(a)
static Pointer New()
interface for all classes able to write themselves to XML files
void Copy(const CameraIntrinsics *other)
void ToXML(TiXmlElement *elem) const override
vnl_vector_fixed< mitk::ScalarType, 2 > GetPrincipalPointAsVnlVector() const
mitk::Point4D GetDistorsionCoeffsAsPoint4D() const
void SetCameraMatrix(const vnl_matrix_fixed< mitk::ScalarType, 3, 3 > &_CameraMatrix)
MITKCORE_EXPORT std::ostream & operator<<(std::ostream &o, DataNode::Pointer &dtn)