MRPT  1.9.9
CObservationStereoImages.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, 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 
10 #include "obs-precomp.h" // Precompiled headers
11 
13 #include <mrpt/math/CMatrix.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 /*---------------------------------------------------------------
29  Constructor
30  ---------------------------------------------------------------*/
32  void* iplImageLeft, void* iplImageRight, void* iplImageDisparity,
33  bool ownMemory)
34  : imageLeft(UNINITIALIZED_IMAGE),
35  imageRight(UNINITIALIZED_IMAGE),
36  imageDisparity(UNINITIALIZED_IMAGE),
37  hasImageDisparity(iplImageDisparity != nullptr),
38  hasImageRight(iplImageRight != nullptr)
39 {
40  if (iplImageLeft)
41  ownMemory ? imageLeft.setFromIplImage(iplImageLeft)
42  : imageLeft.loadFromIplImage(iplImageLeft);
43  if (iplImageRight)
44  ownMemory ? imageRight.setFromIplImage(iplImageRight)
45  : imageRight.loadFromIplImage(iplImageRight);
46  if (iplImageDisparity)
47  ownMemory ? imageDisparity.setFromIplImage(iplImageDisparity)
48  : imageDisparity.loadFromIplImage(iplImageDisparity);
49 }
50 
54 {
55  // The data
56  out << cameraPose << leftCamera << rightCamera << imageLeft;
57  out << hasImageDisparity << hasImageRight;
58  if (hasImageRight) out << imageRight;
59  if (hasImageDisparity) out << imageDisparity;
60  out << timestamp;
61  out << rightCameraPose;
62  out << sensorLabel;
63 }
64 
67 {
68  switch (version)
69  {
70  case 6:
71  {
72  in >> cameraPose >> leftCamera >> rightCamera >> imageLeft;
73  in >> hasImageDisparity >> hasImageRight;
74  if (hasImageRight) in >> imageRight;
75  if (hasImageDisparity) in >> imageDisparity;
76  in >> timestamp;
77  in >> rightCameraPose;
78  in >> sensorLabel;
79  }
80  break;
81 
82  case 0:
83  case 1:
84  case 2:
85  case 3:
86  case 4:
87  case 5:
88  {
89  // This, for backwards compatibility before version 6:
90  hasImageRight = true;
91  hasImageDisparity = false;
92 
93  if (version < 5)
94  {
95  CPose3D aux;
96  in >> aux;
97  cameraPose = CPose3DQuat(aux);
98  }
99 
100  if (version >= 5)
101  {
102  in >> cameraPose >> leftCamera >> rightCamera;
103  }
104  else
105  {
106  CMatrix intParams;
107  in >> intParams; // Get the intrinsic params
108  leftCamera.intrinsicParams =
109  CMatrixDouble33(intParams); // Set them to both cameras
110  rightCamera.intrinsicParams = CMatrixDouble33(
111  intParams); // ... distortion parameters are set to zero
112  }
113 
114  in >> imageLeft >> imageRight; // For all the versions
115 
116  if (version >= 1)
117  in >> timestamp;
118  else
119  timestamp = INVALID_TIMESTAMP; // For version 1 to 5
120  if (version >= 2)
121  {
122  if (version < 5)
123  {
124  CPose3D aux;
125  in >> aux;
126  rightCameraPose = CPose3DQuat(aux);
127  }
128  else
129  in >> rightCameraPose;
130  }
131  else
132  rightCameraPose = CPose3DQuat(
133  0.10f, 0, 0, mrpt::math::CQuaternionDouble(
134  1, 0, 0, 0)); // For version 1 to 5
135 
136  if (version >= 3 && version < 5) // For versions 3 & 4
137  {
138  double foc;
139  in >> foc; // Get the focal length in meters
140  leftCamera.focalLengthMeters = rightCamera.focalLengthMeters =
141  foc; // ... and set it to both cameras
142  }
143  else if (version < 3)
144  leftCamera.focalLengthMeters = rightCamera.focalLengthMeters =
145  0.002; // For version 0, 1 & 2 (from version 5, this
146  // parameter is included in the TCamera objects)
147 
148  if (version >= 4)
149  in >> sensorLabel;
150  else
151  sensorLabel = ""; // For version 1 to 5
152  }
153  break;
154  default:
156  };
157 }
158 
159 /*---------------------------------------------------------------
160  Implements the writing to a mxArray for Matlab
161  ---------------------------------------------------------------*/
162 #if MRPT_HAS_MATLAB
163 // Add to implement mexplus::from template specialization
165 #endif
166 
168 {
169 #if MRPT_HAS_MATLAB
170  const char* fields[] = {"class", "ts", "sensorLabel", "imageL",
171  "imageR", "poseL", "poseLR", "poseR",
172  "paramsL", "paramsR"};
173  mexplus::MxArray obs_struct(
174  mexplus::MxArray::Struct(sizeof(fields) / sizeof(fields[0]), fields));
175 
176  obs_struct.set("class", this->GetRuntimeClass()->className);
177  obs_struct.set("ts", this->timestamp);
178  obs_struct.set("sensorLabel", this->sensorLabel);
179  obs_struct.set("imageL", this->imageLeft);
180  obs_struct.set("imageR", this->imageRight);
181  obs_struct.set("poseL", this->cameraPose);
182  obs_struct.set("poseR", this->cameraPose + this->rightCameraPose);
183  obs_struct.set("poseLR", this->rightCameraPose);
184  obs_struct.set("paramsL", this->leftCamera);
185  obs_struct.set("paramsR", this->rightCamera);
186  return obs_struct.release();
187 #else
188  THROW_EXCEPTION("MRPT was built without MEX (Matlab) support!");
189 #endif
190 }
191 
192 /** Populates a TStereoCamera structure with the parameters in \a leftCamera, \a
193  * rightCamera and \a rightCameraPose */
195  mrpt::img::TStereoCamera& out_params) const
196 {
197  out_params.leftCamera = this->leftCamera;
198  out_params.rightCamera = this->rightCamera;
199  out_params.rightCameraPose = this->rightCameraPose.asTPose();
200 }
201 
202 /** Sets \a leftCamera, \a rightCamera and \a rightCameraPose from a
203  * TStereoCamera structure */
205  const mrpt::img::TStereoCamera& in_params)
206 {
207  this->leftCamera = in_params.leftCamera;
208  this->rightCamera = in_params.rightCamera;
209  this->rightCameraPose = in_params.rightCameraPose;
210 }
211 
212 /** This method only checks whether ALL the distortion parameters in \a
213  * leftCamera are set to zero, which is
214  * the convention in MRPT to denote that this pair of stereo images has been
215  * rectified.
216  */
218 {
219  return std::all_of(
220  leftCamera.dist.begin(), leftCamera.dist.end(),
221  [](auto v) { return v == 0; });
222 }
223 
224 // Do an efficient swap of all data members of this object with "o".
226 {
228 
229  imageLeft.swap(o.imageLeft);
230  imageRight.swap(o.imageRight);
231  imageDisparity.swap(o.imageDisparity);
232 
233  std::swap(hasImageDisparity, o.hasImageDisparity);
234  std::swap(hasImageRight, o.hasImageRight);
235 
236  std::swap(leftCamera, o.leftCamera);
237  std::swap(rightCamera, o.rightCamera);
238 
239  std::swap(cameraPose, o.cameraPose);
240  std::swap(rightCameraPose, o.rightCameraPose);
241 }
242 
244 {
245  using namespace std;
247 
248  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
249  "base:\n";
250  o << cameraPose.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
251  << "Camera pose: " << cameraPose << endl
252  << "Camera pose (YPR): " << CPose3D(cameraPose) << endl
253  << endl;
254 
255  mrpt::img::TStereoCamera stParams;
256  getStereoCameraParams(stParams);
257  o << stParams.dumpAsText() << endl;
258 
259  o << "Right camera pose wrt left camera (YPR):" << endl
260  << CPose3D(stParams.rightCameraPose) << endl;
261 
262  if (imageLeft.isExternallyStored())
263  o << " Left image is stored externally in file: "
264  << imageLeft.getExternalStorageFile() << endl;
265 
266  o << " Right image";
267  if (hasImageRight)
268  {
269  if (imageRight.isExternallyStored())
270  o << " is stored externally in file: "
271  << imageRight.getExternalStorageFile() << endl;
272  }
273  else
274  o << " : No.\n";
275 
276  o << " Disparity image";
277  if (hasImageDisparity)
278  {
279  if (imageDisparity.isExternallyStored())
280  o << " is stored externally in file: "
281  << imageDisparity.getExternalStorageFile() << endl;
282  }
283  else
284  o << " : No.\n";
285 
286  o << format(
287  " Image size: %ux%u pixels\n", (unsigned int)imageLeft.getWidth(),
288  (unsigned int)imageLeft.getHeight());
289 
290  o << " Channels order: " << imageLeft.getChannelsOrder() << endl;
291 
292  o << format(
293  " Rows are stored in top-bottom order: %s\n",
294  imageLeft.isOriginTopLeft() ? "YES" : "NO");
295 }
bool hasImageRight
Whether imageRight actually contains data (Default upon construction: true)
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:41
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)
This must be inserted in 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...
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
A numeric matrix of compile-time fixed size.
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:46
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.
const GLdouble * v
Definition: glext.h:3678
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
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:68
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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...
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:57
GLuint in
Definition: glext.h:7274
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
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:22
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020