YARP
Yet Another Robot Platform
PointCloud.h
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-License-Identifier: BSD-3-Clause
4  */
5 
6 #ifndef YARP_SIG_POINTCLOUD_H
7 #define YARP_SIG_POINTCLOUD_H
8 
9 #include <yarp/sig/Vector.h>
13 
14 
15 namespace yarp::sig {
16 
17 template <class T>
21 class PointCloud : public PointCloudBase
22 {
23  static_assert(std::is_same<T, DataXY>::value ||
24  std::is_same<T, DataXYZ>::value ||
25  std::is_same<T, DataNormal>::value ||
26  std::is_same<T, DataXYZRGBA>::value ||
27  std::is_same<T, DataXYZI>::value ||
28  std::is_same<T, DataInterestPointXYZ>::value ||
29  std::is_same<T, DataXYZNormal>::value ||
30  std::is_same<T, DataXYZNormalRGBA>::value, "yarp::sig::PointCloud<T>: T chosen is not supported");
31 public:
32 
37  {
38  m_storage.clear();
39  setPointType();
40  }
41 
47  template <class T1>
49  {
50  setPointType();
51  copy<T1>(alt);
52  }
53 
59  virtual void resize(size_t width, size_t height)
60  {
61  header.width = width;
63  m_storage.resize(width * height);
64  }
65 
73  virtual void resize(size_t width)
74  {
75  header.width = width;
76  header.height = 1;
77  m_storage.resize(width);
78  }
79 
80  const char* getRawData() const override
81  {
82  return m_storage.getMemoryBlock();
83  }
84 
90  size_t wireSizeBytes() const override
91  {
92  return sizeof(header) + dataSizeBytes();
93  }
94 
100  size_t dataSizeBytes() const override
101  {
102  return header.width * header.height * (sizeof(T));
103  }
104 
105  size_t size() const override
106  {
107  return m_storage.size();
108  }
109 
116  inline T& operator()(size_t u, size_t v)
117  {
118  return m_storage[u + v * width()];
119  }
120 
127  inline const T& operator()(size_t u, size_t v) const
128  {
129  return m_storage[u + v * width()];
130  }
131 
136  inline T& operator()(size_t i)
137  {
138  return m_storage[i];
139  }
140 
145  inline const T& operator()(size_t i) const
146  {
147  return m_storage[i];
148  }
149 
150  template <class T1>
157  {
158  copy(alt);
159  return *this;
160  }
161 
162 
168  inline PointCloud<T>&
170  {
171 
172  yAssert(getPointType() == rhs.getPointType());
173 
174  size_t nr_points = m_storage.size();
175  m_storage.resize(nr_points + rhs.size());
176  for (size_t i = nr_points; i < m_storage.size(); ++i) {
177  m_storage[i] = rhs.m_storage[i - nr_points];
178  }
179 
180  header.width = m_storage.size();
181  header.height = 1;
182  if (rhs.isDense() && isDense()) {
183  header.isDense = 1;
184  } else {
185  header.isDense = 0;
186  }
187  return (*this);
188  }
189 
195  inline const PointCloud<T>
197  {
198  return (PointCloud<T>(*this) += rhs);
199  }
200 
206  inline void push_back(const T& pt)
207  {
208  m_storage.push_back(pt);
209  header.width = m_storage.size();
210  header.height = 1;
211  }
212 
216  virtual inline void clear()
217  {
218  m_storage.clear();
219  header.width = 0;
220  header.height = 0;
221  }
222 
231  virtual void fromExternalPC(const char* source, int type, size_t width, size_t height, bool isDense = true)
232  {
233  yAssert(source);
235  resize(width, height);
236  if (this->getPointType() == type) {
237  memcpy(const_cast<char*>(getRawData()), source, dataSizeBytes());
238  } else {
239  std::vector<int> recipe = getComposition(type);
240  copyFromRawData(getRawData(), source, recipe);
241  }
242  }
243 
244 
245  template <class T1>
251  void copy(const PointCloud<T1>& alt)
252  {
253  resize(alt.width(), alt.height());
254  if (std::is_same<T, T1>::value) {
255  yAssert(dataSizeBytes() == alt.dataSizeBytes());
256  memcpy(const_cast<char*>(getRawData()), alt.getRawData(), dataSizeBytes());
257  } else {
258  std::vector<int> recipe = getComposition(alt.getPointType());
259  copyFromRawData(getRawData(), alt.getRawData(), recipe);
260  }
261  }
262 
263  bool read(yarp::os::ConnectionReader& connection) override
264  {
265  connection.convertTextMode();
267  bool ok = connection.expectBlock((char*)&_header, sizeof(_header));
268  if (!ok) {
269  return false;
270  }
271 
272  m_storage.resize(_header.height * _header.width);
273  std::memset((void*)m_storage.data(), 0, m_storage.size() * sizeof(T));
274 
275  header.height = _header.height;
276  header.width = _header.width;
277  header.isDense = _header.isDense;
278 
279  if (header.pointType == _header.pointType) {
280  return m_storage.read(connection);
281  }
282 
283  T* tmp = m_storage.data();
284 
285  yAssert(tmp != nullptr);
286 
287  // Skip the vector header....
288  connection.expectInt32();
289  connection.expectInt32();
290 
291  std::vector<int> recipe = getComposition(_header.pointType);
292 
294  for (size_t i = 0; i < m_storage.size(); i++) {
295  for (size_t j = 0; j < recipe.size(); j++) {
296  size_t sizeToRead = pointType2Size(recipe[j]);
297  if ((header.pointType & recipe[j])) {
298  size_t offset = getOffset(header.pointType, recipe[j]);
299  connection.expectBlock((char*)&tmp[i] + offset, sizeToRead);
300  } else {
301  dummy.allocateOnNeed(sizeToRead, sizeToRead);
302  connection.expectBlock(dummy.bytes().get(), sizeToRead);
303  }
304  }
305  }
306 
307  connection.convertTextMode();
308  return true;
309  }
310 
311  bool write(yarp::os::ConnectionWriter& writer) const override
312  {
313  writer.appendBlock((char*)&header, sizeof(PointCloudNetworkHeader));
314  return m_storage.write(writer);
315  }
316 
317  virtual std::string toString(int precision = -1, int width = -1) const
318  {
319  std::string ret;
320  if (isOrganized()) {
321  for (size_t r = 0; r < this->width(); r++) {
322  for (size_t c = 0; c < this->height(); c++) {
323  ret += (*this)(r, c).toString(precision, width);
324  }
325  if (r < this->width() - 1) // if it is not the last row
326  {
327  ret += "\n";
328  }
329  }
330 
331  } else {
332  for (size_t i = 0; i < this->size(); i++) {
333  ret += (*this)(i).toString(precision, width);
334  }
335  }
336  return ret;
337  }
338 
344  {
346  ret.addInt32(width());
347  ret.addInt32(height());
348  ret.addInt32(getPointType());
349  ret.addInt32(isDense());
350 
351  for (size_t i = 0; i < this->size(); i++) {
352  ret.addList().append((*this)(i).toBottle());
353  }
354  return ret;
355  }
356 
364  bool fromBottle(const yarp::os::Bottle& bt)
365  {
366  if (bt.isNull()) {
367  return false;
368  }
369 
370  if (this->getPointType() != bt.get(2).asInt32()) {
371  return false;
372  }
373 
374  this->resize(bt.get(0).asInt32(), bt.get(1).asInt32());
375  this->header.isDense = bt.get(3).asInt32();
376 
377  if ((size_t)bt.size() != 4 + width() * height()) {
378  return false;
379  }
380 
381  for (size_t i = 0; i < this->size(); i++) {
382  (*this)(i).fromBottle(bt, i + 4);
383  }
384 
385  return true;
386  }
387 
388  int getBottleTag() const override
389  {
390  return BottleTagMap<T>();
391  }
392 
393 private:
394  yarp::sig::VectorOf<T> m_storage;
395 
396  void setPointType()
397  {
398  if (std::is_same<T, DataXY>::value) {
400  return;
401  }
402 
403  if (std::is_same<T, DataXYZ>::value) {
405  return;
406  }
407 
408  if (std::is_same<T, DataNormal>::value) {
410  return;
411  }
412 
413  if (std::is_same<T, DataXYZRGBA>::value) {
415  return;
416  }
417 
418  if (std::is_same<T, DataXYZI>::value) {
420  return;
421  }
422 
423  if (std::is_same<T, DataInterestPointXYZ>::value) {
425  return;
426  }
427 
428  if (std::is_same<T, DataXYZNormal>::value) {
430  return;
431  }
432 
433  if (std::is_same<T, DataXYZNormalRGBA>::value) {
435  return;
436  }
437 
438 // DataRGBA has sense to implement them?
439 // intensity has sense to implement them?
440 // DataViewpoint has sense to implement them?
441 
442  header.pointType = 0;
443  }
444 };
445 
454 
455 } // namespace yarp::sig
456 
457 template <>
458 inline int BottleTagMap<yarp::sig::DataXY>()
459 {
460  return BOTTLE_TAG_FLOAT64;
461 }
462 
463 template <>
464 inline int BottleTagMap<yarp::sig::DataXYZ>()
465 {
466  return BOTTLE_TAG_FLOAT64;
467 }
468 
469 template <>
470 inline int BottleTagMap<yarp::sig::DataNormal>()
471 {
472  return BOTTLE_TAG_FLOAT64;
473 }
474 
475 template <>
476 inline int BottleTagMap<yarp::sig::DataXYZRGBA>()
477 {
478  return BOTTLE_TAG_FLOAT64;
479 }
480 
481 template <>
482 inline int BottleTagMap<yarp::sig::DataXYZI>()
483 {
484  return BOTTLE_TAG_FLOAT64;
485 }
486 
487 template <>
488 inline int BottleTagMap<yarp::sig::DataInterestPointXYZ>()
489 {
490  return BOTTLE_TAG_FLOAT64;
491 }
492 
493 template <>
494 inline int BottleTagMap<yarp::sig::DataXYZNormal>()
495 {
496  return BOTTLE_TAG_FLOAT64;
497 }
498 
499 template <>
500 inline int BottleTagMap<yarp::sig::DataXYZNormalRGBA>()
501 {
502  return BOTTLE_TAG_FLOAT64;
503 }
504 
505 
506 #endif // YARP_SIG_POINTCLOUD_H
#define BOTTLE_TAG_FLOAT64
Definition: Bottle.h:25
bool ret
#define yAssert(x)
Definition: Log.h:383
contains the definition of a Vector type
A simple collection of objects that can be described and transmitted in a portable way.
Definition: Bottle.h:73
size_type size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:251
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:246
bool isNull() const override
Checks if the object is invalid.
Definition: Bottle.cpp:370
const char * get() const
Definition: Bytes.cpp:27
An interface for reading from a network connection.
virtual bool expectBlock(char *data, size_t len)=0
Read a block of data from the network connection.
virtual std::int32_t expectInt32()=0
Read a 32-bit integer from the network connection.
virtual bool convertTextMode()=0
Reads in a standard description in text mode, and converts it to a standard description in binary.
An interface for writing to a network connection.
virtual void appendBlock(const char *data, size_t len)=0
Send a block of data to the network connection.
An abstraction for a block of bytes, with optional responsibility for allocating/destroying that bloc...
Definition: ManagedBytes.h:21
const Bytes & bytes() const
bool allocateOnNeed(size_t neededLen, size_t allocateLen)
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:204
The PointCloudBase class.
yarp::sig::PointCloudNetworkHeader header
virtual std::vector< int > getComposition(int type_composite) const
virtual size_t width() const
virtual bool isDense() const
virtual void copyFromRawData(const char *dst, const char *source, std::vector< int > &recipe)
virtual bool isOrganized() const
virtual int getPointType() const
virtual size_t height() const
virtual size_t pointType2Size(int type) const
virtual size_t getOffset(int type_composite, int type_basic) const
The yarp::sig::PointCloudNetworkHeader class.
The PointCloud class.
Definition: PointCloud.h:22
T & operator()(size_t u, size_t v)
Obtain the point given by the (column, row) coordinates.
Definition: PointCloud.h:116
virtual void fromExternalPC(const char *source, int type, size_t width, size_t height, bool isDense=true)
Copy the content of an external PointCloud.
Definition: PointCloud.h:231
const PointCloud< T > operator+(const PointCloud< T > &rhs)
Concatenate a point cloud to another cloud.
Definition: PointCloud.h:196
bool fromBottle(const yarp::os::Bottle &bt)
Populate the PointCloud from a yarp::os::Bottle.
Definition: PointCloud.h:364
const T & operator()(size_t u, size_t v) const
Obtain the point given by the (column, row) coordinates (const version).
Definition: PointCloud.h:127
const T & operator()(size_t i) const
Obtain the point given by the index (const version).
Definition: PointCloud.h:145
T & operator()(size_t i)
Obtain the point given by the index.
Definition: PointCloud.h:136
void push_back(const T &pt)
Insert a new point in the cloud, at the end of the container.
Definition: PointCloud.h:206
virtual std::string toString(int precision=-1, int width=-1) const
Definition: PointCloud.h:317
PointCloud< T > & operator+=(const PointCloud< T > &rhs)
Concatenate a point cloud to the current cloud.
Definition: PointCloud.h:169
PointCloud()
PointCloud, default constructor.
Definition: PointCloud.h:36
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
Definition: PointCloud.h:263
const PointCloud< T > & operator=(const PointCloud< T1 > &alt)
Assignment operator.
Definition: PointCloud.h:156
PointCloud(const PointCloud< T1 > &alt)
PointCloud, copy constructor.
Definition: PointCloud.h:48
yarp::os::Bottle toBottle() const
Generate a yarp::os::Bottle filled with the PointCloud data.
Definition: PointCloud.h:343
int getBottleTag() const override
Definition: PointCloud.h:388
virtual void resize(size_t width)
Resize the PointCloud.
Definition: PointCloud.h:73
const char * getRawData() const override
Get the pointer to the data.
Definition: PointCloud.h:80
virtual void clear()
Clear the data.
Definition: PointCloud.h:216
bool write(yarp::os::ConnectionWriter &writer) const override
Write this object to a network connection.
Definition: PointCloud.h:311
virtual void resize(size_t width, size_t height)
Resize the PointCloud.
Definition: PointCloud.h:59
size_t dataSizeBytes() const override
Get the size of the data in terms of number of bytes.
Definition: PointCloud.h:100
void copy(const PointCloud< T1 > &alt)
Copy operator.
Definition: PointCloud.h:251
size_t wireSizeBytes() const override
Get the size of the data + the header in terms of number of bytes.
Definition: PointCloud.h:90
size_t size() const override
Definition: PointCloud.h:105
Provides:
Definition: Vector.h:117
@ PCL_INTEREST_POINT_XYZ
@ PCL_POINT_XYZ_NORMAL
@ PCL_POINT_XYZ_NORMAL_RGBA