MRPT  1.9.9
CObservationStereoImagesFeatures.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 
14 #include <fstream>
15 
16 using namespace mrpt::obs;
17 using namespace mrpt::poses;
18 using namespace mrpt::math;
19 using namespace mrpt::img;
20 using namespace std;
21 
22 // This must be added to any CSerializable class implementation file.
25 
27  const TCamera& cLeft, const TCamera& cRight, const CPose3DQuat& rCPose,
28  const CPose3DQuat& cPORobot)
29 {
30  cameraLeft = cLeft;
31  cameraRight = cRight;
32 
33  rightCameraPose = rCPose;
34  cameraPoseOnRobot = cPORobot;
35 }
36 
38  const std::string& filename)
39 {
40  std::ofstream file(filename);
41  ASSERT_(file.is_open());
42 
43  vector<TStereoImageFeatures>::iterator it;
44  for (it = theFeatures.begin(); it != theFeatures.end(); ++it)
45  file << format(
46  "%u %.2f %.2f %.2f %.2f\n", it->ID, it->pixels.first.x,
47  it->pixels.first.y, it->pixels.second.x, it->pixels.second.y);
48 }
49 
51 {
52  return 0;
53 }
56 {
57  // The data
58  out << cameraLeft;
59  out << cameraRight;
60  out << rightCameraPose << cameraPoseOnRobot;
61  out << (uint32_t)theFeatures.size(); // Write the number of items
62  // within the feature list
63  for (const auto& theFeature : theFeatures)
64  {
65  out << theFeature.pixels.first.x << theFeature.pixels.first.y;
66  out << theFeature.pixels.second.x << theFeature.pixels.second.y;
67  out << (uint32_t)theFeature.ID;
68  }
69  out << sensorLabel << timestamp;
70 }
71 
74 {
75  switch (version)
76  {
77  case 0:
78  {
79  uint32_t nL, nR;
80  in >> cameraLeft;
81  in >> cameraRight;
82  in >> rightCameraPose >> cameraPoseOnRobot;
83  in >> nL;
84  theFeatures.resize(nL);
85  for (auto& theFeature : theFeatures)
86  {
87  in >> theFeature.pixels.first.x >> theFeature.pixels.first.y;
88  in >> theFeature.pixels.second.x >> theFeature.pixels.second.y;
89  in >> nR;
90  theFeature.ID = (unsigned int)nR;
91  }
92  in >> sensorLabel >> timestamp;
93  }
94  break;
95  default:
97  };
98 }
99 
101  std::ostream& o) const
102 {
104 
105  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
106  "base:\n";
107  o << cameraPoseOnRobot.getHomogeneousMatrixVal<CMatrixDouble44>()
108  << cameraPoseOnRobot << endl;
109 
110  o << "Homogeneous matrix for the RIGHT camera's 3D pose, relative to LEFT "
111  "camera reference system:\n";
112  o << rightCameraPose.getHomogeneousMatrixVal<CMatrixDouble44>()
113  << rightCameraPose << endl;
114 
115  o << "Intrinsic parameters matrix for the LEFT camera:" << endl;
116  CMatrixDouble33 aux = cameraLeft.intrinsicParams;
117  o << aux.inMatlabFormat() << endl << aux << endl;
118 
119  o << "Distortion parameters vector for the LEFT camera:" << endl << "[ ";
120  for (unsigned int i = 0; i < 5; ++i) o << cameraLeft.dist[i] << " ";
121  o << "]" << endl;
122 
123  o << "Intrinsic parameters matrix for the RIGHT camera:" << endl;
124  aux = cameraRight.intrinsicParams;
125  o << aux.inMatlabFormat() << endl << aux << endl;
126 
127  o << "Distortion parameters vector for the RIGHT camera:" << endl << "[ ";
128  for (unsigned int i = 0; i < 5; ++i) o << cameraRight.dist[i] << " ";
129  o << "]" << endl;
130 
131  o << endl
132  << format(
133  " Image size: %ux%u pixels\n", (unsigned int)cameraLeft.ncols,
134  (unsigned int)cameraLeft.nrows);
135  o << endl
136  << format(
137  " Number of features in images: %u\n",
138  (unsigned int)theFeatures.size());
139 }
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...
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
Declares a class derived from "CObservation" that encapsules a pair of cameras and a set of matched i...
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
IMPLEMENTS_SERIALIZABLE(CObservationStereoImagesFeatures, CObservation, mrpt::obs) CObservationStereoImagesFeatures
STL namespace.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
unsigned char uint8_t
Definition: rptypes.h:44
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
This namespace contains representation of robot actions and observations.
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:25
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
GLsizei const GLchar ** string
Definition: glext.h:4116
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:53
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
GLuint in
Definition: glext.h:7391
std::string inMatlabFormat(const std::size_t decimal_digits=6) const
Exports the matrix as a string compatible with Matlab/Octave.
GLenum GLsizei GLenum format
Definition: glext.h:3535
unsigned __int32 uint32_t
Definition: rptypes.h:50
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: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at miƩ ago 21 11:50:11 CEST 2019