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