| 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/img/TStereoCamera.h> |
| 14 |
#include <mrpt/serialization/CArchive.h> |
| 15 |
|
| 16 |
using namespace mrpt::img; |
| 17 |
using namespace mrpt::math; |
| 18 |
using namespace std; |
| 19 |
|
| 20 |
IMPLEMENTS_SERIALIZABLE(TStereoCamera, CSerializable, mrpt::img) |
| 21 |
|
| 22 |
/** Save as a config block: |
| 23 |
*/ |
| 24 |
void TStereoCamera::saveToConfigFile( |
| 25 |
const std::string& section, mrpt::config::CConfigFileBase& cfg) const |
| 26 |
{ |
| 27 |
// [<SECTION>_LEFT] |
| 28 |
// ... |
| 29 |
// [<SECTION>_RIGHT] |
| 30 |
// ... |
| 31 |
// [<SECTION>_LEFT2RIGHT_POSE] |
| 32 |
// pose_quaternion = [x y z qr qx qy qz] |
| 33 |
|
| 34 |
leftCamera.saveToConfigFile(section + string("_LEFT"), cfg); |
| 35 |
rightCamera.saveToConfigFile(section + string("_RIGHT"), cfg); |
| 36 |
|
| 37 |
cfg.write(section + string("_LEFT2RIGHT_POSE"), "pose_quaternion", rightCameraPose.asString()); |
| 38 |
} |
| 39 |
|
| 40 |
/** Load all the params from a config source, in the format described in |
| 41 |
* saveToConfigFile() |
| 42 |
*/ |
| 43 |
void TStereoCamera::loadFromConfigFile( |
| 44 |
const std::string& section, const mrpt::config::CConfigFileBase& cfg) |
| 45 |
{ |
| 46 |
// [<SECTION>_LEFT] |
| 47 |
// ... |
| 48 |
// [<SECTION>_RIGHT] |
| 49 |
// ... |
| 50 |
// [<SECTION>_LEFT2RIGHT_POSE] |
| 51 |
// pose_quaternion = [x y z qr qx qy qz] |
| 52 |
|
| 53 |
leftCamera.loadFromConfigFile(section + string("_LEFT"), cfg); |
| 54 |
rightCamera.loadFromConfigFile(section + string("_RIGHT"), cfg); |
| 55 |
rightCameraPose.fromString( |
| 56 |
cfg.read_string(section + string("_LEFT2RIGHT_POSE"), "pose_quaternion", "")); |
| 57 |
} |
| 58 |
|
| 59 |
uint8_t TStereoCamera::serializeGetVersion() const { return 2; } |
| 60 |
void TStereoCamera::serializeTo(mrpt::serialization::CArchive& out) const |
| 61 |
{ |
| 62 |
// v1->v2: rightCameraPose changed from mrpt::poses::CPose3DQuat to |
| 63 |
// mrpt::math::TPose3DQuat |
| 64 |
out << leftCamera << rightCamera << rightCameraPose; |
| 65 |
} |
| 66 |
void TStereoCamera::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version) |
| 67 |
{ |
| 68 |
switch (version) |
| 69 |
{ |
| 70 |
case 0: |
| 71 |
case 1: |
| 72 |
case 2: |
| 73 |
{ |
| 74 |
if (version == 0) |
| 75 |
{ |
| 76 |
uint8_t _model; |
| 77 |
in >> _model; // unused now |
| 78 |
} |
| 79 |
if (version == 1) |
| 80 |
{ |
| 81 |
// Emulate reading a CPose3DQuat object: |
| 82 |
THROW_EXCEPTION( |
| 83 |
"backwards compatibility de-serialization not implemented " |
| 84 |
"yet!"); |
| 85 |
} |
| 86 |
in >> leftCamera >> rightCamera >> rightCameraPose; |
| 87 |
} |
| 88 |
break; |
| 89 |
default: |
| 90 |
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); |
| 91 |
} |
| 92 |
} |
| 93 |
|
| 94 |
std::string TStereoCamera::dumpAsText() const |
| 95 |
{ |
| 96 |
mrpt::config::CConfigFileMemory cfg; |
| 97 |
saveToConfigFile("", cfg); |
| 98 |
return cfg.getContent(); |
| 99 |
} |