Debian Codegraph @

libs/img/src/TCamera.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, 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 //
12 #include <mrpt/config/CConfigFileMemory.h>
13 #include <mrpt/containers/yaml.h>
14 #include <mrpt/img/TCamera.h>
15 #include <mrpt/math/CVectorDynamic.h>
16 #include <mrpt/math/matrix_serialization.h> // For "<<" ">>" operators.
17 #include <mrpt/math/utils_matlab.h>
18
19 using namespace mrpt::img;
20 using namespace mrpt::math;
21 using namespace std;
22
23 /* Implements serialization for the TCamera struct as it will be included within
24 * CObservations objects */
25 IMPLEMENTS_SERIALIZABLE(TCamera, CSerializable, mrpt::img)
26
27 TCamera::TCamera()
28 {
29 // Ensure intrinsics matrix has a 1 in the bottom-right corner:
30 setIntrinsicParamsFromValues(0, 0, 0, 0);
31 }
32
33 /** Dumps all the parameters as a multi-line string, with the same format than
34 * \a saveToConfigFile. \sa saveToConfigFile */
35 std::string TCamera::dumpAsText() const
36 {
37 mrpt::config::CConfigFileMemory cfg;
38 saveToConfigFile("", cfg);
39 return cfg.getContent();
40 }
41
42 uint8_t TCamera::serializeGetVersion() const { return 6; }
43 void TCamera::serializeTo(mrpt::serialization::CArchive& out) const
44 {
45 out << focalLengthMeters;
46 // v3: from 5 to 8 dist params:
47 for (size_t k = 0; k < dist.size(); k++) out << dist[k];
48 // v4: only store the 4 relevant values:
49 out << fx() << fy() << cx() << cy();
50 // version 0 did serialize here a "CMatrixDouble15"
51 out << nrows << ncols; // New in v2
52 out << cameraName; // v5
53 out << static_cast<uint8_t>(distortion); // v6
54 }
55 void TCamera::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
56 {
57 switch (version)
58 {
59 case 0:
60 case 1:
61 case 2:
62 case 3:
63 case 4:
64 case 5:
65 case 6:
66 {
67 in >> focalLengthMeters;
68
69 dist.fill(0);
70 for (unsigned int k = 0; k < 5; k++) in >> dist[k];
71 if (version >= 3)
72 for (unsigned int k = 5; k < 8; k++) in >> dist[k];
73
74 if (version < 4)
75 {
76 in >> intrinsicParams;
77 // Enforce values with fixed 0 or 1 values:
78 intrinsicParams(0, 1) = 0;
79 intrinsicParams(1, 0) = 0;
80 intrinsicParams(2, 0) = 0;
81 intrinsicParams(2, 1) = 0;
82 intrinsicParams(2, 2) = 1;
83 }
84 else
85 {
86 double vfx, vfy, vcx, vcy;
87 in >> vfx >> vfy >> vcx >> vcy;
88 setIntrinsicParamsFromValues(vfx, vfy, vcx, vcy);
89 }
90
91 if (version == 0)
92 {
93 CMatrixDouble15 __distortionParams;
94 in >> __distortionParams;
95 }
96
97 if (version >= 2)
98 in >> nrows >> ncols;
99 else
100 {
101 nrows = 480;
102 ncols = 640;
103 }
104 if (version >= 5) in >> cameraName;
105
106 if (version >= 6)
107 {
108 distortion = static_cast<DistortionModel>(in.ReadAs<uint8_t>());
109 }
110 else
111 {
112 distortion = DistortionModel::plumb_bob;
113 }
114 }
115 break;
116 default:
117 MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
118 }
119 }
120
121 /*---------------------------------------------------------------
122 Implements the writing to a mxArray for Matlab
123 ---------------------------------------------------------------*/
124 #if MRPT_HAS_MATLAB
125 // Add to implement mexplus::from template specialization
126 IMPLEMENTS_MEXPLUS_FROM(mrpt::img::TCamera)
127 #endif
128
129 mxArray* TCamera::writeToMatlab() const
130 {
131 #if MRPT_HAS_MATLAB
132 const char* fields[] = {"K", "dist", "f", "ncols", "nrows"};
133 mexplus::MxArray params_struct(
134 mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
135 params_struct.set("K", mrpt::math::convertToMatlab(this->intrinsicParams));
136 params_struct.set("dist", mrpt::math::convertVectorToMatlab(this->dist));
137 params_struct.set("f", this->focalLengthMeters);
138 params_struct.set("ncols", this->ncols);
139 params_struct.set("nrows", this->nrows);
140 return params_struct.release();
141 #else
142 THROW_EXCEPTION("MRPT built without MATLAB/Mex support");
143 #endif
144 }
145
146 /** Save as a config block:
147 * \code
148 * [SECTION]
149 * resolution = NCOLS NROWS
150 * cx = CX
151 * cy = CY
152 * fx = FX
153 * fy = FY
154 * dist = K1 K2 T1 T2 T3
155 * focal_length = FOCAL_LENGTH
156 * \endcode
157 */
158 void TCamera::saveToConfigFile(const std::string& section, mrpt::config::CConfigFileBase& cfg) const
159 {
160 cfg.write(section, "camera_name", cameraName);
161 cfg.write(
162 section, "resolution", mrpt::format("[%u %u]", (unsigned int)ncols, (unsigned int)nrows));
163 cfg.write(section, "cx", mrpt::format("%.05f", cx()));
164 cfg.write(section, "cy", mrpt::format("%.05f", cy()));
165 cfg.write(section, "fx", mrpt::format("%.05f", fx()));
166 cfg.write(section, "fy", mrpt::format("%.05f", fy()));
167
168 cfg.write(section, "distortion_model", distortion);
169 cfg.write(section, "dist", getDistortionParamsAsRowVector().inMatlabFormat());
170
171 if (focalLengthMeters != 0) cfg.write(section, "focal_length", focalLengthMeters);
172 }
173
174 /** Load all the params from a config source, in the format described in
175 * saveToConfigFile()
176 */
177 void TCamera::loadFromConfigFile(
178 const std::string& section, const mrpt::config::CConfigFileBase& cfg)
179 {
180 *this = TCamera(); // reset to defaults
181
182 vector<uint64_t> out_res;
183 cfg.read_vector(section, "resolution", vector<uint64_t>(), out_res, true);
184 if (out_res.size() != 2) THROW_EXCEPTION("Expected 2-length vector in field 'resolution'");
185 ncols = out_res[0];
186 nrows = out_res[1];
187
188 double fx, fy, cx, cy;
189 fx = cfg.read_double(section, "fx", 0, true);
190 fy = cfg.read_double(section, "fy", 0, true);
191 cx = cfg.read_double(section, "cx", 0, true);
192 cy = cfg.read_double(section, "cy", 0, true);
193
194 if (fx < 2.0) fx *= ncols;
195 if (fy < 2.0) fy *= nrows;
196 if (cx < 2.0) cx *= ncols;
197 if (cy < 2.0) cy *= nrows;
198
199 setIntrinsicParamsFromValues(fx, fy, cx, cy);
200
201 distortion = cfg.read_enum(section, "distortion_model", distortion);
202
203 CVectorDouble v;
204 cfg.read_vector(section, "dist", CVectorDouble(), v, true);
205
206 switch (distortion)
207 {
208 case DistortionModel::none:
209 break;
210 case DistortionModel::plumb_bob:
211 ASSERTMSG_(
212 v.size() == 5 || v.size() == 8, "Expected 5 or 8 distortion parameters for plumb_bob");
213 for (int i = 0; i < v.size(); i++) dist[i] = v[i];
214 break;
215 case DistortionModel::kannala_brandt:
216 ASSERTMSG_(v.size() == 4, "Expected 4 distortion parameters for kannala_brandt");
217 k1(v[0]);
218 k2(v[1]);
219 k3(v[2]);
220 k4(v[3]);
221 break;
222 default:
223 {
224 THROW_EXCEPTION("Invalid distortion model enum value.");
225 }
226 }
227
228 focalLengthMeters = cfg.read_double(section, "focal_length", 0, false /* optional value */);
229
230 cameraName = cfg.read_string(section, "camera_name", cameraName, false /* optional value */);
231 }
232
233 /** Rescale all the parameters for a new camera resolution (it raises an
234 * exception if the aspect ratio is modified, which is not permitted).
235 */
236 void TCamera::scaleToResolution(unsigned int new_ncols, unsigned int new_nrows)
237 {
238 if (ncols == new_ncols && nrows == new_nrows) return; // already done
239
240 ASSERT_(new_nrows > 0 && new_ncols > 0);
241
242 const double prev_aspect_ratio = ncols / double(nrows);
243 const double new_aspect_ratio = new_ncols / double(new_nrows);
244
245 ASSERTMSG_(
246 std::abs(prev_aspect_ratio - new_aspect_ratio) < 1e-3,
247 "TCamera: Trying to scale camera parameters for a resolution of "
248 "different aspect ratio.");
249
250 const double K = new_ncols / double(ncols);
251
252 ncols = new_ncols;
253 nrows = new_nrows;
254
255 // fx fy cx cy
256 intrinsicParams(0, 0) *= K;
257 intrinsicParams(1, 1) *= K;
258 intrinsicParams(0, 2) *= K;
259 intrinsicParams(1, 2) *= K;
260
261 // distortion params: unmodified.
262 }
263
264 bool mrpt::img::operator==(const mrpt::img::TCamera& a, const mrpt::img::TCamera& b)
265 {
266 return a.ncols == b.ncols && a.nrows == b.nrows && a.intrinsicParams == b.intrinsicParams &&
267 a.dist == b.dist && a.focalLengthMeters == b.focalLengthMeters &&
268 a.cameraName == b.cameraName && a.distortion == b.distortion;
269 }
270 bool mrpt::img::operator!=(const mrpt::img::TCamera& a, const mrpt::img::TCamera& b)
271 {
272 return !(a == b);
273 }
274
275 TCamera TCamera::FromYAML(const mrpt::containers::yaml& p)
276 {
277 TCamera c;
278
279 c.ncols = p["image_width"].as<uint32_t>();
280 c.nrows = p["image_height"].as<uint32_t>();
281
282 c.cameraName = p["camera_name"].as<std::string>();
283 p["camera_matrix"].toMatrix(c.intrinsicParams);
284
285 {
286 DistortionModel distortion_model = c.distortion;
287 MCP_LOAD_OPT(p, distortion_model);
288 c.distortion = distortion_model;
289 }
290
291 mrpt::math::CMatrixDouble v;
292 p["distortion_coefficients"].toMatrix(v);
293
294 switch (c.distortion)
295 {
296 case DistortionModel::none:
297 break;
298 case DistortionModel::plumb_bob:
299 ASSERTMSG_(
300 v.size() == 5 || v.size() == 8, "Expected 5 or 8 distortion parameters for plumb_bob");
301 for (size_t i = 0; i < v.size(); i++) c.dist[i] = v[i];
302 break;
303 case DistortionModel::kannala_brandt:
304 ASSERTMSG_(v.size() == 4, "Expected 4 distortion parameters for kannala_brandt");
305 c.k1(v[0]);
306 c.k2(v[1]);
307 c.k3(v[2]);
308 c.k4(v[3]);
309 break;
310 default:
311 {
312 THROW_EXCEPTION("Invalid distortion model enum value.");
313 }
314 }
315
316 c.focalLengthMeters = p.getOrDefault<double>("focal_length_meters", 0.0);
317
318 return c;
319 }
320
321 /* Example:
322 ( From: http://wiki.ros.org/camera_calibration_parsers#YAML )
323
324 image_width: 2448
325 image_height: 2050
326 camera_name: prosilica
327 camera_matrix:
328 rows: 3
329 cols: 3
330 data: [4827.94, 0, 1223.5, 0, 4835.62, 1024.5, 0, 0, 1]
331 distortion_model: plumb_bob
332 distortion_coefficients:
333 rows: 1
334 cols: 5
335 data: [-0.41527, 0.31874, -0.00197, 0.00071, 0]
336 rectification_matrix:
337 rows: 3
338 cols: 3
339 data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
340 projection_matrix:
341 rows: 3
342 cols: 4
343 data: [4827.94, 0, 1223.5, 0, 0, 4835.62, 1024.5, 0, 0, 0, 1, 0]
344
345 */
346 mrpt::containers::yaml TCamera::asYAML() const
347 {
348 mrpt::containers::yaml p = mrpt::containers::yaml::Map();
349
350 p["image_width"] = ncols;
351 p["image_height"] = nrows;
352 p["camera_name"] = cameraName;
353 p["camera_matrix"] = mrpt::containers::yaml::FromMatrix(intrinsicParams);
354 p["distortion_model"] = mrpt::typemeta::enum2str(distortion);
355 p["distortion_coefficients"] =
356 mrpt::containers::yaml::FromMatrix(getDistortionParamsAsRowVector());
357 p["rectification_matrix"] =
358 mrpt::containers::yaml::FromMatrix(mrpt::math::CMatrixDouble33::Identity());
359
360 mrpt::math::CMatrixDouble proj(3, 4);
361 proj.setZero();
362 proj.insertMatrix(0, 0, intrinsicParams);
363 p["projection_matrix"] = mrpt::containers::yaml::FromMatrix(proj);
364
365 p["focal_length_meters"] = focalLengthMeters;
366
367 return p;
368 }
369
370 std::vector<double> TCamera::getDistortionParamsAsVector() const
371 {
372 switch (distortion)
373 {
374 case DistortionModel::none:
375 return {};
376 case DistortionModel::plumb_bob:
377 return {k1(), k2(), p1(), p2(), k3(), k4(), k5(), k6()};
378 case DistortionModel::kannala_brandt:
379 return {k1(), k2(), k3(), k4()};
380 default:
381 {
382 THROW_EXCEPTION("Invalid distortion model enum value.");
383 }
384 }
385 }
386
387 mrpt::math::CMatrixDouble TCamera::getDistortionParamsAsRowVector() const
388 {
389 const auto vals = getDistortionParamsAsVector();
390 mrpt::math::CMatrixDouble v;
391 if (!vals.empty())
392 {
393 v.resize(1, vals.size());
394 for (size_t i = 0; i < vals.size(); i++) v(0, i) = vals[i];
395 }
396 return v;
397 }