15bool PointCloud<T>::compZ(
const T& a,
const T& b)
17 if constexpr (has_member_z<T>::value)
28 header.height = height;
29 m_storage.resize(width * height);
37 m_storage.resize(width);
52 header.isDense = isDense;
53 resize(width, height);
54 if (this->getPointType() == type) {
55 memcpy(
const_cast<char*
>(getRawData()), source, dataSizeBytes());
57 std::vector<int> recipe = getComposition(type);
58 copyFromRawData(getRawData(), source, recipe);
67 bool ok = connection.
expectBlock((
char*)&rcvHeader,
sizeof(rcvHeader));
72 m_storage.resize(rcvHeader.
height * rcvHeader.
width);
73 std::memset((
void*)m_storage.data(), 0, m_storage.size() *
sizeof(T));
75 header.height = rcvHeader.
height;
76 header.width = rcvHeader.
width;
77 header.isDense = rcvHeader.
isDense;
80 T* tmp = m_storage.data();
85 size_t bytes = m_storage.size() *
sizeof(T);
100 auto storagepointer = m_storage.data();
101 writer.
appendBlock((
char*)storagepointer, m_storage.size() *
sizeof(T));
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);
114 if (r < this->width() - 1)
121 for (
size_t i = 0; i < this->size(); i++) {
122 ret += (*this)(i).
toString(precision, width);
137 for (
size_t i = 0; i < this->size(); i++) {
150 if (this->getPointType() != bt.
get(2).
asInt32()) {
157 if ((
size_t)bt.
size() != 4 + width() * height()) {
161 for (
size_t i = 0; i < this->size(); i++) {
162 (*this)(i).fromBottle(bt, i + 4);
174 m_storage.erase(std::remove_if(m_storage.begin(), m_storage.end(), [](
const T& point)
175 { return std::isnan(point.z); }),
178 std::sort(m_storage.begin(), m_storage.end(), compZ);
191 std::remove_if(m_storage.begin(), m_storage.end(), [minZ, maxZ](
const T& data) {
192 return data.z < minZ || data.z > maxZ;
205 if (std::is_same<T, DataXY>::value) {
210 if (std::is_same<T, DataXYZ>::value) {
215 if (std::is_same<T, DataNormal>::value) {
220 if (std::is_same<T, DataXYZRGBA>::value) {
225 if (std::is_same<T, DataXYZI>::value) {
230 if (std::is_same<T, DataInterestPointXYZ>::value) {
235 if (std::is_same<T, DataXYZNormal>::value) {
240 if (std::is_same<T, DataXYZNormalRGBA>::value) {
249 header.pointType = 0;
255 return BottleTagMap<T>();
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];
269 header.width = m_storage.size();
271 if (rhs.
isDense() && isDense()) {
289 header.width = m_storage.size();
std::string toString(const T &value)
convert an arbitrary type to string.
A simple collection of objects that can be described and transmitted in a portable way.
void append(const Bottle &alt)
Append the content of the given bottle to the current list.
Bottle & addList()
Places an empty nested list in the bottle, at the end of the list.
size_type size() const
Gets the number of elements in the bottle.
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
void addInt32(std::int32_t x)
Places a 32-bit integer in the bottle, at the end of the list.
bool isNull() const override
Checks if the object is invalid.
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.
virtual bool isDense() const
virtual int getPointType() const
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
bool sortDataZ()
Rearranges the pointcloud data so that the points are ordered from the nearest to the farthest.
@ PCL_POINT_XYZ_NORMAL_RGBA