YARP
Yet Another Robot Platform
PointCloud.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 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  data.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  data.resize(width * height);
69  }
70 
78  virtual void resize(size_t width)
79  {
80  header.width = width;
81  header.height = 1;
82  data.resize(width);
83  }
84 
85  const char* getRawData() const override
86  {
87  return data.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 data.size();
113  }
114 
121  inline T& operator()(size_t u, size_t v)
122  {
123  yAssert(isOrganized());
124  return data[u + v * width()];
125  }
126 
133  inline const T& operator()(size_t u, size_t v) const
134  {
135  yAssert(isOrganized());
136  return data[u + v * width()];
137  }
138 
143  inline T& operator()(size_t i)
144  {
145  return data[i];
146  }
147 
152  inline const T& operator()(size_t i) const
153  {
154  return data[i];
155  }
156 
157  template <class T1>
164  {
165  copy(alt);
166  return *this;
167  }
168 
169 
175  inline PointCloud<T>&
177  {
178 
179  yAssert(getPointType() == rhs.getPointType());
180 
181  size_t nr_points = data.size();
182  data.resize(nr_points + rhs.size());
183  for (size_t i = nr_points; i < data.size(); ++i) {
184  data[i] = rhs.data[i - nr_points];
185  }
186 
187  header.width = data.size();
188  header.height = 1;
189  if (rhs.isDense() && isDense()) {
190  header.isDense = 1;
191  } else {
192  header.isDense = 0;
193  }
194  return (*this);
195  }
196 
202  inline const PointCloud<T>
204  {
205  return (PointCloud<T>(*this) += rhs);
206  }
207 
213  inline void push_back(const T& pt)
214  {
215  data.push_back(pt);
216  header.width = data.size();
217  header.height = 1;
218  }
219 
223  virtual inline void clear()
224  {
225  data.clear();
226  header.width = 0;
227  header.height = 0;
228  }
229 
238  virtual void fromExternalPC(const char* source, int type, size_t width, size_t height, bool isDense = true)
239  {
240  yAssert(source);
242  resize(width, height);
243  if (this->getPointType() == type) {
244  memcpy(const_cast<char*>(getRawData()), source, dataSizeBytes());
245  } else {
246  std::vector<int> recipe = getComposition(type);
247  copyFromRawData(getRawData(), source, recipe);
248  }
249  }
250 
251 
252  template <class T1>
258  void copy(const PointCloud<T1>& alt)
259  {
260  resize(alt.width(), alt.height());
261  if (std::is_same<T, T1>::value) {
262  yAssert(dataSizeBytes() == alt.dataSizeBytes());
263  memcpy(const_cast<char*>(getRawData()), alt.getRawData(), dataSizeBytes());
264  } else {
265  std::vector<int> recipe = getComposition(alt.getPointType());
266  copyFromRawData(getRawData(), alt.getRawData(), recipe);
267  }
268  }
269 
270  bool read(yarp::os::ConnectionReader& connection) override
271  {
272  connection.convertTextMode();
274  bool ok = connection.expectBlock((char*)&_header, sizeof(_header));
275  if (!ok) {
276  return false;
277  }
278 
279  data.resize(_header.height * _header.width);
280  std::memset((void*)data.getFirst(), 0, data.size() * sizeof(T));
281 
282  header.height = _header.height;
283  header.width = _header.width;
284  header.isDense = _header.isDense;
285 
286  if (header.pointType == _header.pointType) {
287  return data.read(connection);
288  }
289 
290  T* tmp = data.getFirst();
291 
292  yAssert(tmp != nullptr);
293 
294  // Skip the vector header....
295  connection.expectInt32();
296  connection.expectInt32();
297 
298  std::vector<int> recipe = getComposition(_header.pointType);
299 
301  for (size_t i = 0; i < data.size(); i++) {
302  for (size_t j = 0; j < recipe.size(); j++) {
303  size_t sizeToRead = pointType2Size(recipe[j]);
304  if ((header.pointType & recipe[j])) {
305  size_t offset = getOffset(header.pointType, recipe[j]);
306  connection.expectBlock((char*)&tmp[i] + offset, sizeToRead);
307  } else {
308  dummy.allocateOnNeed(sizeToRead, sizeToRead);
309  connection.expectBlock(dummy.bytes().get(), sizeToRead);
310  }
311  }
312  }
313 
314  connection.convertTextMode();
315  return true;
316  }
317 
318  bool write(yarp::os::ConnectionWriter& writer) const override
319  {
320  writer.appendBlock((char*)&header, sizeof(PointCloudNetworkHeader));
321  return data.write(writer);
322  }
323 
324  virtual std::string toString(int precision = -1, int width = -1) const
325  {
326  std::string ret;
327  if (isOrganized()) {
328  for (size_t r = 0; r < this->width(); r++) {
329  for (size_t c = 0; c < this->height(); c++) {
330  ret += (*this)(r, c).toString(precision, width);
331  }
332  if (r < this->width() - 1) // if it is not the last row
333  {
334  ret += "\n";
335  }
336  }
337 
338  } else {
339  for (size_t i = 0; i < this->size(); i++) {
340  ret += (*this)(i).toString(precision, width);
341  }
342  }
343  return ret;
344  }
345 
351  {
353  ret.addInt32(width());
354  ret.addInt32(height());
355  ret.addInt32(getPointType());
356  ret.addInt32(isDense());
357 
358  for (size_t i = 0; i < this->size(); i++) {
359  ret.addList().append((*this)(i).toBottle());
360  }
361  return ret;
362  }
363 
371  bool fromBottle(const yarp::os::Bottle& bt)
372  {
373  if (bt.isNull()) {
374  return false;
375  }
376 
377  if (this->getPointType() != bt.get(2).asInt32()) {
378  return false;
379  }
380 
381  this->resize(bt.get(0).asInt32(), bt.get(1).asInt32());
382  this->header.isDense = bt.get(3).asInt32();
383 
384  if ((size_t)bt.size() != 4 + width() * height()) {
385  return false;
386  }
387 
388  for (size_t i = 0; i < this->size(); i++) {
389  (*this)(i).fromBottle(bt, i + 4);
390  }
391 
392  return true;
393  }
394 
395  int getBottleTag() const override
396  {
397  return BottleTagMap<T>();
398  }
399 
400 private:
402 
404  {
405  if (std::is_same<T, DataXY>::value) {
407  return;
408  }
409 
410  if (std::is_same<T, DataXYZ>::value) {
412  return;
413  }
414 
415  if (std::is_same<T, DataNormal>::value) {
417  return;
418  }
419 
420  if (std::is_same<T, DataXYZRGBA>::value) {
422  return;
423  }
424 
425  if (std::is_same<T, DataXYZI>::value) {
427  return;
428  }
429 
430  if (std::is_same<T, DataInterestPointXYZ>::value) {
432  return;
433  }
434 
435  if (std::is_same<T, DataXYZNormal>::value) {
437  return;
438  }
439 
440  if (std::is_same<T, DataXYZNormalRGBA>::value) {
442  return;
443  }
444 
445 // DataRGBA has sense to implement them?
446 // intensity has sense to implement them?
447 // DataViewpoint has sense to implement them?
448 
449  header.pointType = 0;
450  }
451 };
452 
453 } // namespace sig
454 } // namespace yarp
455 
456 template <>
457 inline int BottleTagMap<yarp::sig::DataXY>()
458 {
459  return BOTTLE_TAG_FLOAT64;
460 }
461 
462 template <>
463 inline int BottleTagMap<yarp::sig::DataXYZ>()
464 {
465  return BOTTLE_TAG_FLOAT64;
466 }
467 
468 template <>
469 inline int BottleTagMap<yarp::sig::DataNormal>()
470 {
471  return BOTTLE_TAG_FLOAT64;
472 }
473 
474 template <>
475 inline int BottleTagMap<yarp::sig::DataXYZRGBA>()
476 {
477  return BOTTLE_TAG_FLOAT64;
478 }
479 
480 template <>
481 inline int BottleTagMap<yarp::sig::DataXYZI>()
482 {
483  return BOTTLE_TAG_FLOAT64;
484 }
485 
486 template <>
487 inline int BottleTagMap<yarp::sig::DataInterestPointXYZ>()
488 {
489  return BOTTLE_TAG_FLOAT64;
490 }
491 
492 template <>
493 inline int BottleTagMap<yarp::sig::DataXYZNormal>()
494 {
495  return BOTTLE_TAG_FLOAT64;
496 }
497 
498 template <>
499 inline int BottleTagMap<yarp::sig::DataXYZNormalRGBA>()
500 {
501  return BOTTLE_TAG_FLOAT64;
502 }
503 
504 
505 #endif // YARP_SIG_POINTCLOUD_H
void push_back(const T &pt)
Insert a new point in the cloud, at the end of the container.
Definition: PointCloud.h:213
PointCloud< T > & operator+=(const PointCloud< T > &rhs)
Concatenate a point cloud to the current cloud.
Definition: PointCloud.h:176
bool fromBottle(const yarp::os::Bottle &bt)
Populate the PointCloud from a yarp::os::Bottle.
Definition: PointCloud.h:371
The PointCloud class.
Definition: PointCloud.h:26
const PointCloud< T > & operator=(const PointCloud< T1 > &alt)
Assignment operator.
Definition: PointCloud.h:163
void append(const Bottle &alt)
Append the content of the given bottle to the current list.
Definition: Bottle.cpp:339
virtual void clear()
Clear the data.
Definition: PointCloud.h:223
const PointCloud< T > operator+(const PointCloud< T > &rhs)
Concatenate a point cloud to another cloud.
Definition: PointCloud.h:203
virtual bool expectBlock(char *data, size_t len)=0
Read a block of data from the network connection.
bool ret
virtual void resize(size_t width)
Resize the PointCloud.
Definition: PointCloud.h:78
virtual void appendBlock(const char *data, size_t len)=0
Send a block of data to the network connection.
virtual std::int32_t asInt32() const
Get 32-bit integer value.
Definition: Value.cpp:206
int getBottleTag() const override
Definition: PointCloud.h:395
bool allocateOnNeed(size_t neededLen, size_t allocateLen)
const T & operator()(size_t i) const
Obtain the point given by the index (const version).
Definition: PointCloud.h:152
Provides:
Definition: Vector.h:30
The main, catch-all namespace for YARP.
Definition: numeric.h:47
virtual bool convertTextMode()=0
Reads in a standard description in text mode, and converts it to a standard description in binary...
yarp::sig::VectorOf< T > data
Definition: PointCloud.h:401
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
Definition: PointCloud.h:270
An interface for writing to a network connection.
virtual void copyFromRawData(const char *dst, const char *source, std::vector< int > &recipe)
size_t size() const override
Definition: PointCloud.h:110
void addInt32(std::int32_t x)
Places a 32-bit integer in the bottle, at the end of the list.
Definition: Bottle.cpp:99
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:238
const Bytes & bytes() const
The PointCloudBase class.
A simple collection of objects that can be described and transmitted in a portable way...
Definition: Bottle.h:72
const char * get() const
Definition: Bytes.cpp:30
yarp::os::Bottle toBottle() const
Generate a yarp::os::Bottle filled with the PointCloud data.
Definition: PointCloud.h:350
PointCloud()
PointCloud, default constructor.
Definition: PointCloud.h:41
T & operator()(size_t i)
Obtain the point given by the index.
Definition: PointCloud.h:143
#define BOTTLE_TAG_FLOAT64
Definition: Bottle.h:27
virtual size_t pointType2Size(int type) const
#define yAssert(x)
Definition: Log.h:116
Bottle & addList()
Places an empty nested list in the bottle, at the end of the list.
Definition: Bottle.cpp:141
virtual std::int32_t expectInt32()=0
Read a 32-bit integer from the network connection.
contains the definition of a Vector type
An interface for reading from a network connection.
yarp::sig::PointCloudNetworkHeader header
virtual std::string toString(int precision=-1, int width=-1) const
Definition: PointCloud.h:324
virtual size_t width() const
void copy(const PointCloud< T1 > &alt)
Copy operator.
Definition: PointCloud.h:258
size_t dataSizeBytes() const override
Get the size of the data in terms of number of bytes.
Definition: PointCloud.h:105
PointCloud(const PointCloud< T1 > &alt)
PointCloud, copy constructor.
Definition: PointCloud.h:53
size_t wireSizeBytes() const override
Get the size of the data + the header in terms of number of bytes.
Definition: PointCloud.h:95
virtual size_t getOffset(int type_composite, int type_basic) const
const T & operator()(size_t u, size_t v) const
Obtain the point given by the (column, row) coordinates (const version).
Definition: PointCloud.h:133
virtual size_t height() const
The yarp::sig::PointCloudNetworkHeader class.
const char * getRawData() const override
Get the pointer to the data.
Definition: PointCloud.h:85
virtual void resize(size_t width, size_t height)
Resize the PointCloud.
Definition: PointCloud.h:64
bool isNull() const override
Checks if the object is invalid.
Definition: Bottle.cpp:329
virtual int getPointType() const
An abstraction for a block of bytes, with optional responsibility for allocating/destroying that bloc...
Definition: ManagedBytes.h:24
bool write(yarp::os::ConnectionWriter &writer) const override
Write this object to a network connection.
Definition: PointCloud.h:318
size_t size() const
Gets the number of elements in the bottle.
Definition: Bottle.cpp:210
virtual std::vector< int > getComposition(int type_composite) const
virtual bool isOrganized() const
T & operator()(size_t u, size_t v)
Obtain the point given by the (column, row) coordinates.
Definition: PointCloud.h:121
Value & get(size_t index) const
Reads a Value v from a certain part of the list.
Definition: Bottle.cpp:205
virtual bool isDense() const