Main MRPT website > C++ reference for MRPT 1.5.7
TCamera.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "base-precomp.h" // Precompiled headers
11 
12 #include <mrpt/utils/TCamera.h>
14 #include <mrpt/math/matrix_serialization.h> // For "<<" ">>" operators.
15 #include <mrpt/math/utils_matlab.h>
16 
17 using namespace mrpt::utils;
18 using namespace mrpt::math;
19 using namespace std;
20 
21 
22 /* Implements serialization for the TCamera struct as it will be included within CObservations objects */
24 
25 /** Dumps all the parameters as a multi-line string, with the same format than \a saveToConfigFile. \sa saveToConfigFile */
26 std::string TCamera::dumpAsText() const
27 {
29  saveToConfigFile("",cfg);
30  return cfg.getContent();
31 }
32 
33 
34 // WriteToStream
35 void TCamera::writeToStream( CStream &out, int *version ) const
36 {
37  if( version )
38  *version = 2;
39  else
40  {
41  out << focalLengthMeters;
42  for(unsigned int k = 0; k < 5; k++) out << dist[k];
43  out << intrinsicParams;
44  // version 0 did serialize here a "CMatrixDouble15"
45  out << nrows << ncols; // New in v2
46  } // end else
47 }
48 
49 // ReadFromStream
51 {
52  switch( version )
53  {
54  case 0:
55  case 1:
56  case 2:
57  {
58  in >> focalLengthMeters;
59 
60  for(unsigned int k = 0; k < 5; k++)
61  in >> dist[k];
62 
63  in >> intrinsicParams;
64 
65  if (version==0)
66  {
67  CMatrixDouble15 __distortionParams;
68  in >> __distortionParams;
69  }
70 
71  if (version>=2)
72  in >> nrows >> ncols;
73  else {
74  nrows = 480;
75  ncols = 640;
76  }
77 
78 
79  } break;
80  default:
82  }
83 }
84 
85 /*---------------------------------------------------------------
86  Implements the writing to a mxArray for Matlab
87  ---------------------------------------------------------------*/
88 #if MRPT_HAS_MATLAB
89 // Add to implement mexplus::from template specialization
91 
93 {
94  const char* fields[] = {"K","dist","f","ncols","nrows"};
95  mexplus::MxArray params_struct( mexplus::MxArray::Struct(sizeof(fields)/sizeof(fields[0]),fields) );
96  params_struct.set("K", mrpt::math::convertToMatlab(this->intrinsicParams));
97  params_struct.set("dist", mrpt::math::convertToMatlab(this->dist));
98  params_struct.set("f", this->focalLengthMeters);
99  params_struct.set("ncols", this->ncols);
100  params_struct.set("nrows", this->nrows);
101  return params_struct.release();
102 }
103 #endif
104 
105 /** Save as a config block:
106  * \code
107  * [SECTION]
108  * resolution = NCOLS NROWS
109  * cx = CX
110  * cy = CY
111  * fx = FX
112  * fy = FY
113  * dist = K1 K2 T1 T2 T3
114  * focal_length = FOCAL_LENGTH
115  * \endcode
116  */
118 {
119  cfg.write(section,"resolution",format("[%u %u]",(unsigned int)ncols,(unsigned int)nrows));
120  cfg.write(section,"cx", format("%.05f",cx()) );
121  cfg.write(section,"cy", format("%.05f",cy()) );
122  cfg.write(section,"fx", format("%.05f",fx()) );
123  cfg.write(section,"fy", format("%.05f",fy()) );
124  cfg.write(section,"dist", format("[%e %e %e %e %e]", dist[0],dist[1],dist[2],dist[3],dist[4] ) );
125  if (focalLengthMeters!=0) cfg.write(section,"focal_length", focalLengthMeters );
126 }
127 
128 /** Load all the params from a config source, in the format described in saveToConfigFile()
129  */
131 {
132  vector<uint64_t> out_res;
133  cfg.read_vector(section,"resolution",vector<uint64_t>(),out_res,true);
134  if (out_res.size()!=2) THROW_EXCEPTION("Expected 2-length vector in field 'resolution'");
135  ncols = out_res[0];
136  nrows = out_res[1];
137 
138  double fx, fy, cx, cy;
139  fx = cfg.read_double(section,"fx",0, true);
140  fy = cfg.read_double(section,"fy",0, true);
141  cx = cfg.read_double(section,"cx",0, true);
142  cy = cfg.read_double(section,"cy",0, true);
143 
144  if( fx < 2.0 ) fx *= ncols;
145  if( fy < 2.0 ) fy *= nrows;
146  if( cx < 2.0 ) cx *= ncols;
147  if( cy < 2.0 ) cy *= nrows;
148 
149  setIntrinsicParamsFromValues( fx, fy, cx, cy );
150 
151  CVectorDouble dists;
152  cfg.read_vector(section,"dist",CVectorDouble(), dists, true);
153  if (dists.size()!=4 && dists.size()!=5) THROW_EXCEPTION("Expected 4 or 5-length vector in field 'dist'");
154 
155  dist.Constant(0);
156  for (CVectorDouble::Index i=0;i<dists.size();i++)
157  dist[i] = dists[i];
158 
159  focalLengthMeters = cfg.read_double(section,"focal_length",0, false /* optional value */ );
160 
161 }
162 
163 /** Rescale all the parameters for a new camera resolution (it raises an exception if the aspect ratio is modified, which is not permitted).
164  */
165 void TCamera::scaleToResolution(unsigned int new_ncols, unsigned int new_nrows)
166 {
167  if (ncols == new_ncols && nrows == new_nrows)
168  return; // already done
169 
170  ASSERT_(new_nrows>0 && new_ncols>0)
171 
172  const double prev_aspect_ratio = ncols/double(nrows);
173  const double new_aspect_ratio = new_ncols/double(new_nrows);
174 
175  ASSERTMSG_(std::abs(prev_aspect_ratio-new_aspect_ratio)<1e-3, "TCamera: Trying to scale camera parameters for a resolution of different aspect ratio." )
176 
177  const double K = new_ncols / double(ncols);
178 
179  ncols = new_ncols;
180  nrows = new_nrows;
181 
182  // fx fy cx cy
183  intrinsicParams(0,0)*=K;
184  intrinsicParams(1,1)*=K;
185  intrinsicParams(0,2)*=K;
186  intrinsicParams(1,2)*=K;
187 
188  // distortion params: unmodified.
189 }
190 
192 {
193  return
194  a.ncols==b.ncols &&
195  a.nrows==b.nrows &&
196  a.intrinsicParams==b.intrinsicParams &&
197  a.dist==b.dist &&
198  a.focalLengthMeters==b.focalLengthMeters;
199 }
201 {
202  return !(a==b);
203 }
bool operator!=(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:285
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
mxArray * convertToMatlab(const Eigen::EigenBase< Derived > &mat)
Convert vectors, arrays and matrices into Matlab vectors/matrices.
Definition: utils_matlab.h:38
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define THROW_EXCEPTION(msg)
void scaleToResolution(unsigned int new_ncols, unsigned int new_nrows)
Rescale all the parameters for a new camera resolution (it raises an exception if the aspect ratio is...
Definition: TCamera.cpp:165
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
STL namespace.
This class allows loading and storing values and vectors of different types from a configuration text...
This class implements a config file-like interface over a memory-stored string list.
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
Definition: TCamera.cpp:50
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
GLubyte GLubyte b
Definition: glext.h:5575
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
int version
Definition: mrpt_jpeglib.h:898
GLsizei const GLchar ** string
Definition: glext.h:3919
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Definition: TCamera.cpp:35
virtual mxArray * writeToMatlab() const
Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class.
Definition: CSerializable.h:79
void write(const std::string &section, const std::string &name, const data_t &value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
This file implements matrix/vector text and binary serialization.
GLuint in
Definition: glext.h:6301
#define ASSERT_(f)
bool operator==(const CArray< T, N > &x, const CArray< T, N > &y)
Definition: CArray.h:277
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:37
#define ASSERTMSG_(f, __ERROR_MSG)
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
void loadFromConfigFile(const std::string &section, const mrpt::utils::CConfigFileBase &cfg)
Load all the params from a config source, in the format used in saveToConfigFile(), that is:
Definition: TCamera.cpp:130
struct mxArray_tag mxArray
Forward declaration for mxArray (avoid #including as much as possible to speed up compiling) ...
Definition: CSerializable.h:17
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:31
void saveToConfigFile(const std::string &section, mrpt::utils::CConfigFileBase &cfg) const
Save as a config block:
Definition: TCamera.cpp:117



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019