MRPT  2.0.1
CObservationStereoImages.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, 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 "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/CMatrixF.h>
15 
16 #if MRPT_HAS_MATLAB
17 #include <mexplus/mxarray.h>
18 #endif
19 #include <algorithm> // all_of
20 
21 using namespace mrpt::obs;
22 using namespace mrpt::math;
23 using namespace mrpt::poses;
24 using namespace mrpt::img;
25 
26 // This must be added to any CSerializable class implementation file.
28 
29 uint8_t CObservationStereoImages::serializeGetVersion() const { return 6; }
32 {
33  // The data
34  out << cameraPose << leftCamera << rightCamera << imageLeft;
35  out << hasImageDisparity << hasImageRight;
36  if (hasImageRight) out << imageRight;
37  if (hasImageDisparity) out << imageDisparity;
38  out << timestamp;
39  out << rightCameraPose;
40  out << sensorLabel;
41 }
42 
44  mrpt::serialization::CArchive& in, uint8_t version)
45 {
46  switch (version)
47  {
48  case 6:
49  {
50  in >> cameraPose >> leftCamera >> rightCamera >> imageLeft;
51  in >> hasImageDisparity >> hasImageRight;
52  if (hasImageRight) in >> imageRight;
53  if (hasImageDisparity) in >> imageDisparity;
54  in >> timestamp;
55  in >> rightCameraPose;
56  in >> sensorLabel;
57  }
58  break;
59 
60  case 0:
61  case 1:
62  case 2:
63  case 3:
64  case 4:
65  case 5:
66  {
67  // This, for backwards compatibility before version 6:
68  hasImageRight = true;
69  hasImageDisparity = false;
70 
71  if (version < 5)
72  {
73  CPose3D aux;
74  in >> aux;
75  cameraPose = CPose3DQuat(aux);
76  }
77 
78  if (version >= 5)
79  {
80  in >> cameraPose >> leftCamera >> rightCamera;
81  }
82  else
83  {
84  CMatrixF intParams;
85  // Get the intrinsic params
86  in >> intParams;
87  // Set them to both cameras
88  leftCamera.intrinsicParams = intParams;
89  // ... distortion parameters are set to zero
90  rightCamera.intrinsicParams = intParams;
91  }
92 
93  in >> imageLeft >> imageRight; // For all the versions
94 
95  if (version >= 1)
96  in >> timestamp;
97  else
98  timestamp = INVALID_TIMESTAMP; // For version 1 to 5
99  if (version >= 2)
100  {
101  if (version < 5)
102  {
103  CPose3D aux;
104  in >> aux;
105  rightCameraPose = CPose3DQuat(aux);
106  }
107  else
108  in >> rightCameraPose;
109  }
110  else
111  rightCameraPose = CPose3DQuat(
112  0.10f, 0, 0,
114  1, 0, 0, 0)); // For version 1 to 5
115 
116  if (version >= 3 && version < 5) // For versions 3 & 4
117  {
118  double foc;
119  in >> foc; // Get the focal length in meters
120  leftCamera.focalLengthMeters = rightCamera.focalLengthMeters =
121  foc; // ... and set it to both cameras
122  }
123  else if (version < 3)
124  leftCamera.focalLengthMeters = rightCamera.focalLengthMeters =
125  0.002; // For version 0, 1 & 2 (from version 5, this
126  // parameter is included in the TCamera objects)
127 
128  if (version >= 4)
129  in >> sensorLabel;
130  else
131  sensorLabel = ""; // For version 1 to 5
132  }
133  break;
134  default:
136  };
137 }
138 
139 /*---------------------------------------------------------------
140  Implements the writing to a mxArray for Matlab
141  ---------------------------------------------------------------*/
142 #if MRPT_HAS_MATLAB
143 // Add to implement mexplus::from template specialization
145 #endif
146 
148 {
149 #if MRPT_HAS_MATLAB
150  const char* fields[] = {"class", "ts", "sensorLabel", "imageL",
151  "imageR", "poseL", "poseLR", "poseR",
152  "paramsL", "paramsR"};
153  mexplus::MxArray obs_struct(
154  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
155 
156  obs_struct.set("class", this->GetRuntimeClass()->className);
157  obs_struct.set("ts", mrpt::Clock::toDouble(timestamp));
158  obs_struct.set("sensorLabel", this->sensorLabel);
159  obs_struct.set("imageL", this->imageLeft);
160  obs_struct.set("imageR", this->imageRight);
161  obs_struct.set("poseL", this->cameraPose);
162  obs_struct.set("poseR", this->cameraPose + this->rightCameraPose);
163  obs_struct.set("poseLR", this->rightCameraPose);
164  obs_struct.set("paramsL", this->leftCamera);
165  obs_struct.set("paramsR", this->rightCamera);
166  return obs_struct.release();
167 #else
168  THROW_EXCEPTION("MRPT was built without MEX (Matlab) support!");
169 #endif
170 }
171 
172 /** Populates a TStereoCamera structure with the parameters in \a leftCamera, \a
173  * rightCamera and \a rightCameraPose */
175  mrpt::img::TStereoCamera& out_params) const
176 {
177  out_params.leftCamera = this->leftCamera;
178  out_params.rightCamera = this->rightCamera;
179  out_params.rightCameraPose = this->rightCameraPose.asTPose();
180 }
181 
182 /** Sets \a leftCamera, \a rightCamera and \a rightCameraPose from a
183  * TStereoCamera structure */
185  const mrpt::img::TStereoCamera& in_params)
186 {
187  this->leftCamera = in_params.leftCamera;
188  this->rightCamera = in_params.rightCamera;
189  this->rightCameraPose = in_params.rightCameraPose;
190 }
191 
192 /** This method only checks whether ALL the distortion parameters in \a
193  * leftCamera are set to zero, which is
194  * the convention in MRPT to denote that this pair of stereo images has been
195  * rectified.
196  */
198 {
199  return std::all_of(
200  leftCamera.dist.begin(), leftCamera.dist.end(),
201  [](auto v) { return v == 0; });
202 }
203 
204 // Do an efficient swap of all data members of this object with "o".
206 {
208 
209  imageLeft.swap(o.imageLeft);
210  imageRight.swap(o.imageRight);
211  imageDisparity.swap(o.imageDisparity);
212 
213  std::swap(hasImageDisparity, o.hasImageDisparity);
214  std::swap(hasImageRight, o.hasImageRight);
215 
216  std::swap(leftCamera, o.leftCamera);
217  std::swap(rightCamera, o.rightCamera);
218 
219  std::swap(cameraPose, o.cameraPose);
220  std::swap(rightCameraPose, o.rightCameraPose);
221 }
222 
224 {
225  using namespace std;
227 
228  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
229  "base:\n";
230  o << cameraPose.getHomogeneousMatrixVal<CMatrixDouble44>() << "\n"
231  << "Camera pose: " << cameraPose << "\n"
232  << "Camera pose (YPR): " << CPose3D(cameraPose) << "\n"
233  << "\n";
234 
235  mrpt::img::TStereoCamera stParams;
236  getStereoCameraParams(stParams);
237  o << stParams.dumpAsText() << "\n";
238 
239  o << "Right camera pose wrt left camera (YPR):"
240  << "\n"
241  << CPose3D(stParams.rightCameraPose) << "\n";
242 
243  if (imageLeft.isExternallyStored())
244  o << " Left image is stored externally in file: "
245  << imageLeft.getExternalStorageFile() << "\n";
246 
247  o << " Right image";
248  if (hasImageRight)
249  {
250  if (imageRight.isExternallyStored())
251  o << " is stored externally in file: "
252  << imageRight.getExternalStorageFile() << "\n";
253  }
254  else
255  o << " : No.\n";
256 
257  o << " Disparity image";
258  if (hasImageDisparity)
259  {
260  if (imageDisparity.isExternallyStored())
261  o << " is stored externally in file: "
262  << imageDisparity.getExternalStorageFile() << "\n";
263  }
264  else
265  o << " : No.\n";
266 
267  if (!imageLeft.isEmpty())
268  {
269  o << format(
270  " Image size: %ux%u pixels\n", (unsigned int)imageLeft.getWidth(),
271  (unsigned int)imageLeft.getHeight());
272 
273  o << " Channels order: " << imageLeft.getChannelsOrder() << "\n";
274 
275  o << format(
276  " Rows are stored in top-bottom order: %s\n",
277  imageLeft.isOriginTopLeft() ? "YES" : "NO");
278  }
279 }
280 
282 {
283  imageLeft.forceLoad();
284  imageRight.forceLoad();
285  imageDisparity.forceLoad();
286 }
static double toDouble(const time_point t) noexcept
Converts a timestamp to a UNIX time_t-like number, with fractional part.
Definition: Clock.cpp:106
bool hasImageRight
Whether imageRight actually contains data (Default upon construction: true)
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
mrpt::img::CImage imageDisparity
Disparity image, only contains a valid image if hasImageDisparity == true.
mrpt::img::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
bool areImagesRectified() const
This method only checks whether ALL the distortion parameters in leftCamera are set to zero...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void getStereoCameraParams(mrpt::img::TStereoCamera &out_params) const
Populates a TStereoCamera structure with the parameters in leftCamera, rightCamera and rightCameraPos...
STL namespace.
bool hasImageDisparity
Whether imageDisparity actually contains data (Default upon construction: false)
void swap(CObservation &o)
Swap with another observation, ONLY the data defined here in the base class CObservation.
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
This base provides a set of functions for maths stuff.
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
This namespace contains representation of robot actions and observations.
std::string dumpAsText() const
Dumps all the parameters as a multi-line string, with the same format than saveToConfigFile.
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:45
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
struct mxArray_tag mxArray
Forward declaration for mxArray (avoid #including as much as possible to speed up compiling) ...
Definition: CSerializable.h:18
void setStereoCameraParams(const mrpt::img::TStereoCamera &in_params)
Sets leftCamera, rightCamera and rightCameraPose from a TStereoCamera structure.
void load() const override
Makes sure all images and other fields which may be externally stored are loaded in memory...
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
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:90
void swap(CObservationStereoImages &o)
Do an efficient swap of all data members of this object with "o".
mrpt::poses::CPose3DQuat rightCameraPose
The pose of the right camera, relative to the left one: Note that using the conventional reference co...
TCamera leftCamera
Intrinsic and distortion parameters of the left and right cameras.
Definition: TStereoCamera.h:28
mrpt::math::TPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
Definition: TStereoCamera.h:31
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:44
mrpt::img::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:23
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
mrpt::img::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020