MRPT  1.9.9
TCamera.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "img-precomp.h" // Precompiled headers
11 
13 #include <mrpt/img/TCamera.h>
15 #include <mrpt/math/matrix_serialization.h> // For "<<" ">>" operators.
16 #include <mrpt/math/utils_matlab.h>
17 
18 using namespace mrpt::img;
19 using namespace mrpt::math;
20 using namespace std;
21 
22 /* Implements serialization for the TCamera struct as it will be included within
23  * CObservations objects */
25 
27 {
28  // Ensure intrinsics matrix has a 1 in the bottom-right corner:
29  setIntrinsicParamsFromValues(0, 0, 0, 0);
30 }
31 
32 /** Dumps all the parameters as a multi-line string, with the same format than
33  * \a saveToConfigFile. \sa saveToConfigFile */
34 std::string TCamera::dumpAsText() const
35 {
37  saveToConfigFile("", cfg);
38  return cfg.getContent();
39 }
40 
41 uint8_t TCamera::serializeGetVersion() const { return 4; }
43 {
44  out << focalLengthMeters;
45  // v3: from 5 to 8 dist params:
46  for (size_t k = 0; k < dist.size(); k++) out << dist[k];
47  // v4: only store the 4 relevant values:
48  out << fx() << fy() << cx() << cy();
49  // version 0 did serialize here a "CMatrixDouble15"
50  out << nrows << ncols; // New in v2
51 }
53 {
54  switch (version)
55  {
56  case 0:
57  case 1:
58  case 2:
59  case 3:
60  case 4:
61  {
62  in >> focalLengthMeters;
63 
64  dist.fill(0);
65  for (unsigned int k = 0; k < 5; k++) in >> dist[k];
66  if (version >= 3)
67  for (unsigned int k = 5; k < 8; k++) in >> dist[k];
68 
69  if (version < 4)
70  {
71  in >> intrinsicParams;
72  // Enforce values with fixed 0 or 1 values:
73  intrinsicParams(0, 1) = 0;
74  intrinsicParams(1, 0) = 0;
75  intrinsicParams(2, 0) = 0;
76  intrinsicParams(2, 1) = 0;
77  intrinsicParams(2, 2) = 1;
78  }
79  else
80  {
81  double vfx, vfy, vcx, vcy;
82  in >> vfx >> vfy >> vcx >> vcy;
83  setIntrinsicParamsFromValues(vfx, vfy, vcx, vcy);
84  }
85 
86  if (version == 0)
87  {
88  CMatrixDouble15 __distortionParams;
89  in >> __distortionParams;
90  }
91 
92  if (version >= 2)
93  in >> nrows >> ncols;
94  else
95  {
96  nrows = 480;
97  ncols = 640;
98  }
99  }
100  break;
101  default:
103  }
104 }
105 
106 /*---------------------------------------------------------------
107  Implements the writing to a mxArray for Matlab
108  ---------------------------------------------------------------*/
109 #if MRPT_HAS_MATLAB
110 // Add to implement mexplus::from template specialization
112 #endif
113 
115 {
116 #if MRPT_HAS_MATLAB
117  const char* fields[] = {"K", "dist", "f", "ncols", "nrows"};
118  mexplus::MxArray params_struct(
119  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
120  params_struct.set("K", mrpt::math::convertToMatlab(this->intrinsicParams));
121  params_struct.set("dist", mrpt::math::convertVectorToMatlab(this->dist));
122  params_struct.set("f", this->focalLengthMeters);
123  params_struct.set("ncols", this->ncols);
124  params_struct.set("nrows", this->nrows);
125  return params_struct.release();
126 #else
127  THROW_EXCEPTION("MRPT built without MATLAB/Mex support");
128 #endif
129 }
130 
131 /** Save as a config block:
132  * \code
133  * [SECTION]
134  * resolution = NCOLS NROWS
135  * cx = CX
136  * cy = CY
137  * fx = FX
138  * fy = FY
139  * dist = K1 K2 T1 T2 T3
140  * focal_length = FOCAL_LENGTH
141  * \endcode
142  */
144  const std::string& section, mrpt::config::CConfigFileBase& cfg) const
145 {
146  cfg.write(
147  section, "resolution",
148  format("[%u %u]", (unsigned int)ncols, (unsigned int)nrows));
149  cfg.write(section, "cx", format("%.05f", cx()));
150  cfg.write(section, "cy", format("%.05f", cy()));
151  cfg.write(section, "fx", format("%.05f", fx()));
152  cfg.write(section, "fy", format("%.05f", fy()));
153  cfg.write(
154  section, "dist",
155  format(
156  "[%e %e %e %e %e %e %e %e]", dist[0], dist[1], dist[2], dist[3],
157  dist[4], dist[5], dist[6], dist[7]));
158  if (focalLengthMeters != 0)
159  cfg.write(section, "focal_length", focalLengthMeters);
160 }
161 
162 /** Load all the params from a config source, in the format described in
163  * saveToConfigFile()
164  */
166  const std::string& section, const mrpt::config::CConfigFileBase& cfg)
167 {
168  vector<uint64_t> out_res;
169  cfg.read_vector(section, "resolution", vector<uint64_t>(), out_res, true);
170  if (out_res.size() != 2)
171  THROW_EXCEPTION("Expected 2-length vector in field 'resolution'");
172  ncols = out_res[0];
173  nrows = out_res[1];
174 
175  double fx, fy, cx, cy;
176  fx = cfg.read_double(section, "fx", 0, true);
177  fy = cfg.read_double(section, "fy", 0, true);
178  cx = cfg.read_double(section, "cx", 0, true);
179  cy = cfg.read_double(section, "cy", 0, true);
180 
181  if (fx < 2.0) fx *= ncols;
182  if (fy < 2.0) fy *= nrows;
183  if (cx < 2.0) cx *= ncols;
184  if (cy < 2.0) cy *= nrows;
185 
186  setIntrinsicParamsFromValues(fx, fy, cx, cy);
187 
188  CVectorDouble dists;
189  cfg.read_vector(section, "dist", CVectorDouble(), dists, true);
190  if (dists.size() != 4 && dists.size() != 5 && dists.size() != 8)
191  THROW_EXCEPTION("Expected 4,5 or 8-length vector in field 'dist'");
192 
193  dist.fill(0);
194  for (CVectorDouble::Index i = 0; i < dists.size(); i++) dist[i] = dists[i];
195 
196  focalLengthMeters =
197  cfg.read_double(section, "focal_length", 0, false /* optional value */);
198 }
199 
200 /** Rescale all the parameters for a new camera resolution (it raises an
201  * exception if the aspect ratio is modified, which is not permitted).
202  */
203 void TCamera::scaleToResolution(unsigned int new_ncols, unsigned int new_nrows)
204 {
205  if (ncols == new_ncols && nrows == new_nrows) return; // already done
206 
207  ASSERT_(new_nrows > 0 && new_ncols > 0);
208 
209  const double prev_aspect_ratio = ncols / double(nrows);
210  const double new_aspect_ratio = new_ncols / double(new_nrows);
211 
212  ASSERTMSG_(
213  std::abs(prev_aspect_ratio - new_aspect_ratio) < 1e-3,
214  "TCamera: Trying to scale camera parameters for a resolution of "
215  "different aspect ratio.");
216 
217  const double K = new_ncols / double(ncols);
218 
219  ncols = new_ncols;
220  nrows = new_nrows;
221 
222  // fx fy cx cy
223  intrinsicParams(0, 0) *= K;
224  intrinsicParams(1, 1) *= K;
225  intrinsicParams(0, 2) *= K;
226  intrinsicParams(1, 2) *= K;
227 
228  // distortion params: unmodified.
229 }
230 
232  const mrpt::img::TCamera& a, const mrpt::img::TCamera& b)
233 {
234  return a.ncols == b.ncols && a.nrows == b.nrows &&
235  a.intrinsicParams == b.intrinsicParams && a.dist == b.dist &&
237 }
239  const mrpt::img::TCamera& a, const mrpt::img::TCamera& b)
240 {
241  return !(a == b);
242 }
This class implements a config file-like interface over a memory-stored string list.
uint32_t nrows
Definition: TCamera.h:40
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
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:203
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
STL namespace.
std::string dumpAsText() const
Dumps all the parameters as a multi-line string, with the same format than saveToConfigFile.
Definition: TCamera.cpp:34
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: TCamera.cpp:52
void loadFromConfigFile(const std::string &section, const mrpt::config::CConfigFileBase &cfg)
Load all the params from a config source, in the format used in saveToConfigFile(), that is:
Definition: TCamera.cpp:165
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates): ...
Definition: TCamera.h:50
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
double focalLengthMeters
The focal length of the camera, in meters (can be used among &#39;intrinsicParams&#39; to determine the pixel...
Definition: TCamera.h:56
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
CVectorDynamic< double > CVectorDouble
mxArray * convertVectorToMatlab(const CONTAINER &vec)
Convert std::vector<> or std::deque<> of numeric types into Matlab vectors.
Definition: utils_matlab.h:50
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
std::array< double, 8 > dist
[k1 k2 t1 t2 k3 k4 k5 k6] -> k_i: parameters of radial distortion, t_i: parameters of tangential dist...
Definition: TCamera.h:53
struct mxArray_tag mxArray
Forward declaration for mxArray (avoid #including as much as possible to speed up compiling) ...
Definition: CSerializable.h:18
void write(const std::string &section, const std::string &name, enum_t value, const int name_padding_width=-1, const int value_padding_width=-1, const std::string &comment=std::string())
bool operator==(const mrpt::img::TCamera &a, const mrpt::img::TCamera &b)
Definition: TCamera.cpp:231
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: TCamera.cpp:41
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
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:90
void saveToConfigFile(const std::string &section, mrpt::config::CConfigFileBase &cfg) const
Save as a config block:
Definition: TCamera.cpp:143
This file implements matrix/vector text and binary serialization.
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
bool operator!=(const mrpt::img::TCamera &a, const mrpt::img::TCamera &b)
Definition: TCamera.cpp:238
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: TCamera.cpp:42
uint32_t ncols
Camera resolution.
Definition: TCamera.h:40
mxArray * convertToMatlab(const MATRIX &mat)
Convert vectors, arrays and matrices into Matlab vectors/matrices.
Definition: utils_matlab.h:35
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 ...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020