MRPT  2.0.1
CObservationPointCloud.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-2020, 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 "maps-precomp.h" // Precomp header
11 
20 #include <mrpt/system/filesystem.h>
21 #include <fstream>
22 #include <iostream>
23 
24 using namespace mrpt::obs;
25 
27 
29 {
31  pointcloud->loadFromRangeScan(o);
32 }
33 
35  mrpt::poses::CPose3D& out_sensorPose) const
36 {
37  out_sensorPose = sensorPose;
38 }
40 {
41  sensorPose = p;
42 }
44 {
46  o << "Homogeneous matrix for the sensor pose wrt vehicle:\n";
48  << sensorPose << std::endl;
49 
50  o << "Pointcloud class: ";
51  if (!this->pointcloud)
52  {
53  o << "nullptr\n";
54  }
55  else
56  {
57  o << pointcloud->GetRuntimeClass()->className << "\n";
58  o << "Number of points: " << pointcloud->size() << "\n";
59  }
60 
62  o << "Pointcloud is stored externally in format `"
63  << static_cast<int>(m_externally_stored) << "` in file `"
64  << m_external_file << "`\n";
65 }
66 
67 uint8_t CObservationPointCloud::serializeGetVersion() const { return 0; }
70 {
71  out << sensorLabel << timestamp; // Base class data
72 
73  out << sensorPose;
74  out.WriteAs<uint8_t>(m_externally_stored);
75 
76  if (isExternallyStored())
77  {
79  }
80  else
81  {
82  out << pointcloud;
83  }
84 }
85 
87  mrpt::serialization::CArchive& in, uint8_t version)
88 {
89  switch (version)
90  {
91  case 0:
92  {
93  pointcloud.reset();
94  in >> sensorLabel >> timestamp; // Base class data
95 
96  in >> sensorPose;
98  static_cast<ExternalStorageFormat>(in.ReadPOD<uint8_t>());
99 
100  if (isExternallyStored())
101  {
102  in >> m_external_file;
103  }
104  else
105  {
106  m_external_file.clear();
107  in >> pointcloud;
108  }
109  }
110  break;
111  default:
113  };
114 }
115 
117 {
118  MRPT_START
119 
120  // Already loaded?
121  if (!isExternallyStored() || (isExternallyStored() && pointcloud)) return;
122 
123  const auto abs_filename = m_external_file;
124 
125  switch (m_externally_stored)
126  {
128  break;
130  {
132  bool ok = pts->loadFromKittiVelodyneFile(abs_filename);
133  ASSERTMSG_(
134  ok, mrpt::format(
135  "[kitti format] Error loading lazy-load point cloud "
136  "file: '%s'",
137  abs_filename.c_str()));
138  auto pc = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(pts);
139  const_cast<mrpt::maps::CPointsMap::Ptr&>(pointcloud) = pc;
140  }
141  break;
143  {
145  data.loadFromTextFile(abs_filename);
146  if (data.rows() == 0)
148  "Empty external point cloud plain text file? `%s`",
149  abs_filename.c_str());
150 
152 
153  if (data.cols() == 3 || data.cols() == 2)
154  {
155  pc = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(
157  }
158  else if (data.cols() == 6)
159  {
160  pc = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(
162  }
163  else if (data.cols() == 4)
164  {
165  pc = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(
167  }
168  else
170  "Unexpected number of columns in point cloud file: cols=%u",
171  static_cast<unsigned int>(data.cols()));
172 
173  pc->resize(data.rows());
174  std::vector<float> vals(data.cols());
175  for (int i = 0; i < data.rows(); i++)
176  {
177  data.extractRow(i, vals);
178  pc->setPointAllFieldsFast(i, vals);
179  }
180 
181  // Assign:
182  const_cast<mrpt::maps::CPointsMap::Ptr&>(pointcloud) = pc;
183  }
184  break;
186  {
187  mrpt::io::CFileGZInputStream f(abs_filename);
189  auto obj = ar.ReadObject();
190  auto pc = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(obj);
191  const_cast<mrpt::maps::CPointsMap::Ptr&>(pointcloud) = pc;
192 
193  ASSERTMSG_(
195  "[mrpt-serialization format] Error loading "
196  "lazy-load point cloud file: %s",
197  abs_filename.c_str()));
198  }
199  break;
200  };
201 
202  MRPT_END
203 }
205 {
206  MRPT_START
208  {
209  // Free memory, saving to the file if it doesn't exist:
210  const auto abs_filename = m_external_file;
211  if (!mrpt::system::fileExists(abs_filename))
212  {
213  switch (m_externally_stored)
214  {
216  break;
218  {
219  THROW_EXCEPTION("Saving to kitti format not supported.");
220  }
222  {
223  std::ofstream f(abs_filename);
224  ASSERT_(f.is_open());
225  std::vector<float> row;
226  for (size_t i = 0; i < pointcloud->size(); i++)
227  {
228  pointcloud->getPointAllFieldsFast(i, row);
229  for (const float v : row) f << v << " ";
230  f << "\n";
231  }
232  }
233  break;
235  {
236  mrpt::io::CFileGZOutputStream f(abs_filename);
238  ar << *pointcloud;
239  }
240  break;
241  };
242  }
243 
244  // Now we can safely free the mem:
245  pointcloud.reset();
246  }
247  MRPT_END
248 }
249 
251  const std::string& fileName,
253 {
254  MRPT_START
255  ASSERTMSG_(!isExternallyStored(), "Already marked as externally-stored.");
256  m_external_file = fileName;
257  m_externally_stored = fmt;
258 
259  MRPT_END
260 }
static Ptr Create(Args &&... args)
T ReadPOD()
Reads a simple POD type and returns by value.
Definition: CArchive.h:303
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
void load() const override
Makes sure all images and other fields which may be externally stored are loaded in memory...
#define MRPT_START
Definition: exceptions.h:241
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
An observation from any sensor that can be summarized as a pointcloud.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
void setSensorPose(const mrpt::poses::CPose3D &p) override
A general method to change the sensor pose on the robot.
mrpt::poses::CPose3D sensorPose
Sensor placement wrt the vehicle/robot.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:592
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void unload() override
Unload all images, for the case they being delayed-load images stored in external files (othewise...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:65
This namespace contains representation of robot actions and observations.
static Ptr Create(Args &&... args)
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
mrpt::maps::CPointsMap::Ptr pointcloud
The pointcloud.
IMPLEMENTS_SERIALIZABLE(CObservationPointCloud, CObservation, mrpt::obs)
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
Definition: CObservation.h:62
static Ptr Create(Args &&... args)
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp.
Definition: CObservation.h:60
void setAsExternalStorage(const std::string &fileName, const ExternalStorageFormat fmt)
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
#define MRPT_END
Definition: exceptions.h:245
Transparently opens a compressed "gz" file and reads uncompressed data from it.
MATRIX44 getHomogeneousMatrixVal() const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
Definition: CPoseOrPoint.h:278
Saves data to a file and transparently compress the data using the given compression level...
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
This template class provides the basic functionality for a general 2D any-size, resizable container o...
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
static struct FontData data
Definition: gltext.cpp:144



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020