Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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>
14 #include <mrpt/utils/CStream.h>
15 #if MRPT_HAS_MATLAB
16 # include <mexplus/mxarray.h>
17 #endif
18 
19 using namespace mrpt::obs;
20 using namespace mrpt::math;
21 using namespace mrpt::utils;
22 using namespace mrpt::poses;
23 
24 // This must be added to any CSerializable class implementation file.
26 
27 /*---------------------------------------------------------------
28  Constructor
29  ---------------------------------------------------------------*/
30 CObservationStereoImages::CObservationStereoImages( void *iplImageLeft,void *iplImageRight, void *iplImageDisparity,bool ownMemory ) :
31  imageLeft( UNINITIALIZED_IMAGE ),
32  imageRight( UNINITIALIZED_IMAGE ),
33  imageDisparity( UNINITIALIZED_IMAGE ),
34  hasImageDisparity( iplImageDisparity!=NULL ),
35  hasImageRight( iplImageRight!=NULL )
36 {
37  if (iplImageLeft)
38  ownMemory ? imageLeft.setFromIplImage(iplImageLeft) : imageLeft.loadFromIplImage(iplImageLeft);
39  if (iplImageRight)
40  ownMemory ? imageRight.setFromIplImage(iplImageRight) : imageRight.loadFromIplImage(iplImageRight);
41  if (iplImageDisparity)
42  ownMemory ? imageDisparity.setFromIplImage(iplImageDisparity) : imageDisparity.loadFromIplImage(iplImageDisparity);
43 }
44 
45 /*---------------------------------------------------------------
46  Default Constructor
47  ---------------------------------------------------------------*/
49  hasImageDisparity(false),
50  hasImageRight(true)
51 {
52 }
53 
54 /*---------------------------------------------------------------
55  Destructor
56  ---------------------------------------------------------------*/
58 {
59 }
60 
61 /*---------------------------------------------------------------
62  Implements the writing to a CStream capability of CSerializable objects
63  ---------------------------------------------------------------*/
65 {
66  if (version)
67  *version = 6 ;
68  else
69  {
70  // The data
71  out << cameraPose << leftCamera << rightCamera
72  << imageLeft;
73 
75 
76  if (hasImageRight)
77  out << imageRight;
78 
80  out << imageDisparity;
81 
82  out << timestamp;
83  out << rightCameraPose;
84  out << sensorLabel;
85 
86  }
87 }
88 
89 /*---------------------------------------------------------------
90  Implements the reading from a CStream capability of CSerializable objects
91  ---------------------------------------------------------------*/
93 {
94  switch(version)
95  {
96  case 6:
97  {
99  >> imageLeft;
100 
102 
103  if (hasImageRight)
104  in >> imageRight;
105 
106  if (hasImageDisparity)
107  in >> imageDisparity;
108 
109  in >> timestamp;
110  in >> rightCameraPose;
111  in >> sensorLabel;
112  }
113  break;
114 
115  case 0:
116  case 1:
117  case 2:
118  case 3:
119  case 4:
120  case 5:
121  {
122  // This, for backwards compatibility before version 6:
123  hasImageRight = true;
124  hasImageDisparity = false;
125 
126 
127  if( version < 5 )
128  {
129  CPose3D aux;
130  in >> aux;
131  cameraPose = CPose3DQuat( aux );
132  }
133 
134  if( version >= 5 )
135  {
137  }
138  else
139  {
140  CMatrix intParams;
141  in >> intParams; // Get the intrinsic params
142  leftCamera.intrinsicParams = CMatrixDouble33(intParams); // Set them to both cameras
143  rightCamera.intrinsicParams = CMatrixDouble33(intParams); // ... distortion parameters are set to zero
144  }
145 
146  in >> imageLeft >> imageRight; // For all the versions
147 
148  if(version >= 1) in >> timestamp; else timestamp = INVALID_TIMESTAMP; // For version 1 to 5
149  if(version >= 2)
150  {
151  if(version < 5)
152  {
153  CPose3D aux;
154  in >> aux;
155  rightCameraPose = CPose3DQuat( aux );
156  }
157  else
158  in >> rightCameraPose;
159  }
160  else
161  rightCameraPose = CPose3DQuat( 0.10f, 0, 0, mrpt::math::CQuaternionDouble(1,0,0,0) ); // For version 1 to 5
162 
163  if(version >= 3 && version < 5) // For versions 3 & 4
164  {
165  double foc;
166  in >> foc; // Get the focal length in meters
167  leftCamera.focalLengthMeters = rightCamera.focalLengthMeters = foc; // ... and set it to both cameras
168  }
169  else
170  if( version < 3 )
171  leftCamera.focalLengthMeters = rightCamera.focalLengthMeters = 0.002; // For version 0, 1 & 2 (from version 5, this parameter is included in the TCamera objects)
172 
173  if(version >= 4) in >> sensorLabel; else sensorLabel = ""; // For version 1 to 5
174 
175  } break;
176  default:
178  };
179 }
180 
181 /*---------------------------------------------------------------
182  Implements the writing to a mxArray for Matlab
183  ---------------------------------------------------------------*/
184 #if MRPT_HAS_MATLAB
185 // Add to implement mexplus::from template specialization
187 
189 {
190  const char* fields[] = {"class",
191  "ts","sensorLabel",
192  "imageL","imageR",
193  "poseL","poseLR","poseR",
194  "paramsL","paramsR"};
195  mexplus::MxArray obs_struct( mexplus::MxArray::Struct(sizeof(fields)/sizeof(fields[0]),fields) );
196 
197  obs_struct.set("class", this->GetRuntimeClass()->className);
198  obs_struct.set("ts", this->timestamp);
199  obs_struct.set("sensorLabel", this->sensorLabel);
200  obs_struct.set("imageL", this->imageLeft);
201  obs_struct.set("imageR", this->imageRight);
202  obs_struct.set("poseL", this->cameraPose);
203  obs_struct.set("poseR", this->cameraPose + this->rightCameraPose);
204  obs_struct.set("poseLR", this->rightCameraPose);
205  obs_struct.set("paramsL", this->leftCamera);
206  obs_struct.set("paramsR", this->rightCamera);
207  return obs_struct.release();
208 }
209 #endif
210 
211 /** Populates a TStereoCamera structure with the parameters in \a leftCamera, \a rightCamera and \a rightCameraPose */
213 {
214  out_params.leftCamera = this->leftCamera;
215  out_params.rightCamera = this->rightCamera;
216  out_params.rightCameraPose = this->rightCameraPose;
217 }
218 
219 /** Sets \a leftCamera, \a rightCamera and \a rightCameraPose from a TStereoCamera structure */
221 {
222  this->leftCamera = in_params.leftCamera;
223  this->rightCamera = in_params.rightCamera;
224  this->rightCameraPose = in_params.rightCameraPose;
225 }
226 
227 /** This method only checks whether ALL the distortion parameters in \a leftCamera are set to zero, which is
228  * the convention in MRPT to denote that this pair of stereo images has been rectified.
229  */
231 {
232  return (leftCamera.dist.array()==0).all();
233 }
234 
235 // Do an efficient swap of all data members of this object with "o".
237 {
239 
243 
244  std::swap(hasImageDisparity, o.hasImageDisparity);
245  std::swap(hasImageRight, o.hasImageRight);
246 
247  std::swap(leftCamera,o.leftCamera);
248  std::swap(rightCamera, o.rightCamera);
249 
250  std::swap(cameraPose, o.cameraPose);
251  std::swap(rightCameraPose, o.rightCameraPose);
252 }
253 
255 {
256  using namespace std;
258 
259  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot base:\n";
260  o << cameraPose.getHomogeneousMatrixVal() << endl
261  << "Camera pose: " << cameraPose << endl
262  << "Camera pose (YPR): " << CPose3D(cameraPose) << endl
263  << endl;
264 
266  getStereoCameraParams(stParams);
267  o << stParams.dumpAsText() << endl;
268 
269  o << "Right camera pose wrt left camera (YPR):" << endl << CPose3D(stParams.rightCameraPose) << endl;
270 
272  o << " Left image is stored externally in file: " << imageLeft.getExternalStorageFile() << endl;
273 
274  o << " Right image";
275  if (hasImageRight )
276  {
278  o << " is stored externally in file: " << imageRight.getExternalStorageFile() << endl;
279  }
280  else o << " : No.\n";
281 
282  o << " Disparity image";
283  if (hasImageDisparity )
284  {
286  o << " is stored externally in file: " << imageDisparity.getExternalStorageFile() << endl;
287  }
288  else o << " : No.\n";
289 
290  o << format(" Image size: %ux%u pixels\n", (unsigned int)imageLeft.getWidth(), (unsigned int)imageLeft.getHeight() );
291 
292  o << " Channels order: " << imageLeft.getChannelsOrder() << endl;
293 
294  o << format(" Rows are stored in top-bottom order: %s\n", imageLeft.isOriginTopLeft() ? "YES" : "NO");
295 }
296 
297 
bool hasImageRight
Whether imageRight actually contains data (Default upon construction: true)
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
mrpt::utils::CImage imageLeft
Image from the left camera (this image will be ALWAYS present)
double focalLengthMeters
The focal length of the camera, in meters (can be used among &#39;intrinsicParams&#39; to determine the pixel...
Definition: TCamera.h:56
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.
const char * getChannelsOrder() const
Returns a string of the form "BGR","RGB" or "GRAY" indicating the channels ordering.
Definition: CImage.cpp:1180
std::string getExternalStorageFile() const MRPT_NO_THROWS
< Only if isExternallyStored() returns true.
Definition: CImage.h:673
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. It&#39;s protected since it&#39;ll be only called from child classes that should know what else to swap appart from these common data.
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:191
void getDescriptionAsText(std::ostream &o) const MRPT_OVERRIDE
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
Structure to hold the parameters of a pinhole stereo camera model.
Definition: TStereoCamera.h:25
mrpt::poses::CPose3DQuat cameraPose
The pose of the LEFT camera, relative to the robot.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
TCamera rightCamera
Intrinsic and distortion parameters of the left and right cameras.
Definition: TStereoCamera.h:29
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
std::string dumpAsText() const
Dumps all the parameters as a multi-line string, with the same format than saveToConfigFile.
This namespace contains representation of robot actions and observations.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
#define IMPLEMENTS_MEXPLUS_FROM(complete_type)
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:48
bool isExternallyStored() const MRPT_NO_THROWS
See setExternalStorage().
Definition: CImage.h:671
int version
Definition: mrpt_jpeglib.h:898
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
void getStereoCameraParams(mrpt::utils::TStereoCamera &out_params) const
Populates a TStereoCamera structure with the parameters in leftCamera, rightCamera and rightCameraPos...
void swap(CImage &o)
Very efficient swap of two images (just swap the internal pointers)
Definition: CImage.cpp:135
mrpt::utils::TCamera leftCamera
Parameters for the left/right cameras: individual intrinsic and distortion parameters of the cameras...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::utils::CImage imageRight
Image from the right camera, only contains a valid image if hasImageRight == true.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
mrpt::math::CMatrixDouble33 intrinsicParams
Matrix of intrinsic parameters (containing the focal length and principal point coordinates) ...
Definition: TCamera.h:54
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const
Returns information about the class of an object in runtime.
void setStereoCameraParams(const mrpt::utils::TStereoCamera &in_params)
Sets leftCamera, rightCamera and rightCameraPose from a TStereoCamera structure.
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
mrpt::math::CArrayDouble< 5 > dist
[k1 k2 t1 t2 k3] -> k_i: parameters of radial distortion, t_i: parameters of tangential distortion (d...
Definition: TCamera.h:55
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:79
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents any robot&#39;s observation.
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:6301
bool isOriginTopLeft() const
Returns true if the coordinates origin is top-left, or false if it is bottom-left.
Definition: CImage.cpp:927
mrpt::utils::CImage imageDisparity
Disparity image, only contains a valid image if hasImageDisparity == true.
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:43
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:30
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
size_t getWidth() const MRPT_OVERRIDE
Returns the width of the image in pixels.
Definition: CImage.cpp:855
struct mxArray_tag mxArray
Forward declaration for mxArray (avoid #including as much as possible to speed up compiling) ...
Definition: CSerializable.h:17
mrpt::poses::CPose3DQuat rightCameraPose
Pose of the right camera with respect to the coordinate origin of the left camera.
Definition: TStereoCamera.h:30
size_t getHeight() const MRPT_OVERRIDE
Returns the height of the image in pixels.
Definition: CImage.cpp:884
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.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019