MRPT  1.9.9
TCamera.h
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 #pragma once
10 
12 #include <mrpt/math/CMatrixFixed.h>
14 #include <array>
15 
16 namespace mrpt::img
17 {
18 /** Parameters for the Brown-Conrady camera lens distortion model.
19  * The parameters obtained for one camera resolution can be used for any other
20  * resolution by means of the method TCamera::scaleToResolution()
21  *
22  * \sa The application camera-calib-gui for calibrating a camera
23  * \ingroup mrpt_img_grp
24  */
26 {
28 
29  // This must be added for declaration of MEX-related functions
31 
32  public:
33  TCamera();
34 
35  /** @name Camera parameters
36  @{ */
37 
38  /** Camera resolution */
39  uint32_t ncols{640}, nrows{480};
40 
41  /** Matrix of intrinsic parameters (containing the focal length and
42  * principal point coordinates):
43  *
44  * [ fx 0 cx ]
45  * A = [ 0 fy cy ]
46  * [ 0 0 1 ]
47  *
48  */
50  /** [k1 k2 t1 t2 k3 k4 k5 k6] -> k_i: parameters of radial distortion, t_i:
51  * parameters of tangential distortion (default=0) */
52  std::array<double, 8> dist{{.0, .0, .0, .0, .0, .0, .0, .0}};
53  /** The focal length of the camera, in meters (can be used among
54  * 'intrinsicParams' to determine the pixel size). */
55  double focalLengthMeters{.0};
56 
57  /** @} */
58 
59  /** Rescale all the parameters for a new camera resolution (it raises an
60  * exception if the aspect ratio is modified, which is not permitted).
61  */
62  void scaleToResolution(unsigned int new_ncols, unsigned int new_nrows);
63 
64  /** Save as a config block:
65  * \code
66  * [SECTION]
67  * resolution = [NCOLS NROWS]
68  * cx = CX
69  * cy = CY
70  * fx = FX
71  * fy = FY
72  * dist = [K1 K2 T1 T2 K3]
73  * focal_length = FOCAL_LENGTH
74  * \endcode
75  */
76  void saveToConfigFile(
77  const std::string& section, mrpt::config::CConfigFileBase& cfg) const;
78 
79  /** Load all the params from a config source, in the format used in
80  * saveToConfigFile(), that is:
81  *
82  * \code
83  * [SECTION]
84  * resolution = [NCOLS NROWS]
85  * cx = CX
86  * cy = CY
87  * fx = FX
88  * fy = FY
89  * dist = [K1 K2 T1 T2 K3]
90  * focal_length = FOCAL_LENGTH [optional field]
91  * \endcode
92  * \exception std::exception on missing fields
93  */
94  void loadFromConfigFile(
95  const std::string& section, const mrpt::config::CConfigFileBase& cfg);
96  /** overload This signature is consistent with the rest of MRPT APIs */
97  inline void loadFromConfigFile(
98  const mrpt::config::CConfigFileBase& cfg, const std::string& section)
99  {
100  loadFromConfigFile(section, cfg);
101  }
102 
103  /** Dumps all the parameters as a multi-line string, with the same format
104  * than \a saveToConfigFile. \sa saveToConfigFile */
105  std::string dumpAsText() const;
106 
107  /** Set the matrix of intrinsic params of the camera from the individual
108  * values of focal length and principal point coordinates (in pixels)
109  */
111  double fx, double fy, double cx, double cy)
112  {
114  intrinsicParams(0, 0) = fx;
115  intrinsicParams(1, 1) = fy;
116  intrinsicParams(0, 2) = cx;
117  intrinsicParams(1, 2) = cy;
118  intrinsicParams(2, 2) = 1.0;
119  }
120 
121  /** Get the vector of distortion params of the camera */
123  mrpt::math::CMatrixDouble15& distParVector) const
124  {
125  for (size_t i = 0; i < distParVector.size(); i++)
126  distParVector(0, i) = dist[i];
127  }
128 
129  /** Get a vector with the distortion params of the camera */
130  inline std::vector<double> getDistortionParamsAsVector() const
131  {
132  std::vector<double> v(8);
133  for (size_t i = 0; i < 8; i++) v[i] = dist[i];
134  return v;
135  }
136 
137  /** Set the whole vector of distortion params of the camera */
139  const mrpt::math::CMatrixDouble15& distParVector)
140  {
141  dist.fill(0);
142  for (size_t i = 0; i < 5; i++) dist[i] = distParVector(0, i);
143  }
144 
145  /** Set the whole vector of distortion params of the camera from a 4, 5, or
146  * 8-vector (see definition of \a dist for parameter order) */
147  template <class VECTORLIKE>
148  void setDistortionParamsVector(const VECTORLIKE& distParVector)
149  {
150  auto N = static_cast<size_t>(distParVector.size());
151  ASSERT_(N == 4 || N == 5 || N == 8);
152  dist.fill(0); // Default values
153  for (size_t i = 0; i < N; i++) dist[i] = distParVector[i];
154  }
155 
156  /** Set the vector of distortion params of the camera from the individual
157  * values of the distortion coefficients
158  */
160  double k1, double k2, double p1, double p2, double k3 = 0)
161  {
162  dist[0] = k1;
163  dist[1] = k2;
164  dist[2] = p1;
165  dist[3] = p2;
166  dist[4] = k3;
167  }
168 
169  /** Get the value of the principal point x-coordinate (in pixels). */
170  inline double cx() const { return intrinsicParams(0, 2); }
171  /** Get the value of the principal point y-coordinate (in pixels). */
172  inline double cy() const { return intrinsicParams(1, 2); }
173  /** Get the value of the focal length x-value (in pixels). */
174  inline double fx() const { return intrinsicParams(0, 0); }
175  /** Get the value of the focal length y-value (in pixels). */
176  inline double fy() const { return intrinsicParams(1, 1); }
177  /** Set the value of the principal point x-coordinate (in pixels). */
178  inline void cx(double val) { intrinsicParams(0, 2) = val; }
179  /** Set the value of the principal point y-coordinate (in pixels). */
180  inline void cy(double val) { intrinsicParams(1, 2) = val; }
181  /** Set the value of the focal length x-value (in pixels). */
182  inline void fx(double val) { intrinsicParams(0, 0) = val; }
183  /** Set the value of the focal length y-value (in pixels). */
184  inline void fy(double val) { intrinsicParams(1, 1) = val; }
185  /** Get the value of the k1 distortion parameter. */
186  inline double k1() const { return dist[0]; }
187  /** Get the value of the k2 distortion parameter. */
188  inline double k2() const { return dist[1]; }
189  /** Get the value of the p1 distortion parameter. */
190  inline double p1() const { return dist[2]; }
191  /** Get the value of the p2 distortion parameter. */
192  inline double p2() const { return dist[3]; }
193  /** Get the value of the k3 distortion parameter. */
194  inline double k3() const { return dist[4]; }
195  /** Get the value of the k4 distortion parameter. */
196  inline double k4() const { return dist[5]; }
197  /** Get the value of the k5 distortion parameter. */
198  inline double k5() const { return dist[6]; }
199  /** Get the value of the k6 distortion parameter. */
200  inline double k6() const { return dist[7]; }
201 
202  /** Set the value of the k1 distortion parameter. */
203  inline void k1(double val) { dist[0] = val; }
204  /** Set the value of the k2 distortion parameter. */
205  inline void k2(double val) { dist[1] = val; }
206  /** Set the value of the p1 distortion parameter. */
207  inline void p1(double val) { dist[2] = val; }
208  /** Set the value of the p2 distortion parameter. */
209  inline void p2(double val) { dist[3] = val; }
210  /** Set the value of the k3 distortion parameter. */
211  inline void k3(double val) { dist[4] = val; }
212  /** Set the value of the k4 distortion parameter. */
213  inline void k4(double val) { dist[5] = val; }
214  /** Set the value of the k5 distortion parameter. */
215  inline void k5(double val) { dist[6] = val; }
216  /** Set the value of the k6 distortion parameter. */
217  inline void k6(double val) { dist[7] = val; }
218 
219 }; // end class TCamera
220 
221 bool operator==(const mrpt::img::TCamera& a, const mrpt::img::TCamera& b);
222 bool operator!=(const mrpt::img::TCamera& a, const mrpt::img::TCamera& b);
223 
224 } // namespace mrpt::img
225 // Add for declaration of mexplus::from template specialization
226 DECLARE_MEXPLUS_FROM(mrpt::img::TCamera) // Not working at the beginning?
double k3() const
Get the value of the k3 distortion parameter.
Definition: TCamera.h:194
uint32_t nrows
Definition: TCamera.h:39
void k2(double val)
Set the value of the k2 distortion parameter.
Definition: TCamera.h:205
void loadFromConfigFile(const mrpt::config::CConfigFileBase &cfg, const std::string &section)
overload This signature is consistent with the rest of MRPT APIs
Definition: TCamera.h:97
constexpr matrix_size_t size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Definition: CMatrixFixed.h:233
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
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:174
double k6() const
Get the value of the k6 distortion parameter.
Definition: TCamera.h:200
void setDistortionParamsFromValues(double k1, double k2, double p1, double p2, double k3=0)
Set the vector of distortion params of the camera from the individual values of the distortion coeffi...
Definition: TCamera.h:159
#define DECLARE_MEXPLUS_FROM(complete_type)
This must be inserted if a custom conversion method for MEX API is implemented in the class...
void p2(double val)
Set the value of the p2 distortion parameter.
Definition: TCamera.h:209
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:176
void fy(double val)
Set the value of the focal length y-value (in pixels).
Definition: TCamera.h:184
std::string dumpAsText() const
Dumps all the parameters as a multi-line string, with the same format than saveToConfigFile.
Definition: TCamera.cpp:34
void k1(double val)
Set the value of the k1 distortion parameter.
Definition: TCamera.h:203
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
double k2() const
Get the value of the k2 distortion parameter.
Definition: TCamera.h:188
void k3(double val)
Set the value of the k3 distortion parameter.
Definition: TCamera.h:211
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates): ...
Definition: TCamera.h:49
#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:55
This class allows loading and storing values and vectors of different types from a configuration text...
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:172
#define DECLARE_MEX_CONVERSION
This must be inserted if a custom conversion method for MEX API is implemented in the class...
void setDistortionParamsVector(const mrpt::math::CMatrixDouble15 &distParVector)
Set the whole vector of distortion params of the camera.
Definition: TCamera.h:138
void getDistortionParamsVector(mrpt::math::CMatrixDouble15 &distParVector) const
Get the vector of distortion params of the camera.
Definition: TCamera.h:122
int val
Definition: mrpt_jpeglib.h:957
GLubyte GLubyte b
Definition: glext.h:6372
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:25
void cy(double val)
Set the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:180
GLsizei const GLchar ** string
Definition: glext.h:4116
double p1() const
Get the value of the p1 distortion parameter.
Definition: TCamera.h:190
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:52
void k4(double val)
Set the value of the k4 distortion parameter.
Definition: TCamera.h:213
double k5() const
Get the value of the k5 distortion parameter.
Definition: TCamera.h:198
bool operator==(const mrpt::img::TCamera &a, const mrpt::img::TCamera &b)
Definition: TCamera.cpp:231
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:170
const GLdouble * v
Definition: glext.h:3684
void k6(double val)
Set the value of the k6 distortion parameter.
Definition: TCamera.h:217
void saveToConfigFile(const std::string &section, mrpt::config::CConfigFileBase &cfg) const
Save as a config block:
Definition: TCamera.cpp:143
void setDistortionParamsVector(const VECTORLIKE &distParVector)
Set the whole vector of distortion params of the camera from a 4, 5, or 8-vector (see definition of d...
Definition: TCamera.h:148
void fx(double val)
Set the value of the focal length x-value (in pixels).
Definition: TCamera.h:182
double k4() const
Get the value of the k4 distortion parameter.
Definition: TCamera.h:196
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
void p1(double val)
Set the value of the p1 distortion parameter.
Definition: TCamera.h:207
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void cx(double val)
Set the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:178
double p2() const
Get the value of the p2 distortion parameter.
Definition: TCamera.h:192
double k1() const
Get the value of the k1 distortion parameter.
Definition: TCamera.h:186
void setIntrinsicParamsFromValues(double fx, double fy, double cx, double cy)
Set the matrix of intrinsic params of the camera from the individual values of focal length and princ...
Definition: TCamera.h:110
bool operator!=(const mrpt::img::TCamera &a, const mrpt::img::TCamera &b)
Definition: TCamera.cpp:238
GLubyte GLubyte GLubyte a
Definition: glext.h:6372
uint32_t ncols
Camera resolution.
Definition: TCamera.h:39
void k5(double val)
Set the value of the k5 distortion parameter.
Definition: TCamera.h:215
std::vector< double > getDistortionParamsAsVector() const
Get a vector with the distortion params of the camera.
Definition: TCamera.h:130



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 45d659fbb Tue Dec 10 18:21:14 2019 +0100 at mar dic 10 18:30:09 CET 2019