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>
15 
16 
17 
18 namespace yarp {
19 namespace sig {
20 
21 template <class T>
25 class PointCloud : public PointCloudBase
26 {
27  static_assert(std::is_same<T, DataXY>::value ||
28  std::is_same<T, DataXYZ>::value ||
29  std::is_same<T, DataNormal>::value ||
30  std::is_same<T, DataXYZRGBA>::value ||
31  std::is_same<T, DataXYZI>::value ||
32  std::is_same<T, DataInterestPointXYZ>::value ||
33  std::is_same<T, DataXYZNormal>::value ||
34  std::is_same<T, DataXYZNormalRGBA>::value, "yarp::sig::PointCloud<T>: T chosen is not supported");
35 public:
36 
41  {
42  data.clear();
43  setPointType();
44  }
45 
51  template <class T1>
53  {
54  setPointType();
55  copy<T1>(alt);
56  }
57 
63  virtual void resize(size_t width, size_t height)
64  {
65  header.width = width;
67  data.resize(width * height);
68  }
69 
77  virtual void resize(size_t width)
78  {
79  header.width = width;
80  header.height = 1;
81  data.resize(width);
82  }
83 
84  virtual const char* getRawData() const override
85  {
86  return data.getMemoryBlock();
87  }
88 
94  virtual size_t wireSizeBytes() const override
95  {
96  return sizeof(header) + dataSizeBytes();
97  }
98 
104  virtual size_t dataSizeBytes() const override
105  {
106  return header.width * header.height * (sizeof(T));
107  }
108 
109  virtual size_t size() const override
110  {
111  return data.size();
112  }
113 
120  inline T& operator()(size_t u, size_t v)
121  {
122  yAssert(isOrganized());
123  return data[u + v * width()];
124  }
125 
132  inline const T& operator()(size_t u, size_t v) const
133  {
134  yAssert(isOrganized());
135  return data[u + v * width()];
136  }
137 
142  inline T& operator()(size_t i)
143  {
144  return data[i];
145  }
146 
151  inline const T& operator()(size_t i) const
152  {
153  return data[i];
154  }
155 
156  template <class T1>
163  {
164  copy(alt);
165  return *this;
166  }
167 
168 
174  inline PointCloud<T>&
176  {
177 
178  yAssert(getPointType() == rhs.getPointType());
179 
180  size_t nr_points = data.size();
181  data.resize(nr_points + rhs.size());
182  for (size_t i = nr_points; i < data.size(); ++i) {
183  data[i] = rhs.data[i - nr_points];
184  }
185 
186  header.width = data.size();
187  header.height = 1;
188  if (rhs.isDense() && isDense()) {
189  header.isDense = 1;
190  } else {
191  header.isDense = 0;
192  }
193  return (*this);
194  }
195 
201  inline const PointCloud<T>
203  {
204  return (PointCloud<T>(*this) += rhs);
205  }
206 
212  inline void push_back(const T& pt)
213  {
214  data.push_back(pt);
215  header.width = data.size();
216  header.height = 1;
217  }
218 
222  virtual inline void clear()
223  {
224  data.clear();
225  header.width = 0;
226  header.height = 0;
227  }
228 
237  virtual void fromExternalPC(const char* source, int type, size_t width, size_t height, bool isDense = true)
238  {
239  yAssert(source);
241  resize(width, height);
242  if (this->getPointType() == type) {
243  memcpy(const_cast<char*>(getRawData()), source, dataSizeBytes());
244  } else {
245  std::vector<int> recipe = getComposition(type);
246  copyFromRawData(getRawData(), source, recipe);
247  }
248  }
249 
250 
251  template <class T1>
257  void copy(const PointCloud<T1>& alt)
258  {
259  resize(alt.width(), alt.height());
260  if (std::is_same<T, T1>::value) {
261  yAssert(dataSizeBytes() == alt.dataSizeBytes());
262  memcpy(const_cast<char*>(getRawData()), alt.getRawData(), dataSizeBytes());
263  } else {
264  std::vector<int> recipe = getComposition(alt.getPointType());
265  copyFromRawData(getRawData(), alt.getRawData(), recipe);
266  }
267  }
268 
269  virtual bool read(yarp::os::ConnectionReader& connection) override
270  {
271  connection.convertTextMode();
273  bool ok = connection.expectBlock((char*)&_header, sizeof(_header));
274  if (!ok) {
275  return false;
276  }
277 
278  data.resize(_header.height * _header.width);
279  std::memset((void*)data.getFirst(), 0, data.size() * sizeof(T));
280 
281  header.height = _header.height;
282  header.width = _header.width;
283  header.isDense = _header.isDense;
284 
285  if (header.pointType == _header.pointType) {
286  return data.read(connection);
287  }
288 
289  T* tmp = data.getFirst();
290 
291  yAssert(tmp != nullptr);
292 
293  // Skip the vector header....
294  connection.expectInt32();
295  connection.expectInt32();
296 
297  std::vector<int> recipe = getComposition(_header.pointType);
298 
300  for (size_t i = 0; i < data.size(); i++) {
301  for (size_t j = 0; j < recipe.size(); j++) {
302  size_t sizeToRead = pointType2Size(recipe[j]);
303  if ((header.pointType & recipe[j])) {
304  size_t offset = getOffset(header.pointType, recipe[j]);
305  connection.expectBlock((char*)&tmp[i] + offset, sizeToRead);
306  } else {
307  dummy.allocateOnNeed(sizeToRead, sizeToRead);
308  connection.expectBlock(dummy.bytes().get(), sizeToRead);
309  }
310  }
311  }
312 
313  connection.convertTextMode();
314  return true;
315  }
316 
317  virtual bool write(yarp::os::ConnectionWriter& writer) const override
318  {
319  writer.appendBlock((char*)&header, sizeof(PointCloudNetworkHeader));
320  return data.write(writer);
321  }
322 
323  virtual std::string toString(int precision = -1, int width = -1) const
324  {
325  std::string ret;
326  if (isOrganized()) {
327  for (size_t r = 0; r < this->width(); r++) {
328  for (size_t c = 0; c < this->height(); c++) {
329  ret += (*this)(r, c).toString(precision, width);
330  }
331  if (r < this->width() - 1) // if it is not the last row
332  {
333  ret += "\n";
334  }
335  }
336 
337  } else {
338  for (size_t i = 0; i < this->size(); i++) {
339  ret += (*this)(i).toString(precision, width);
340  }
341  }
342  return ret;
343  }
344 
350  {
352  ret.addInt32(width());
353  ret.addInt32(height());
354  ret.addInt32(getPointType());
355  ret.addInt32(isDense());
356 
357  for (size_t i = 0; i < this->size(); i++) {
358  ret.addList().append((*this)(i).toBottle());
359  }
360  return ret;
361  }
362 
370  bool fromBottle(const yarp::os::Bottle& bt)
371  {
372  if (bt.isNull()) {
373  return false;
374  }
375 
376  if (this->getPointType() != bt.get(2).asInt32()) {
377  return false;
378  }
379 
380  this->resize(bt.get(0).asInt32(), bt.get(1).asInt32());
381  this->header.isDense = bt.get(3).asInt32();
382 
383  if ((size_t)bt.size() != 4 + width() * height()) {
384  return false;
385  }
386 
387  for (size_t i = 0; i < this->size(); i++) {
388  (*this)(i).fromBottle(bt, i + 4);
389  }
390 
391  return true;
392  }
393 
394  virtual int getBottleTag() const override
395  {
396  return BottleTagMap<T>();
397  }
398 
399 private:
401 
403  {
404  if (std::is_same<T, DataXY>::value) {
406  return;
407  }
408 
409  if (std::is_same<T, DataXYZ>::value) {
411  return;
412  }
413 
414  if (std::is_same<T, DataNormal>::value) {
416  return;
417  }
418 
419  if (std::is_same<T, DataXYZRGBA>::value) {
421  return;
422  }
423 
424  if (std::is_same<T, DataXYZI>::value) {
426  return;
427  }
428 
429  if (std::is_same<T, DataInterestPointXYZ>::value) {
431  return;
432  }
433 
434  if (std::is_same<T, DataXYZNormal>::value) {
436  return;
437  }
438 
439  if (std::is_same<T, DataXYZNormalRGBA>::value) {
441  return;
442  }
443 
444 // DataRGBA has sense to implement them?
445 // intensity has sense to implement them?
446 // DataViewpoint has sense to implement them?
447 
448  header.pointType = 0;
449  }
450 };
451 
452 } // namespace sig
453 } // namespace yarp
454 
455 template <>
456 inline int BottleTagMap<yarp::sig::DataXY>()
457 {
458  return BOTTLE_TAG_FLOAT64;
459 }
460 
461 template <>
462 inline int BottleTagMap<yarp::sig::DataXYZ>()
463 {
464  return BOTTLE_TAG_FLOAT64;
465 }
466 
467 template <>
468 inline int BottleTagMap<yarp::sig::DataNormal>()
469 {
470  return BOTTLE_TAG_FLOAT64;
471 }
472 
473 template <>
474 inline int BottleTagMap<yarp::sig::DataXYZRGBA>()
475 {
476  return BOTTLE_TAG_FLOAT64;
477 }
478 
479 template <>
480 inline int BottleTagMap<yarp::sig::DataXYZI>()
481 {
482  return BOTTLE_TAG_FLOAT64;
483 }
484 
485 template <>
486 inline int BottleTagMap<yarp::sig::DataInterestPointXYZ>()
487 {
488  return BOTTLE_TAG_FLOAT64;
489 }
490 
491 template <>
492 inline int BottleTagMap<yarp::sig::DataXYZNormal>()
493 {
494  return BOTTLE_TAG_FLOAT64;
495 }
496 
497 template <>
498 inline int BottleTagMap<yarp::sig::DataXYZNormalRGBA>()
499 {
500  return BOTTLE_TAG_FLOAT64;
501 }
502 
503 
504 #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:212
PointCloud< T > & operator+=(const PointCloud< T > &rhs)
Concatenate a point cloud to the current cloud.
Definition: PointCloud.h:175
bool fromBottle(const yarp::os::Bottle &bt)
Populate the PointCloud from a yarp::os::Bottle.
Definition: PointCloud.h:370
The PointCloud class.
Definition: PointCloud.h:25
const PointCloud< T > & operator=(const PointCloud< T1 > &alt)
Assignment operator.
Definition: PointCloud.h:162
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:222
const PointCloud< T > operator+(const PointCloud< T > &rhs)
Concatenate a point cloud to another cloud.
Definition: PointCloud.h:202
virtual bool expectBlock(char *data, size_t len)=0
Read a block of data from the network connection.
virtual bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
Definition: PointCloud.h:269
virtual const char * getRawData() const override
Get the pointer to the data.
Definition: PointCloud.h:84
bool ret
virtual void resize(size_t width)
Resize the PointCloud.
Definition: PointCloud.h:77
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
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:151
Provides:
Definition: Vector.h:29
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:400
virtual size_t wireSizeBytes() const override
Get the size of the data + the header in terms of number of bytes.
Definition: PointCloud.h:94
An interface for writing to a network connection.
virtual void copyFromRawData(const char *dst, const char *source, std::vector< int > &recipe)
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:237
virtual size_t size() const override
Definition: PointCloud.h:109
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:70
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:349
PointCloud()
PointCloud, default constructor.
Definition: PointCloud.h:40
T & operator()(size_t i)
Obtain the point given by the index.
Definition: PointCloud.h:142
#define BOTTLE_TAG_FLOAT64
Definition: Bottle.h:27
virtual size_t pointType2Size(int type) const
#define yAssert(x)
Definition: Log.h:104
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 int getBottleTag() const override
Definition: PointCloud.h:394
virtual std::string toString(int precision=-1, int width=-1) const
Definition: PointCloud.h:323
virtual size_t width() const
void copy(const PointCloud< T1 > &alt)
Copy operator.
Definition: PointCloud.h:257
virtual bool write(yarp::os::ConnectionWriter &writer) const override
Write this object to a network connection.
Definition: PointCloud.h:317
PointCloud(const PointCloud< T1 > &alt)
PointCloud, copy constructor.
Definition: PointCloud.h:52
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:132
virtual size_t height() const
The yarp::sig::PointCloudNetworkHeader class.
virtual void resize(size_t width, size_t height)
Resize the PointCloud.
Definition: PointCloud.h:63
virtual 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:27
virtual size_t dataSizeBytes() const override
Get the size of the data in terms of number of bytes.
Definition: PointCloud.h:104
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:120
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