Main MRPT website > C++ reference for MRPT 1.4.0
TCamera.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, 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#ifndef TCamera_H
10#define TCamera_H
11
18
19namespace mrpt
20{
21 namespace utils
22 {
23 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( TCamera, mrpt::utils::CSerializable )
24
25 /** Structure to hold the parameters of a pinhole camera model.
26 * The parameters obtained for one camera resolution can be used for any other resolution by means of the method TCamera::scaleToResolution()
27 *
28 * \sa mrpt::vision::CCamModel, the application <a href="http://www.mrpt.org/Application:camera-calib-gui" >camera-calib-gui</a> for calibrating a camera
29 * \ingroup mrpt_base_grp
30 */
31 class BASE_IMPEXP TCamera : public mrpt::utils::CSerializable
32 {
34
35 // This must be added for declaration of MEX-related functions
37
38 public:
39 TCamera() : ncols(640), nrows(480), focalLengthMeters(0)
40 {
41 intrinsicParams.set_unsafe(0,0,507.808);
42 intrinsicParams.set_unsafe(1,1,507.808);
43 intrinsicParams.set_unsafe(0,2,356.2368);
44 intrinsicParams.set_unsafe(1,2,252.9216);
45 intrinsicParams.set_unsafe(2,2,1);
46 for (size_t i=0;i<dist.SizeAtCompileTime ;i++)
47 dist[i] = 0;
48 }
49
50 /** @name Camera parameters
51 @{ */
52
53 uint32_t ncols,nrows; //!< Camera resolution
54 mrpt::math::CMatrixDouble33 intrinsicParams; //!< Matrix of intrinsic parameters (containing the focal length and principal point coordinates)
55 mrpt::math::CArrayDouble<5> dist; //!< [k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (default=0)
56 double focalLengthMeters; //!< The focal length of the camera, in meters (can be used among 'intrinsicParams' to determine the pixel size).
57
58 /** @} */
59
60 /** Rescale all the parameters for a new camera resolution (it raises an 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( const std::string &section, mrpt::utils::CConfigFileBase &cfg ) const;
77
78 /** Load all the params from a config source, in the format used in saveToConfigFile(), that is:
79 *
80 * \code
81 * [SECTION]
82 * resolution = [NCOLS NROWS]
83 * cx = CX
84 * cy = CY
85 * fx = FX
86 * fy = FY
87 * dist = [K1 K2 T1 T2 K3]
88 * focal_length = FOCAL_LENGTH [optional field]
89 * \endcode
90 * \exception std::exception on missing fields
91 */
92 void loadFromConfigFile(const std::string &section, const mrpt::utils::CConfigFileBase &cfg );
93 /** \overload This signature is consistent with the rest of MRPT APIs */
94 inline void loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg,const std::string &section) { loadFromConfigFile(section,cfg); }
95
96 /** Dumps all the parameters as a multi-line string, with the same format than \a saveToConfigFile. \sa saveToConfigFile */
97 std::string dumpAsText() const;
98
99
100 /** Set the matrix of intrinsic params of the camera from the individual values of focal length and principal point coordinates (in pixels)
101 */
102 inline void setIntrinsicParamsFromValues ( double fx, double fy, double cx, double cy )
103 {
104 intrinsicParams.set_unsafe( 0, 0, fx );
105 intrinsicParams.set_unsafe( 1, 1, fy );
106 intrinsicParams.set_unsafe( 0, 2, cx );
107 intrinsicParams.set_unsafe( 1, 2, cy );
108 }
109
110 /** Get the vector of distortion params of the camera */
111 inline void getDistortionParamsVector ( mrpt::math::CMatrixDouble15 &distParVector ) const
112 {
113 for (size_t i=0;i<5;i++)
114 distParVector.set_unsafe(0,i, dist[i]);
115 }
116
117 /** Get a vector with the distortion params of the camera */
118 inline std::vector<double> getDistortionParamsAsVector () const {
119 std::vector<double> v(5);
120 for (size_t i=0;i<5;i++)
121 v[i] = dist[i];
122 return v;
123 }
124
125 /** Set the whole vector of distortion params of the camera */
127 {
128 for (size_t i=0;i<5;i++)
129 dist[i] = distParVector.get_unsafe(0,i);
130 }
131
132 /** Set the whole vector of distortion params of the camera from a 4 or 5-vector */
133 template <class VECTORLIKE>
134 void setDistortionParamsVector( const VECTORLIKE &distParVector )
135 {
136 size_t N = static_cast<size_t>(distParVector.size());
137 ASSERT_(N==4 || N==5)
138 dist[4] = 0; // Default value
139 for (size_t i=0;i<N;i++) dist[i] = distParVector[i];
140 }
141
142 /** Set the vector of distortion params of the camera from the individual values of the distortion coefficients
143 */
144 inline void setDistortionParamsFromValues( double k1, double k2, double p1, double p2, double k3 = 0 )
145 {
146 dist[0] = k1;
147 dist[1] = k2;
148 dist[2] = p1;
149 dist[3] = p2;
150 dist[4] = k3;
151 }
152
153 /** Get the value of the principal point x-coordinate (in pixels). */
154 inline double cx() const { return intrinsicParams(0,2); }
155 /** Get the value of the principal point y-coordinate (in pixels). */
156 inline double cy() const { return intrinsicParams(1,2); }
157 /** Get the value of the focal length x-value (in pixels). */
158 inline double fx() const { return intrinsicParams(0,0); }
159 /** Get the value of the focal length y-value (in pixels). */
160 inline double fy() const { return intrinsicParams(1,1); }
161
162 /** Set the value of the principal point x-coordinate (in pixels). */
163 inline void cx(double val) { intrinsicParams(0,2)=val; }
164 /** Set the value of the principal point y-coordinate (in pixels). */
165 inline void cy(double val) { intrinsicParams(1,2)=val; }
166 /** Set the value of the focal length x-value (in pixels). */
167 inline void fx(double val) { intrinsicParams(0,0)=val; }
168 /** Set the value of the focal length y-value (in pixels). */
169 inline void fy(double val) { intrinsicParams(1,1)=val; }
170
171 /** Get the value of the k1 distortion parameter. */
172 inline double k1() const { return dist[0]; }
173 /** Get the value of the k2 distortion parameter. */
174 inline double k2() const { return dist[1]; }
175 /** Get the value of the p1 distortion parameter. */
176 inline double p1() const { return dist[2]; }
177 /** Get the value of the p2 distortion parameter. */
178 inline double p2() const { return dist[3]; }
179 /** Get the value of the k3 distortion parameter. */
180 inline double k3() const { return dist[4]; }
181
182 /** Get the value of the k1 distortion parameter. */
183 inline void k1(double val) { dist[0]=val; }
184 /** Get the value of the k2 distortion parameter. */
185 inline void k2(double val) { dist[1]=val; }
186 /** Get the value of the p1 distortion parameter. */
187 inline void p1(double val) { dist[2]=val; }
188 /** Get the value of the p2 distortion parameter. */
189 inline void p2(double val) { dist[3]=val; }
190 /** Get the value of the k3 distortion parameter. */
191 inline void k3(double val) { dist[4]=val; }
192 }; // end class TCamera
193 DEFINE_SERIALIZABLE_POST_CUSTOM_BASE( TCamera, mrpt::utils::CSerializable )
194
195
196 bool BASE_IMPEXP operator ==(const mrpt::utils::TCamera& a, const mrpt::utils::TCamera& b);
197 bool BASE_IMPEXP operator !=(const mrpt::utils::TCamera& a, const mrpt::utils::TCamera& b);
198
199 } // End of namespace
200} // end of namespace
201
202// Add for declaration of mexplus::from template specialization
203DECLARE_MEXPLUS_FROM( mrpt::utils::TCamera ) // Not working at the beginning?
204#endif
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE(class_name, base_name)
#define DECLARE_MEXPLUS_FROM(complete_type)
This must be inserted if a custom conversion method for MEX API is implemented in the class.
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(class_name, base_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
#define DECLARE_MEX_CONVERSION
This must be inserted if a custom conversion method for MEX API is implemented in the class.
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:75
A numeric matrix of compile-time fixed size.
This class allows loading and storing values and vectors of different types from a configuration text...
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:32
double focalLengthMeters
The focal length of the camera, in meters (can be used among 'intrinsicParams' to determine the pixel...
Definition: TCamera.h:56
void p2(double val)
Get the value of the p2 distortion parameter.
Definition: TCamera.h:189
void fy(double val)
Set the value of the focal length y-value (in pixels).
Definition: TCamera.h:169
void setDistortionParamsVector(const mrpt::math::CMatrixDouble15 &distParVector)
Set the whole vector of distortion params of the camera.
Definition: TCamera.h:126
void k2(double val)
Get the value of the k2 distortion parameter.
Definition: TCamera.h:185
std::string dumpAsText() const
Dumps all the parameters as a multi-line string, with the same format than saveToConfigFile.
void fx(double val)
Set the value of the focal length x-value (in pixels).
Definition: TCamera.h:167
mrpt::math::CArrayDouble< 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
Definition: TCamera.h:55
void cy(double val)
Set the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:165
void saveToConfigFile(const std::string &section, mrpt::utils::CConfigFileBase &cfg) const
Save as a config block:
void k1(double val)
Get the value of the k1 distortion parameter.
Definition: TCamera.h:183
uint32_t ncols
Definition: TCamera.h:53
void k3(double val)
Get the value of the k3 distortion parameter.
Definition: TCamera.h:191
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &cfg, const std::string &section)
Definition: TCamera.h:94
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(),...
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates)
Definition: TCamera.h:54
void getDistortionParamsVector(mrpt::math::CMatrixDouble15 &distParVector) const
Get the vector of distortion params of the camera
Definition: TCamera.h:111
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...
double cx() const
Get the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:154
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:144
void cx(double val)
Set the value of the principal point x-coordinate (in pixels).
Definition: TCamera.h:163
double k3() const
Get the value of the k3 distortion parameter.
Definition: TCamera.h:180
double fx() const
Get the value of the focal length x-value (in pixels).
Definition: TCamera.h:158
double p2() const
Get the value of the p2 distortion parameter.
Definition: TCamera.h:178
void p1(double val)
Get the value of the p1 distortion parameter.
Definition: TCamera.h:187
double p1() const
Get the value of the p1 distortion parameter.
Definition: TCamera.h:176
double cy() const
Get the value of the principal point y-coordinate (in pixels).
Definition: TCamera.h:156
double fy() const
Get the value of the focal length y-value (in pixels).
Definition: TCamera.h:160
double k1() const
Get the value of the k1 distortion parameter.
Definition: TCamera.h:172
std::vector< double > getDistortionParamsAsVector() const
Get a vector with the distortion params of the camera
Definition: TCamera.h:118
double k2() const
Get the value of the k2 distortion parameter.
Definition: TCamera.h:174
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:102
void setDistortionParamsVector(const VECTORLIKE &distParVector)
Set the whole vector of distortion params of the camera from a 4 or 5-vector.
Definition: TCamera.h:134
#define ASSERT_(f)
Definition: mrpt_macros.h:261
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
unsigned long uint32_t
Definition: pstdint.h:216



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Sun Nov 27 03:17:04 UTC 2022