MRPT  1.9.9
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-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 
10 #include "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/CMatrixF.h>
15 #if MRPT_HAS_MATLAB
16 #include <mexplus/mxarray.h>
17 #endif
18 #include <algorithm> // all_of
19 
20 using namespace mrpt::obs;
21 using namespace mrpt::math;
22 using namespace mrpt::poses;
23 using namespace mrpt::img;
24 
25 // This must be added to any CSerializable class implementation file.
27 
28 uint8_t CObservationStereoImages::serializeGetVersion() const { return 6; }
31 {
32  // The data
33  out << cameraPose << leftCamera << rightCamera << imageLeft;
34  out << hasImageDisparity << hasImageRight;
35  if (hasImageRight) out << imageRight;
36  if (hasImageDisparity) out << imageDisparity;
37  out << timestamp;
38  out << rightCameraPose;
39  out << sensorLabel;
40 }
41 
43  mrpt::serialization::CArchive& in, uint8_t version)
44 {
45  switch (version)
46  {
47  case 6:
48  {
49  in >> cameraPose >> leftCamera >> rightCamera >> imageLeft;
50  in >> hasImageDisparity >> hasImageRight;
51  if (hasImageRight) in >> imageRight;
52  if (hasImageDisparity) in >> imageDisparity;
53  in >> timestamp;
54  in >> rightCameraPose;
55  in >> sensorLabel;
56  }
57  break;
58 
59  case 0:
60  case 1:
61  case 2:
62  case 3:
63  case 4:
64  case 5:
65  {
66  // This, for backwards compatibility before version 6:
67  hasImageRight = true;
68  hasImageDisparity = false;
69 
70  if (version < 5)
71  {
72  CPose3D aux;
73  in >> aux;
74  cameraPose = CPose3DQuat(aux);
75  }
76 
77  if (version >= 5)
78  {
79  in >> cameraPose >> leftCamera >> rightCamera;
80  }
81  else
82  {
83  CMatrixF intParams;
84  // Get the intrinsic params
85  in >> intParams;
86  // Set them to both cameras
87  leftCamera.intrinsicParams = intParams;
88  // ... distortion parameters are set to zero
89  rightCamera.intrinsicParams = intParams;
90  }
91 
92  in >> imageLeft >> imageRight; // For all the versions
93 
94  if (version >= 1)
95  in >> timestamp;
96  else
97  timestamp = INVALID_TIMESTAMP; // For version 1 to 5
98  if (version >= 2)
99  {
100  if (version < 5)
101  {
102  CPose3D aux;
103  in >> aux;
104  rightCameraPose = CPose3DQuat(aux);
105  }
106  else
107  in >> rightCameraPose;
108  }
109  else
110  rightCameraPose = CPose3DQuat(
111  0.10f, 0, 0,
113  1, 0, 0, 0)); // For version 1 to 5
114 
115  if (version >= 3 && version < 5) // For versions 3 & 4
116  {
117  double foc;
118  in >> foc; // Get the focal length in meters
119  leftCamera.focalLengthMeters = rightCamera.focalLengthMeters =
120  foc; // ... and set it to both cameras
121  }
122  else if (version < 3)
123  leftCamera.focalLengthMeters = rightCamera.focalLengthMeters =
124  0.002; // For version 0, 1 & 2 (from version 5, this
125  // parameter is included in the TCamera objects)
126 
127  if (version >= 4)
128  in >> sensorLabel;
129  else
130  sensorLabel = ""; // For version 1 to 5
131  }
132  break;
133  default:
135  };
136 }
137 
138 /*---------------------------------------------------------------
139  Implements the writing to a mxArray for Matlab
140  ---------------------------------------------------------------*/
141 #if MRPT_HAS_MATLAB
142 // Add to implement mexplus::from template specialization
144 #endif
145 
147 {
148 #if MRPT_HAS_MATLAB
149  const char* fields[] = {"class", "ts", "sensorLabel", "imageL",
150  "imageR", "poseL", "poseLR", "poseR",
151  "paramsL", "paramsR"};
152  mexplus::MxArray obs_struct(
153  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
154 
155  obs_struct.set("class", this->GetRuntimeClass()->className);
156  obs_struct.set("ts", mrpt::Clock::toDouble(timestamp));
157  obs_struct.set("sensorLabel", this->sensorLabel);
158  obs_struct.set("imageL", this->imageLeft);
159  obs_struct.set("imageR", this->imageRight);
160  obs_struct.set("poseL", this->cameraPose);
161  obs_struct.set("poseR", this->cameraPose + this->rightCameraPose);
162  obs_struct.set("poseLR", this->rightCameraPose);
163  obs_struct.set("paramsL", this->leftCamera);
164  obs_struct.set("paramsR", this->rightCamera);
165  return obs_struct.release();
166 #else
167  THROW_EXCEPTION("MRPT was built without MEX (Matlab) support!");
168 #endif
169 }
170 
171 /** Populates a TStereoCamera structure with the parameters in \a leftCamera, \a
172  * rightCamera and \a rightCameraPose */
174  mrpt::img::TStereoCamera& out_params) const
175 {
176  out_params.leftCamera = this->leftCamera;
177  out_params.rightCamera = this->rightCamera;
178  out_params.rightCameraPose = this->rightCameraPose.asTPose();
179 }
180 
181 /** Sets \a leftCamera, \a rightCamera and \a rightCameraPose from a
182  * TStereoCamera structure */
184  const mrpt::img::TStereoCamera& in_params)
185 {
186  this->leftCamera = in_params.leftCamera;
187  this->rightCamera = in_params.rightCamera;
188  this->rightCameraPose = in_params.rightCameraPose;
189 }
190 
191 /** This method only checks whether ALL the distortion parameters in \a
192  * leftCamera are set to zero, which is
193  * the convention in MRPT to denote that this pair of stereo images has been
194  * rectified.
195  */
197 {
198  return std::all_of(
199  leftCamera.dist.begin(), leftCamera.dist.end(),
200  [](auto v) { return v == 0; });
201 }
202 
203 // Do an efficient swap of all data members of this object with "o".
205 {
207 
208  imageLeft.swap(o.imageLeft);
209  imageRight.swap(o.imageRight);
210  imageDisparity.swap(o.imageDisparity);
211 
212  std::swap(hasImageDisparity, o.hasImageDisparity);
213  std::swap(hasImageRight, o.hasImageRight);
214 
215  std::swap(leftCamera, o.leftCamera);
216  std::swap(rightCamera, o.rightCamera);
217 
218  std::swap(cameraPose, o.cameraPose);
219  std::swap(rightCameraPose, o.rightCameraPose);
220 }
221 
223 {
224  using namespace std;
226 
227  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
228  "base:\n";
229  o << cameraPose.getHomogeneousMatrixVal<CMatrixDouble44>() << "\n"
230  << "Camera pose: " << cameraPose << "\n"
231  << "Camera pose (YPR): " << CPose3D(cameraPose) << "\n"
232  << "\n";
233 
234  mrpt::img::TStereoCamera stParams;
235  getStereoCameraParams(stParams);
236  o << stParams.dumpAsText() << "\n";
237 
238  o << "Right camera pose wrt left camera (YPR):"
239  << "\n"
240  << CPose3D(stParams.rightCameraPose) << "\n";
241 
242  if (imageLeft.isExternallyStored())
243  o << " Left image is stored externally in file: "
244  << imageLeft.getExternalStorageFile() << "\n";
245 
246  o << " Right image";
247  if (hasImageRight)
248  {
249  if (imageRight.isExternallyStored())
250  o << " is stored externally in file: "
251  << imageRight.getExternalStorageFile() << "\n";
252  }
253  else
254  o << " : No.\n";
255 
256  o << " Disparity image";
257  if (hasImageDisparity)
258  {
259  if (imageDisparity.isExternallyStored())
260  o << " is stored externally in file: "
261  << imageDisparity.getExternalStorageFile() << "\n";
262  }
263  else
264  o << " : No.\n";
265 
266  if (!imageLeft.isEmpty())
267  {
268  o << format(
269  " Image size: %ux%u pixels\n", (unsigned int)imageLeft.getWidth(),
270  (unsigned int)imageLeft.getHeight());
271 
272  o << " Channels order: " << imageLeft.getChannelsOrder() << "\n";
273 
274  o << format(
275  " Rows are stored in top-bottom order: %s\n",
276  imageLeft.isOriginTopLeft() ? "YES" : "NO");
277  }
278 }
static double toDouble(const time_point t) noexcept
Converts a timestamp to a UNIX time_t-like number, with fractional part.
Definition: Clock.cpp:58
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.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
const GLdouble * v
Definition: glext.h:3684
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...
GLuint in
Definition: glext.h:7391
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 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019