YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
PointCloud.cpp
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
7
8#include <algorithm>
9#include <type_traits>
10#include <cmath>
11
12namespace yarp::sig {
13
14template <class T>
15bool PointCloud<T>::compZ(const T& a, const T& b)
16{
17 if constexpr (has_member_z<T>::value)
18 {
19 return (a.z < b.z);
20 }
21 return false;
22}
23
24template <class T>
25void PointCloud<T>::resize(size_t width, size_t height)
26{
27 header.width = width;
28 header.height = height;
29 m_storage.resize(width * height);
30}
31
32template <class T>
33void PointCloud<T>::resize(size_t width)
34{
35 header.width = width;
36 header.height = 1;
37 m_storage.resize(width);
38}
39
40template <class T>
42{
43 m_storage.clear();
44 header.width = 0;
45 header.height = 0;
46}
47
48template <class T>
49void PointCloud<T>::fromExternalPC(const char* source, int type, size_t width, size_t height, bool isDense)
50{
51 yAssert(source);
52 header.isDense = isDense;
53 resize(width, height);
54 if (this->getPointType() == type) {
55 memcpy(const_cast<char*>(getRawData()), source, dataSizeBytes());
56 } else {
57 std::vector<int> recipe = getComposition(type);
58 copyFromRawData(getRawData(), source, recipe);
59 }
60}
61
62template <class T>
64{
65 connection.convertTextMode();
67 bool ok = connection.expectBlock((char*)&rcvHeader, sizeof(rcvHeader));
68 if (!ok) {
69 return false;
70 }
71
72 m_storage.resize(rcvHeader.height * rcvHeader.width);
73 std::memset((void*)m_storage.data(), 0, m_storage.size() * sizeof(T));
74
75 header.height = rcvHeader.height;
76 header.width = rcvHeader.width;
77 header.isDense = rcvHeader.isDense;
78 header.pointType = rcvHeader.pointType;
79
80 T* tmp = m_storage.data();
81 yAssert(tmp != nullptr);
82
83 // Writer always sends a contiguous block containing the raw array of T,
84 // so read it in one go.
85 size_t bytes = m_storage.size() * sizeof(T);
86 if (bytes > 0) {
87 if (!connection.expectBlock((char*)tmp, bytes)) {
88 return false;
89 }
90 }
91
92 connection.convertTextMode();
93 return true;
94}
95
96template <class T>
98{
99 writer.appendBlock((char*)&header, sizeof(PointCloudNetworkHeader));
100 auto storagepointer = m_storage.data();
101 writer.appendBlock((char*)storagepointer, m_storage.size() * sizeof(T));
102 return !writer.isError();
103}
104
105template <class T>
106std::string PointCloud<T>::toString(int precision, int width) const
107{
108 std::string ret;
109 if (isOrganized()) {
110 for (size_t r = 0; r < this->width(); r++) {
111 for (size_t c = 0; c < this->height(); c++) {
112 ret += (*this)(r, c).toString(precision, width);
113 }
114 if (r < this->width() - 1) // if it is not the last row
115 {
116 ret += "\n";
117 }
118 }
119
120 } else {
121 for (size_t i = 0; i < this->size(); i++) {
122 ret += (*this)(i).toString(precision, width);
123 }
124 }
125 return ret;
126}
127
128template <class T>
130{
132 ret.addInt32(width());
133 ret.addInt32(height());
134 ret.addInt32(getPointType());
135 ret.addInt32(isDense());
136
137 for (size_t i = 0; i < this->size(); i++) {
138 ret.addList().append((*this)(i).toBottle());
139 }
140 return ret;
141}
142
143template <class T>
145{
146 if (bt.isNull()) {
147 return false;
148 }
149
150 if (this->getPointType() != bt.get(2).asInt32()) {
151 return false;
152 }
153
154 this->resize(bt.get(0).asInt32(), bt.get(1).asInt32());
155 this->header.isDense = bt.get(3).asInt32();
156
157 if ((size_t)bt.size() != 4 + width() * height()) {
158 return false;
159 }
160
161 for (size_t i = 0; i < this->size(); i++) {
162 (*this)(i).fromBottle(bt, i + 4);
163 }
164
165 return true;
166}
167
168template <class T>
170{
171 if constexpr (has_member_z<T>::value)
172 {
173 //remove the points whose z value is nan
174 m_storage.erase(std::remove_if(m_storage.begin(), m_storage.end(), [](const T& point)
175 { return std::isnan(point.z); }),
176 m_storage.end());
177 //sort the points
178 std::sort(m_storage.begin(), m_storage.end(), compZ);
179 return true;
180 }
181
182 return false;
183}
184
185template <class T>
186bool PointCloud<T>::filterDataZ(double minZ, double maxZ)
187{
188 if constexpr (has_member_z<T>::value)
189 {
190 m_storage.erase(
191 std::remove_if(m_storage.begin(), m_storage.end(), [minZ, maxZ](const T& data) {
192 return data.z < minZ || data.z > maxZ;
193 }),
194 m_storage.end()
195 );
196 return true;
197 }
198
199 return false;
200}
201
202template <class T>
204{
205 if (std::is_same<T, DataXY>::value) {
206 header.pointType = PCL_POINT2D_XY;
207 return;
208 }
209
210 if (std::is_same<T, DataXYZ>::value) {
211 header.pointType = PCL_POINT_XYZ;
212 return;
213 }
214
215 if (std::is_same<T, DataNormal>::value) {
216 header.pointType = PCL_NORMAL;
217 return;
218 }
219
220 if (std::is_same<T, DataXYZRGBA>::value) {
221 header.pointType = PCL_POINT_XYZ_RGBA;
222 return;
223 }
224
225 if (std::is_same<T, DataXYZI>::value) {
226 header.pointType = PCL_POINT_XYZ_I;
227 return;
228 }
229
230 if (std::is_same<T, DataInterestPointXYZ>::value) {
231 header.pointType = PCL_INTEREST_POINT_XYZ;
232 return;
233 }
234
235 if (std::is_same<T, DataXYZNormal>::value) {
236 header.pointType = PCL_POINT_XYZ_NORMAL;
237 return;
238 }
239
240 if (std::is_same<T, DataXYZNormalRGBA>::value) {
241 header.pointType = PCL_POINT_XYZ_NORMAL_RGBA;
242 return;
243 }
244
245 // DataRGBA has sense to implement them?
246 // intensity has sense to implement them?
247 // DataViewpoint has sense to implement them?
248
249 header.pointType = 0;
250}
251
252template <class T>
254{
255 return BottleTagMap<T>();
256}
257
258template <class T>
260{
261 yAssert(getPointType() == rhs.getPointType());
262
263 size_t nr_points = m_storage.size();
264 m_storage.resize(nr_points + rhs.size());
265 for (size_t i = nr_points; i < m_storage.size(); ++i) {
266 m_storage[i] = rhs.m_storage[i - nr_points];
267 }
268
269 header.width = m_storage.size();
270 header.height = 1;
271 if (rhs.isDense() && isDense()) {
272 header.isDense = 1;
273 } else {
274 header.isDense = 0;
275 }
276 return (*this);
277}
278
279template <class T>
281{
282 return (PointCloud<T>(*this) += rhs);
283}
284
285template <class T>
287{
288 m_storage.push_back(pt);
289 header.width = m_storage.size();
290 header.height = 1;
291}
292
293} //namespace yarp::sig
294
std::string toString(const T &value)
convert an arbitrary type to string.
#define yAssert(x)
Definition Log.h:388
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:64
void append(const Bottle &alt)
Append the content of the given bottle to the current list.
Definition Bottle.cpp:354
Bottle & addList()
Places an empty nested list in the bottle, at the end of the list.
Definition Bottle.cpp:188
size_type size() const
Gets the number of elements in the bottle.
Definition Bottle.cpp:252
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition Bottle.cpp:247
void addInt32(std::int32_t x)
Places a 32-bit integer in the bottle, at the end of the list.
Definition Bottle.cpp:140
bool isNull() const override
Checks if the object is invalid.
Definition Bottle.cpp:344
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 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 bool isError() const =0
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:204
virtual bool isDense() const
virtual int getPointType() const
The yarp::sig::PointCloudNetworkHeader class.
The PointCloud class.
Definition PointCloud.h:21
const PointCloud< T > operator+(const PointCloud< T > &rhs)
Concatenate a point cloud to another cloud.
bool fromBottle(const yarp::os::Bottle &bt)
Populate the PointCloud from a yarp::os::Bottle.
virtual void resize(size_t width, size_t height)
Resize the PointCloud.
void push_back(const T &pt)
Insert a new point in the cloud, at the end of the container.
virtual void fromExternalPC(const char *source, int type, size_t width, size_t height, bool isDense=true)
Copy the content of an external PointCloud.
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
yarp::os::Bottle toBottle() const
Generate a yarp::os::Bottle filled with the PointCloud data.
int getBottleTag() const override
PointCloud< T > & operator+=(const PointCloud< T > &rhs)
Concatenate a point cloud to the current cloud.
virtual void clear()
Clear the data.
bool write(yarp::os::ConnectionWriter &writer) const override
Write this object to a network connection.
virtual std::string toString(int precision=-1, int width=-1) const
bool filterDataZ(double minZ=0, double maxZ=std::numeric_limits< double >::infinity())
Filter out points which are not included in the specified range.
size_t size() const override
Definition PointCloud.h:99
bool sortDataZ()
Rearranges the pointcloud data so that the points are ordered from the nearest to the farthest.
@ PCL_POINT_XYZ_NORMAL_RGBA
#define YARP_sig_API
Definition api.h:18