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-2019, 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 
26 /** Dumps all the parameters as a multi-line string, with the same format than
27  * \a saveToConfigFile. \sa saveToConfigFile */
28 std::string TCamera::dumpAsText() const
29 {
31  saveToConfigFile("", cfg);
32  return cfg.getContent();
33 }
34 
37 {
38  out << focalLengthMeters;
39  for (unsigned int k = 0; k < 5; k++) out << dist[k];
40  out << intrinsicParams;
41  // version 0 did serialize here a "CMatrixDouble15"
42  out << nrows << ncols; // New in v2
43 }
45 {
46  switch (version)
47  {
48  case 0:
49  case 1:
50  case 2:
51  {
52  in >> focalLengthMeters;
53 
54  for (unsigned int k = 0; k < 5; k++) in >> dist[k];
55 
56  in >> intrinsicParams;
57 
58  if (version == 0)
59  {
60  CMatrixDouble15 __distortionParams;
61  in >> __distortionParams;
62  }
63 
64  if (version >= 2)
65  in >> nrows >> ncols;
66  else
67  {
68  nrows = 480;
69  ncols = 640;
70  }
71  }
72  break;
73  default:
75  }
76 }
77 
78 /*---------------------------------------------------------------
79  Implements the writing to a mxArray for Matlab
80  ---------------------------------------------------------------*/
81 #if MRPT_HAS_MATLAB
82 // Add to implement mexplus::from template specialization
84 #endif
85 
87 {
88 #if MRPT_HAS_MATLAB
89  const char* fields[] = {"K", "dist", "f", "ncols", "nrows"};
90  mexplus::MxArray params_struct(
91  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
92  params_struct.set("K", mrpt::math::convertToMatlab(this->intrinsicParams));
93  params_struct.set("dist", mrpt::math::convertVectorToMatlab(this->dist));
94  params_struct.set("f", this->focalLengthMeters);
95  params_struct.set("ncols", this->ncols);
96  params_struct.set("nrows", this->nrows);
97  return params_struct.release();
98 #else
99  THROW_EXCEPTION("MRPT built without MATLAB/Mex support");
100 #endif
101 }
102 
103 /** Save as a config block:
104  * \code
105  * [SECTION]
106  * resolution = NCOLS NROWS
107  * cx = CX
108  * cy = CY
109  * fx = FX
110  * fy = FY
111  * dist = K1 K2 T1 T2 T3
112  * focal_length = FOCAL_LENGTH
113  * \endcode
114  */
116  const std::string& section, mrpt::config::CConfigFileBase& cfg) const
117 {
118  cfg.write(
119  section, "resolution",
120  format("[%u %u]", (unsigned int)ncols, (unsigned int)nrows));
121  cfg.write(section, "cx", format("%.05f", cx()));
122  cfg.write(section, "cy", format("%.05f", cy()));
123  cfg.write(section, "fx", format("%.05f", fx()));
124  cfg.write(section, "fy", format("%.05f", fy()));
125  cfg.write(
126  section, "dist",
127  format(
128  "[%e %e %e %e %e]", dist[0], dist[1], dist[2], dist[3], dist[4]));
129  if (focalLengthMeters != 0)
130  cfg.write(section, "focal_length", focalLengthMeters);
131 }
132 
133 /** Load all the params from a config source, in the format described in
134  * saveToConfigFile()
135  */
137  const std::string& section, const mrpt::config::CConfigFileBase& cfg)
138 {
139  vector<uint64_t> out_res;
140  cfg.read_vector(section, "resolution", vector<uint64_t>(), out_res, true);
141  if (out_res.size() != 2)
142  THROW_EXCEPTION("Expected 2-length vector in field 'resolution'");
143  ncols = out_res[0];
144  nrows = out_res[1];
145 
146  double fx, fy, cx, cy;
147  fx = cfg.read_double(section, "fx", 0, true);
148  fy = cfg.read_double(section, "fy", 0, true);
149  cx = cfg.read_double(section, "cx", 0, true);
150  cy = cfg.read_double(section, "cy", 0, true);
151 
152  if (fx < 2.0) fx *= ncols;
153  if (fy < 2.0) fy *= nrows;
154  if (cx < 2.0) cx *= ncols;
155  if (cy < 2.0) cy *= nrows;
156 
157  setIntrinsicParamsFromValues(fx, fy, cx, cy);
158 
159  CVectorDouble dists;
160  cfg.read_vector(section, "dist", CVectorDouble(), dists, true);
161  if (dists.size() != 4 && dists.size() != 5)
162  THROW_EXCEPTION("Expected 4 or 5-length vector in field 'dist'");
163 
164  dist.fill(0);
165  for (CVectorDouble::Index i = 0; i < dists.size(); i++) dist[i] = dists[i];
166 
167  focalLengthMeters =
168  cfg.read_double(section, "focal_length", 0, false /* optional value */);
169 }
170 
171 /** Rescale all the parameters for a new camera resolution (it raises an
172  * exception if the aspect ratio is modified, which is not permitted).
173  */
174 void TCamera::scaleToResolution(unsigned int new_ncols, unsigned int new_nrows)
175 {
176  if (ncols == new_ncols && nrows == new_nrows) return; // already done
177 
178  ASSERT_(new_nrows > 0 && new_ncols > 0);
179 
180  const double prev_aspect_ratio = ncols / double(nrows);
181  const double new_aspect_ratio = new_ncols / double(new_nrows);
182 
183  ASSERTMSG_(
184  std::abs(prev_aspect_ratio - new_aspect_ratio) < 1e-3,
185  "TCamera: Trying to scale camera parameters for a resolution of "
186  "different aspect ratio.");
187 
188  const double K = new_ncols / double(ncols);
189 
190  ncols = new_ncols;
191  nrows = new_nrows;
192 
193  // fx fy cx cy
194  intrinsicParams(0, 0) *= K;
195  intrinsicParams(1, 1) *= K;
196  intrinsicParams(0, 2) *= K;
197  intrinsicParams(1, 2) *= K;
198 
199  // distortion params: unmodified.
200 }
201 
203  const mrpt::img::TCamera& a, const mrpt::img::TCamera& b)
204 {
205  return a.ncols == b.ncols && a.nrows == b.nrows &&
206  a.intrinsicParams == b.intrinsicParams && a.dist == b.dist &&
207  a.focalLengthMeters == b.focalLengthMeters;
208 }
210  const mrpt::img::TCamera& a, const mrpt::img::TCamera& b)
211 {
212  return !(a == b);
213 }
This class implements a config file-like interface over a memory-stored string list.
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:174
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
#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.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: TCamera.cpp:44
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:136
unsigned char uint8_t
Definition: rptypes.h:44
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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
GLubyte GLubyte b
Definition: glext.h:6372
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:25
#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
GLsizei const GLchar ** string
Definition: glext.h:4116
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:202
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: TCamera.cpp:35
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:53
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:115
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
This file implements matrix/vector text and binary serialization.
void getContent(std::string &str) const
Return the current contents of the virtual "config file".
GLuint in
Definition: glext.h:7391
bool operator!=(const mrpt::img::TCamera &a, const mrpt::img::TCamera &b)
Definition: TCamera.cpp:209
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: TCamera.cpp:36
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
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: a429c6f2f Mon Aug 19 15:45:44 2019 +0200 at lun ago 19 15:50:16 CEST 2019