YARP
Yet Another Robot Platform
PointCloud.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  *
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
9 #ifndef YARP_SIG_POINTCLOUD_H
10 #define YARP_SIG_POINTCLOUD_H
11 
12 #include <yarp/sig/Vector.h>
16 
17 
18 
19 namespace yarp {
20 namespace sig {
21 
22 template <class T>
26 class PointCloud : public PointCloudBase
27 {
28  static_assert(std::is_same<T, DataXY>::value ||
29  std::is_same<T, DataXYZ>::value ||
30  std::is_same<T, DataNormal>::value ||
31  std::is_same<T, DataXYZRGBA>::value ||
32  std::is_same<T, DataXYZI>::value ||
33  std::is_same<T, DataInterestPointXYZ>::value ||
34  std::is_same<T, DataXYZNormal>::value ||
35  std::is_same<T, DataXYZNormalRGBA>::value, "yarp::sig::PointCloud<T>: T chosen is not supported");
36 public:
37 
42  {
43  m_storage.clear();
44  setPointType();
45  }
46 
52  template <class T1>
54  {
55  setPointType();
56  copy<T1>(alt);
57  }
58 
64  virtual void resize(size_t width, size_t height)
65  {
66  header.width = width;
68  m_storage.resize(width * height);
69  }
70 
78  virtual void resize(size_t width)
79  {
80  header.width = width;
81  header.height = 1;
82  m_storage.resize(width);
83  }
84 
85  const char* getRawData() const override
86  {
87  return m_storage.getMemoryBlock();
88  }
89 
95  size_t wireSizeBytes() const override
96  {
97  return sizeof(header) + dataSizeBytes();
98  }
99 
105  size_t dataSizeBytes() const override
106  {
107  return header.width * header.height * (sizeof(T));
108  }
109 
110  size_t size() const override
111  {
112  return m_storage.size();
113  }
114 
121  inline T& operator()(size_t u, size_t v)
122  {
123  return m_storage[u + v * width()];
124  }
125 
132  inline const T& operator()(size_t u, size_t v) const
133  {
134  return m_storage[u + v * width()];
135  }
136 
141  inline T& operator()(size_t i)
142  {
143  return m_storage[i];
144  }
145 
150  inline const T& operator()(size_t i) const
151  {
152  return m_storage[i];
153  }
154 
155  template <class T1>
162  {
163  copy(alt);
164  return *this;
165  }
166 
167 
173  inline PointCloud<T>&
175  {
176 
177  yAssert(getPointType() == rhs.getPointType());
178 
179  size_t nr_points = m_storage.size();
180  m_storage.resize(nr_points + rhs.size());
181  for (size_t i = nr_points; i < m_storage.size(); ++i) {
182  m_storage[i] = rhs.m_storage[i - nr_points];
183  }
184 
185  header.width = m_storage.size();
186  header.height = 1;
187  if (rhs.isDense() && isDense()) {
188  header.isDense = 1;
189  } else {
190  header.isDense = 0;
191  }
192  return (*this);
193  }
194 
200  inline const PointCloud<T>
202  {
203  return (PointCloud<T>(*this) += rhs);
204  }
205 
211  inline void push_back(const T& pt)
212  {
213  m_storage.push_back(pt);
214  header.width = m_storage.size();
215  header.height = 1;
216  }
217 
221  virtual inline void clear()
222  {
223  m_storage.clear();
224  header.width = 0;
225  header.height = 0;
226  }
227 
236  virtual void fromExternalPC(const char* source, int type, size_t width, size_t height, bool isDense = true)
237  {
238  yAssert(source);
240  resize(width, height);
241  if (this->getPointType() == type) {
242  memcpy(const_cast<char*>(getRawData()), source, dataSizeBytes());
243  } else {
244  std::vector<int> recipe = getComposition(type);
245  copyFromRawData(getRawData(), source, recipe);
246  }
247  }
248 
249 
250  template <class T1>
256  void copy(const PointCloud<T1>& alt)
257  {
258  resize(alt.width(), alt.height());
259  if (std::is_same<T, T1>::value) {
260  yAssert(dataSizeBytes() == alt.dataSizeBytes());
261  memcpy(const_cast<char*>(getRawData()), alt.getRawData(), dataSizeBytes());
262  } else {
263  std::vector<int> recipe = getComposition(alt.getPointType());
264  copyFromRawData(getRawData(), alt.getRawData(), recipe);
265  }
266  }
267 
268  bool read(yarp::os::ConnectionReader& connection) override
269  {
270  connection.convertTextMode();
272  bool ok = connection.expectBlock((char*)&_header, sizeof(_header));
273  if (!ok) {
274  return false;
275  }
276 
277  m_storage.resize(_header.height * _header.width);
278  std::memset((void*)m_storage.data(), 0, m_storage.size() * sizeof(T));
279 
280  header.height = _header.height;
281  header.width = _header.width;
282  header.isDense = _header.isDense;
283 
284  if (header.pointType == _header.pointType) {
285  return m_storage.read(connection);
286  }
287 
288  T* tmp = m_storage.data();
289 
290  yAssert(tmp != nullptr);
291 
292  // Skip the vector header....
293  connection.expectInt32();
294  connection.expectInt32();
295 
296  std::vector<int> recipe = getComposition(_header.pointType);
297 
299  for (size_t i = 0; i < m_storage.size(); i++) {
300  for (size_t j = 0; j < recipe.size(); j++) {
301  size_t sizeToRead = pointType2Size(recipe[j]);
302  if ((header.pointType & recipe[j])) {
303  size_t offset = getOffset(header.pointType, recipe[j]);
304  connection.expectBlock((char*)&tmp[i] + offset, sizeToRead);
305  } else {
306  dummy.allocateOnNeed(sizeToRead, sizeToRead);
307  connection.expectBlock(dummy.bytes().get(), sizeToRead);
308  }
309  }
310  }
311 
312  connection.convertTextMode();
313  return true;
314  }
315 
316  bool write(yarp::os::ConnectionWriter& writer) const override
317  {
318  writer.appendBlock((char*)&header, sizeof(PointCloudNetworkHeader));
319  return m_storage.write(writer);
320  }
321 
322  virtual std::string toString(int precision = -1, int width = -1) const
323  {
324  std::string ret;
325  if (isOrganized()) {
326  for (size_t r = 0; r < this->width(); r++) {
327  for (size_t c = 0; c < this->height(); c++) {
328  ret += (*this)(r, c).toString(precision, width);
329  }
330  if (r < this->width() - 1) // if it is not the last row
331  {
332  ret += "\n";
333  }
334  }
335 
336  } else {
337  for (size_t i = 0; i < this->size(); i++) {
338  ret += (*this)(i).toString(precision, width);
339  }
340  }
341  return ret;
342  }
343 
349  {
351  ret.addInt32(width());
352  ret.addInt32(height());
353  ret.addInt32(getPointType());
354  ret.addInt32(isDense());
355 
356  for (size_t i = 0; i < this->size(); i++) {
357  ret.addList().append((*this)(i).toBottle());
358  }
359  return ret;
360  }
361 
369  bool fromBottle(const yarp::os::Bottle& bt)
370  {
371  if (bt.isNull()) {
372  return false;
373  }
374 
375  if (this->getPointType() != bt.get(2).asInt32()) {
376  return false;
377  }
378 
379  this->resize(bt.get(0).asInt32(), bt.get(1).asInt32());
380  this->header.isDense = bt.get(3).asInt32();
381 
382  if ((size_t)bt.size() != 4 + width() * height()) {
383  return false;
384  }
385 
386  for (size_t i = 0; i < this->size(); i++) {
387  (*this)(i).fromBottle(bt, i + 4);
388  }
389 
390  return true;
391  }
392 
393  int getBottleTag() const override
394  {
395  return BottleTagMap<T>();
396  }
397 
398 private:
399  yarp::sig::VectorOf<T> m_storage;
400 
401  void setPointType()
402  {
403  if (std::is_same<T, DataXY>::value) {
405  return;
406  }
407 
408  if (std::is_same<T, DataXYZ>::value) {
410  return;
411  }
412 
413  if (std::is_same<T, DataNormal>::value) {
415  return;
416  }
417 
418  if (std::is_same<T, DataXYZRGBA>::value) {
420  return;
421  }
422 
423  if (std::is_same<T, DataXYZI>::value) {
425  return;
426  }
427 
428  if (std::is_same<T, DataInterestPointXYZ>::value) {
430  return;
431  }
432 
433  if (std::is_same<T, DataXYZNormal>::value) {
435  return;
436  }
437 
438  if (std::is_same<T, DataXYZNormalRGBA>::value) {
440  return;
441  }
442 
443 // DataRGBA has sense to implement them?
444 // intensity has sense to implement them?
445 // DataViewpoint has sense to implement them?
446 
447  header.pointType = 0;
448  }
449 };
450 
459 
460 } // namespace sig
461 } // namespace yarp
462 
463 template <>
464 inline int BottleTagMap<yarp::sig::DataXY>()
465 {
466  return BOTTLE_TAG_FLOAT64;
467 }
468 
469 template <>
470 inline int BottleTagMap<yarp::sig::DataXYZ>()
471 {
472  return BOTTLE_TAG_FLOAT64;
473 }
474 
475 template <>
476 inline int BottleTagMap<yarp::sig::DataNormal>()
477 {
478  return BOTTLE_TAG_FLOAT64;
479 }
480 
481 template <>
482 inline int BottleTagMap<yarp::sig::DataXYZRGBA>()
483 {
484  return BOTTLE_TAG_FLOAT64;
485 }
486 
487 template <>
488 inline int BottleTagMap<yarp::sig::DataXYZI>()
489 {
490  return BOTTLE_TAG_FLOAT64;
491 }
492 
493 template <>
494 inline int BottleTagMap<yarp::sig::DataInterestPointXYZ>()
495 {
496  return BOTTLE_TAG_FLOAT64;
497 }
498 
499 template <>
500 inline int BottleTagMap<yarp::sig::DataXYZNormal>()
501 {
502  return BOTTLE_TAG_FLOAT64;
503 }
504 
505 template <>
506 inline int BottleTagMap<yarp::sig::DataXYZNormalRGBA>()
507 {
508  return BOTTLE_TAG_FLOAT64;
509 }
510 
511 
512 #endif // YARP_SIG_POINTCLOUD_H
#define BOTTLE_TAG_FLOAT64
Definition: Bottle.h:27
bool ret
#define yAssert(x)
Definition: Log.h:297
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:76
size_type size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:254
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:249
bool isNull() const override
Checks if the object is invalid.
Definition: Bottle.cpp:373
const char * get() const
Definition: Bytes.cpp:30
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:25
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:207
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:27
T & operator()(size_t u, size_t v)
Obtain the point given by the (column, row) coordinates.
Definition: PointCloud.h:121
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:236
const PointCloud< T > operator+(const PointCloud< T > &rhs)
Concatenate a point cloud to another cloud.
Definition: PointCloud.h:201
bool fromBottle(const yarp::os::Bottle &bt)
Populate the PointCloud from a yarp::os::Bottle.
Definition: PointCloud.h:369
const T & operator()(size_t u, size_t v) const
Obtain the point given by the (column, row) coordinates (const version).
Definition: PointCloud.h:132
const T & operator()(size_t i) const
Obtain the point given by the index (const version).
Definition: PointCloud.h:150
T & operator()(size_t i)
Obtain the point given by the index.
Definition: PointCloud.h:141
void push_back(const T &pt)
Insert a new point in the cloud, at the end of the container.
Definition: PointCloud.h:211
virtual std::string toString(int precision=-1, int width=-1) const
Definition: PointCloud.h:322
PointCloud< T > & operator+=(const PointCloud< T > &rhs)
Concatenate a point cloud to the current cloud.
Definition: PointCloud.h:174
PointCloud()
PointCloud, default constructor.
Definition: PointCloud.h:41
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
Definition: PointCloud.h:268
const PointCloud< T > & operator=(const PointCloud< T1 > &alt)
Assignment operator.
Definition: PointCloud.h:161
PointCloud(const PointCloud< T1 > &alt)
PointCloud, copy constructor.
Definition: PointCloud.h:53
yarp::os::Bottle toBottle() const
Generate a yarp::os::Bottle filled with the PointCloud data.
Definition: PointCloud.h:348
int getBottleTag() const override
Definition: PointCloud.h:393
virtual void resize(size_t width)
Resize the PointCloud.
Definition: PointCloud.h:78
const char * getRawData() const override
Get the pointer to the data.
Definition: PointCloud.h:85
virtual void clear()
Clear the data.
Definition: PointCloud.h:221
bool write(yarp::os::ConnectionWriter &writer) const override
Write this object to a network connection.
Definition: PointCloud.h:316
virtual void resize(size_t width, size_t height)
Resize the PointCloud.
Definition: PointCloud.h:64
size_t dataSizeBytes() const override
Get the size of the data in terms of number of bytes.
Definition: PointCloud.h:105
void copy(const PointCloud< T1 > &alt)
Copy operator.
Definition: PointCloud.h:256
size_t wireSizeBytes() const override
Get the size of the data + the header in terms of number of bytes.
Definition: PointCloud.h:95
size_t size() const override
Definition: PointCloud.h:110
Provides:
Definition: Vector.h:122
@ PCL_INTEREST_POINT_XYZ
@ PCL_POINT_XYZ_NORMAL
@ PCL_POINT_XYZ_NORMAL_RGBA
The main, catch-all namespace for YARP.
Definition: environment.h:25