Main MRPT website > C++ reference for MRPT 1.5.6
CObservationStereoImagesFeatures.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
12 
14 #include <mrpt/utils/CStream.h>
15 
16 using namespace mrpt::obs;
17 using namespace mrpt::utils;
18 using namespace mrpt::poses;
19 using namespace mrpt::math;
20 using namespace std;
21 
22 // This must be added to any CSerializable class implementation file.
24 
26  cameraLeft(),
27  cameraRight(),
28  rightCameraPose(),
29  cameraPoseOnRobot()
30  {}
31 
33  const CMatrixDouble33 &iPLeft, const CMatrixDouble33 &iPRight,
34  const CArrayDouble<5> &dPLeft, const CArrayDouble<5> &dPRight,
35  const CPose3DQuat &rCPose, const CPose3DQuat &cPORobot )
36 {
37  cameraLeft.intrinsicParams = iPLeft;
38  cameraLeft.dist = dPLeft;
39 
40  cameraRight.intrinsicParams = iPRight;
41  cameraRight.dist = dPRight;
42 
43  rightCameraPose = rCPose;
44  cameraPoseOnRobot = cPORobot;
45 }
46 
48  const TCamera &cLeft, const TCamera &cRight,
49  const CPose3DQuat &rCPose, const CPose3DQuat &cPORobot )
50 {
51  cameraLeft = cLeft;
52  cameraRight = cRight;
53 
54  rightCameraPose = rCPose;
55  cameraPoseOnRobot = cPORobot;
56 }
57 
58 
60 {}
61 
63 {
64  CFileOutputStream file( filename );
65 
67  for( it = theFeatures.begin(); it != theFeatures.end(); ++it )
68  file << format("%u %.2f %.2f %.2f %.2f\n", it->ID, it->pixels.first.x, it->pixels.first.y, it->pixels.second.x, it->pixels.second.y );
69 
70  file.close();
71 }
72 
73 /*---------------------------------------------------------------
74  Implements the writing to a CStream capability of CSerializable objects
75  ---------------------------------------------------------------*/
77 {
78  if (version)
79  *version = 0 ;
80  else
81  {
82  // The data
83  out << cameraLeft;
84  out << cameraRight;
85  out << rightCameraPose << cameraPoseOnRobot;
86  out << (uint32_t)theFeatures.size(); // Write the number of items within the feature list
87  for( unsigned int i = 0; i < theFeatures.size(); ++i )
88  {
89  out << theFeatures[i].pixels.first.x << theFeatures[i].pixels.first.y;
90  out << theFeatures[i].pixels.second.x << theFeatures[i].pixels.second.y;
91  out << (uint32_t)theFeatures[i].ID;
92  }
93  out << sensorLabel << timestamp;
94  }
95 }
96 
97 /*---------------------------------------------------------------
98  Implements the reading from a CStream capability of CSerializable objects
99  ---------------------------------------------------------------*/
101 {
102  switch(version)
103  {
104  case 0:
105  {
106  uint32_t nL, nR;
107  in >> cameraLeft;
108  in >> cameraRight;
109  in >> rightCameraPose >> cameraPoseOnRobot;
110  in >> nL;
111  theFeatures.resize( nL );
112  for( unsigned int i = 0; i < theFeatures.size(); ++i )
113  {
114  in >> theFeatures[i].pixels.first.x >> theFeatures[i].pixels.first.y;
115  in >> theFeatures[i].pixels.second.x >> theFeatures[i].pixels.second.y;
116  in >> nR;
117  theFeatures[i].ID = (unsigned int)nR;
118  }
119  in >> sensorLabel >> timestamp;
120  } break;
121  default:
123  };
124 }
125 
127 {
129 
130  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot base:\n";
131  o << cameraPoseOnRobot.getHomogeneousMatrixVal()
132  << cameraPoseOnRobot << endl;
133 
134  o << "Homogeneous matrix for the RIGHT camera's 3D pose, relative to LEFT camera reference system:\n";
135  o << rightCameraPose.getHomogeneousMatrixVal()
136  << rightCameraPose << endl;
137 
138  o << "Intrinsic parameters matrix for the LEFT camera:"<< endl;
139  CMatrixDouble33 aux = cameraLeft.intrinsicParams;
140  o << aux.inMatlabFormat() << endl << aux << endl;
141 
142  o << "Distortion parameters vector for the LEFT camera:"<< endl << "[ ";
143  for( unsigned int i = 0; i < 5; ++i )
144  o << cameraLeft.dist[i] << " ";
145  o << "]" << endl;
146 
147  o << "Intrinsic parameters matrix for the RIGHT camera:"<< endl;
148  aux = cameraRight.intrinsicParams;
149  o << aux.inMatlabFormat() << endl << aux << endl;
150 
151  o << "Distortion parameters vector for the RIGHT camera:"<< endl << "[ ";
152  for( unsigned int i = 0; i < 5; ++i )
153  o << cameraRight.dist[i] << " ";
154  o << "]"<< endl;
155 
156  o << endl << format(" Image size: %ux%u pixels\n", (unsigned int)cameraLeft.ncols, (unsigned int)cameraLeft.nrows );
157  o << endl << format(" Number of features in images: %u\n", (unsigned int)theFeatures.size() );
158 
159 
160 }
161 
162 
void close()
Close the stream.
void saveFeaturesToTextFile(const std::string &filename)
A method for storing the set of observed features in a text file in the format: ID ul vl ur vr be...
Declares a class derived from "CObservation" that encapsules a pair of cameras and a set of matched i...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
Scalar * iterator
Definition: eigen_plugins.h:23
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
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...
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...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This CStream derived class allow using a file as a write-only, binary stream.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
GLint GLint GLsizei GLsizei GLsizei GLint GLenum format
Definition: glew.h:1168
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
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
GLsizei const GLcharARB ** string
Definition: glew.h:3293
Declares a class that represents any robot's observation.
unsigned __int32 uint32_t
Definition: rptypes.h:49
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:31



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