Main MRPT website > C++ reference for MRPT 1.5.6
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  {
136  in >> cameraPose >> leftCamera >> rightCamera;
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 
188 mxArray* CObservationStereoImages::writeToMatlab() const
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)
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 'intrinsicParams' to determine the pixel...
Definition: TCamera.h:56
bool isOriginTopLeft() const
Returns true if the coordinates origin is top-left, or false if it is bottom-left.
Definition: CImage.cpp:927
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
std::string getExternalStorageFile() const MRPT_NO_THROWS
< Only if isExternallyStored() returns true.
Definition: CImage.h:673
mrpt::math::CMatrixDouble44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:191
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's protected since it'll be only called from child classes that should know what else to swap appart from these common data.
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...
GLuint in
Definition: glew.h:7146
virtual void getDescriptionAsText(std::ostream &o) const
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
#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 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
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
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...
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
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...
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
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'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...
mrpt::utils::CImage imageDisparity
Disparity image, only contains a valid image if hasImageDisparity == true.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
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
const char * getChannelsOrder() const
Returns a string of the form "BGR","RGB" or "GRAY" indicating the channels ordering.
Definition: CImage.cpp:1180
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
bool areImagesRectified() const
This method only checks whether ALL the distortion parameters in leftCamera are set to zero...
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
void getStereoCameraParams(mrpt::utils::TStereoCamera &out_params) const
Populates a TStereoCamera structure with the parameters in leftCamera, rightCamera and rightCameraPos...
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018