From 36dab075f7ab571dd619929543a239ebaf48ac7b Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Fri, 12 Aug 2022 15:16:53 +0000 Subject: [PATCH 01/54] add grpc interface for point clouds --- seerep-com/CMakeLists.txt | 1 + seerep-com/fbs/point_cloud_service.fbs | 12 ++++++++++++ 2 files changed, 13 insertions(+) create mode 100644 seerep-com/fbs/point_cloud_service.fbs diff --git a/seerep-com/CMakeLists.txt b/seerep-com/CMakeLists.txt index 25d84f79b..f34b9495b 100644 --- a/seerep-com/CMakeLists.txt +++ b/seerep-com/CMakeLists.txt @@ -35,6 +35,7 @@ set(MY_FBS_FILES fbs/point_service.fbs fbs/tf_service.fbs fbs/transfer_sensor_msgs.fbs + fbs/point_cloud_service.fbs ) build_flatbuffers("${MY_FBS_FILES}" "${SeerepMsgs_FLATBUFFERS_IMPORT_DIRS}" fbschemas "" "${CMAKE_CURRENT_BINARY_DIR}/fbs" "${CMAKE_CURRENT_BINARY_DIR}/fbs" "" ) diff --git a/seerep-com/fbs/point_cloud_service.fbs b/seerep-com/fbs/point_cloud_service.fbs new file mode 100644 index 000000000..da81bcf43 --- /dev/null +++ b/seerep-com/fbs/point_cloud_service.fbs @@ -0,0 +1,12 @@ + +include "point_cloud_2.fbs"; +include "query.fbs"; + +include "server_response.fbs"; + +namespace seerep.fb; + +rpc_service PointCloudService { + GetPointCloud2(seerep.fb.Query):seerep.fb.PointCloud2 (streaming: "server"); + TransferPointCloud2(seerep.fb.PointCloud2):seerep.fb.ServerResponse (streaming: "client"); +} From 55fd9c0a2398e5404f33f08164d67893035a0386 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mark=20H=C3=B6llmann?= Date: Fri, 12 Aug 2022 17:52:37 +0000 Subject: [PATCH 02/54] fix build error grpc fb service pointcloud --- seerep-msgs/fbs/point_field.fbs | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/seerep-msgs/fbs/point_field.fbs b/seerep-msgs/fbs/point_field.fbs index 2286adf50..1ddf33697 100644 --- a/seerep-msgs/fbs/point_field.fbs +++ b/seerep-msgs/fbs/point_field.fbs @@ -1,5 +1,5 @@ -namespace seerep.fb.PointField_; +namespace seerep.fb; enum Datatype : int { UNSET = 0, @@ -13,11 +13,9 @@ enum Datatype : int { FLOAT64 = 8, } -namespace seerep.fb; - table PointField { name:string; offset:uint; - datatype:seerep.fb.PointField_.Datatype; + datatype:Datatype; count:uint; } From 8f9163a499d31613b81ae8aa74949af0566d477d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Mark=20H=C3=B6llmann?= Date: Mon, 15 Aug 2022 06:14:33 +0000 Subject: [PATCH 03/54] apply changed pointcloud fb-msg to other pkgs --- .../src/seerep_ros_conversions_fb/conversions.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/seerep-ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp b/seerep-ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp index ba015b527..1f27eacdc 100644 --- a/seerep-ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp +++ b/seerep-ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp @@ -62,7 +62,7 @@ flatbuffers::Offset toFlat(const sensor_msgs::PointField seerep::fb::PointFieldBuilder pointFieldBuilder(builder); pointFieldBuilder.add_name(nameOffset); pointFieldBuilder.add_offset(point_field.offset); - pointFieldBuilder.add_datatype(seerep::fb::PointField_::Datatype(point_field.datatype)); + pointFieldBuilder.add_datatype(seerep::fb::Datatype(point_field.datatype)); pointFieldBuilder.add_count(point_field.count); return pointFieldBuilder.Finish(); } From 7ad2fcf280c23d29cd18b724a26d347ee03ceea4 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 29 Aug 2022 08:23:03 +0000 Subject: [PATCH 04/54] update point_field fb message, due to name clashing --- seerep-msgs/fbs/point_field.fbs | 4 ++-- .../src/seerep_ros_conversions_fb/conversions.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/seerep-msgs/fbs/point_field.fbs b/seerep-msgs/fbs/point_field.fbs index 1ddf33697..ec8349092 100644 --- a/seerep-msgs/fbs/point_field.fbs +++ b/seerep-msgs/fbs/point_field.fbs @@ -1,7 +1,7 @@ namespace seerep.fb; -enum Datatype : int { +enum Point_Field_Datatype : int { UNSET = 0, INT8 = 1, UINT8 = 2, @@ -16,6 +16,6 @@ enum Datatype : int { table PointField { name:string; offset:uint; - datatype:Datatype; + datatype:Point_Field_Datatype; count:uint; } diff --git a/seerep-ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp b/seerep-ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp index 1f27eacdc..1908d373d 100644 --- a/seerep-ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp +++ b/seerep-ros/seerep_ros_conversions_fb/src/seerep_ros_conversions_fb/conversions.cpp @@ -62,7 +62,7 @@ flatbuffers::Offset toFlat(const sensor_msgs::PointField seerep::fb::PointFieldBuilder pointFieldBuilder(builder); pointFieldBuilder.add_name(nameOffset); pointFieldBuilder.add_offset(point_field.offset); - pointFieldBuilder.add_datatype(seerep::fb::Datatype(point_field.datatype)); + pointFieldBuilder.add_datatype(seerep::fb::Point_Field_Datatype(point_field.datatype)); pointFieldBuilder.add_count(point_field.count); return pointFieldBuilder.Finish(); } From 1563b4ca37263807a907e2e83b15285b7750df10 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 30 Aug 2022 06:23:23 +0000 Subject: [PATCH 05/54] WIP: point cloud iterator for fb --- seerep-hdf5/seerep-hdf5-fb/CMakeLists.txt | 1 + .../hdf5-fb-point-cloud2-iterator.h | 334 ++++++++++++++++++ .../impl/hdf5-fb-point-cloud2-iterator.hpp | 297 ++++++++++++++++ 3 files changed, 632 insertions(+) create mode 100644 seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h create mode 100644 seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp diff --git a/seerep-hdf5/seerep-hdf5-fb/CMakeLists.txt b/seerep-hdf5/seerep-hdf5-fb/CMakeLists.txt index 315647068..eb05a7a2b 100644 --- a/seerep-hdf5/seerep-hdf5-fb/CMakeLists.txt +++ b/seerep-hdf5/seerep-hdf5-fb/CMakeLists.txt @@ -41,6 +41,7 @@ add_library(seerephdf5fb src/hdf5-fb-general.cpp src/hdf5-fb-tf.cpp src/hdf5-fb-image.cpp + src/hdf5-fb-pointcloud.cpp src/hdf5-fb-point.cpp ) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h new file mode 100644 index 000000000..3398b72ad --- /dev/null +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h @@ -0,0 +1,334 @@ +// Copyright (c) 2013, Open Source Robotics Foundation, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// This file is originally from: +// https://github.com/ros/common_msgs/blob/275b09a/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h + +#ifndef SENSOR_MSGS__POINT_CLOUD2_ITERATOR_HPP_ +#define SENSOR_MSGS__POINT_CLOUD2_ITERATOR_HPP_ + +#include +#include +#include + +#include +#include +#include + +/** + * @brief Tools for manipulating sensor_msgs + * + * This file provides two sets of utilities to modify and parse PointCloud2 + * The first set allows you to conveniently set the fields by hand: + * + * @verbatim + * #include + * // Create a PointCloud2 + * seerep::PointCloud2 cloud_msg; + * // Fill some internals of the PoinCloud2 like the header/width/height ... + * cloud_msgs.height = 1; cloud_msgs.width = 4; + * // Set the point fields to xyzrgb and resize the vector with the following command + * // 4 is for the number of added fields. Each come in triplet: the name of the PointField, + * // the number of occurrences of the type in the PointField, the type of the PointField + * seerep::PointCloud2Modifier modifier(cloud_msg); + * modifier.setPointCloud2Fields(4, "x", 1, seerep::PointField::FLOAT32, + * "y", 1, seerep::PointField::FLOAT32, + * "z", 1, seerep::PointField::FLOAT32, + * "rgb", 1, seerep::PointField::FLOAT32); + * // For convenience and the xyz, rgb, rgba fields, you can also use the following overloaded function. + * // You have to be aware that the following function does add extra padding for backward compatibility though + * // so it is definitely the solution of choice for PointXYZ and PointXYZRGB + * // 2 is for the number of fields to add + * modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + * // You can then reserve / resize as usual + * modifier.resize(100); + * @endverbatim + * + * The second set allows you to traverse your PointCloud using an iterator: + * + * @verbatim + * // Define some raw data we'll put in the PointCloud2 + * float point_data[] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0}; + * uint8_t color_data[] = {40, 80, 120, 160, 200, 240, 20, 40, 60, 80, 100, 120}; + * // Define the iterators. When doing so, you define the Field you would like to iterate upon and + * // the type of you would like returned: it is not necessary the type of the PointField as sometimes + * // you pack data in another type (e.g. 3 uchar + 1 uchar for RGB are packed in a float) + * sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); + * sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); + * sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); + * // Even though the r,g,b,a fields do not exist (it's usually rgb, rgba), you can create iterators for + * // those: they will handle data packing for you (in little endian RGB is packed as \*,R,G,B in a float + * // and RGBA as A,R,G,B) + * sensor_msgs::PointCloud2Iterator iter_r(cloud_msg, "r"); + * sensor_msgs::PointCloud2Iterator iter_g(cloud_msg, "g"); + * sensor_msgs::PointCloud2Iterator iter_b(cloud_msg, "b"); + * // Fill the PointCloud2 + * for(size_t i=0; i class V> +class PointCloud2IteratorBase +{ +public: + /** + */ + PointCloud2IteratorBase(); + + /** + * @param cloud_msg The PointCloud2 to iterate upon + * @param field_name The field to iterate upon + */ + PointCloud2IteratorBase(C& cloud_msg, const std::string& field_name); + + /** Assignment operator + * @param iter the iterator to copy data from + * @return a reference to *this + */ + V& operator=(const V& iter); + + /** Access the i th element starting at the current pointer (useful when a field has several elements of the same + * type) + * @param i + * @return a reference to the i^th value from the current position + */ + TT& operator[](size_t i) const; + + /** Dereference the iterator. Equivalent to accessing it through [0] + * @return the value to which the iterator is pointing + */ + TT& operator*() const; + + /** Increase the iterator to the next element + * @return a reference to the updated iterator + */ + V& operator++(); + + /** Basic pointer addition + * @param i the amount to increase the iterator by + * @return an iterator with an increased position + */ + V operator+(int i); + + /** Increase the iterator by a certain amount + * @return a reference to the updated iterator + */ + V& operator+=(int i); + + /** Compare to another iterator + * @return whether the current iterator points to a different address than the other one + */ + bool operator!=(const V& iter) const; + + /** Return the end iterator + * @return the end iterator (useful when performing normal iterator processing with ++) + */ + V end() const; + + /** Common code to set the field of the PointCloud2 + * @param cloud_msg the PointCloud2 to modify + * @param field_name the name of the field to iterate upon + * @return the offset at which the field is found + */ + int set_field(const seerep::fb::PointCloud2& cloud_msg, const std::string& field_name); + + /** The "point_step" of the original cloud */ + int point_step_; + /** The raw data in uchar* where the iterator is */ + U* data_char_; + /** The cast data where the iterator is */ + TT* data_; + /** The end() pointer of the iterator */ + TT* data_end_; + /** Whether the fields are stored as bigendian */ + bool is_bigendian_; +}; +} // namespace impl + +/** + * @brief Class that can iterate over a PointCloud2 + * + * T type of the element being iterated upon + * E.g, you create your PointClou2 message as follows: + * @verbatim + * setPointCloud2FieldsByString(cloud_msg, 2, "xyz", "rgb"); + * @endverbatim + * + * For iterating over XYZ, you do : + * @verbatim + * seerep::PointCloud2Iterator iter_x(cloud_msg, "x"); + * @endverbatim + * and then access X through iter_x[0] or *iter_x + * You could create an iterator for Y and Z too but as they are consecutive, + * you can just use iter_x[1] and iter_x[2] + * + * For iterating over RGB, you do: + * @verbatim + * seerep::PointCloud2Iterator iter_rgb(cloud_msg, "rgb"); + * @endverbatim + * and then access R,G,B through iter_rgb[0], iter_rgb[1], iter_rgb[2] + */ +template +class PointCloud2Iterator + : public impl::PointCloud2IteratorBase +{ +public: + /** + * @brief Construct a new PointCloud2Const iterator based on a cloud message. + * + * @param cloud_msg the cloud message to use + * @param field_name the field to iterate over + */ + PointCloud2Iterator(seerep::fb::PointCloud2& cloud_msg, const std::string& field_name) + : impl::PointCloud2IteratorBase::PointCloud2IteratorBase(cloud_msg, field_name) + { + int offset = this->set_field(cloud_msg, field_name); + this->data_char_ = reinterpret_cast(cloud_msg.mutable_data()->data() + offset); + this->data_ = reinterpret_cast(this->data_char_); + this->data_end_ = reinterpret_cast(cloud_msg.mutable_data()->data() + cloud_msg.data()->size() + offset); + } +}; + +/** + * @brief Same as a PointCloud2Iterator but for const data + */ +template +class PointCloud2ConstIterator + : public impl::PointCloud2IteratorBase +{ +public: + /** + * @brief Construct a new PointCloud2Const iterator based on a cloud message. + * + * @param cloud_msg the cloud message to use + * @param field_name the field to iterate over + */ + PointCloud2ConstIterator(const seerep::fb::PointCloud2& cloud_msg, const std::string& field_name) + : impl::PointCloud2IteratorBase::PointCloud2IteratorBase(cloud_msg, + field_name) + { + int offset = this->set_field(cloud_msg, field_name); + this->data_char_ = reinterpret_cast(cloud_msg.data()->data() + offset); + this->data_ = reinterpret_cast(this->data_char_); + this->data_end_ = reinterpret_cast(cloud_msg.data()->data() + cloud_msg.data()->size() + offset); + } +}; +} // namespace seerep_hdf5_fb + +#include "impl/hdf5-fb-point-cloud2-iterator.hpp" // NOLINT + +#endif // SENSOR_MSGS__POINT_CLOUD2_ITERATOR_HPP_ diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp new file mode 100644 index 000000000..e31d333c7 --- /dev/null +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp @@ -0,0 +1,297 @@ +// Copyright (c) 2013, Open Source Robotics Foundation, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +// This file is originally from: +// https://github.com/ros/common_msgs/blob/50ee957/sensor_msgs/include/sensor_msgs/impl/point_cloud2_iterator.h + +#ifndef seerep_hdf5_fb__IMPL__POINT_CLOUD2_ITERATOR_HPP_ +#define seerep_hdf5_fb__IMPL__POINT_CLOUD2_ITERATOR_HPP_ + +#include +#include + +#include +#include +#include +#include + +// namespace +// { +// /** Return the size of a datatype (which is an enum of seerep::PointField::) in bytes +// * @param datatype one of the enums of seerep::PointField:: +// */ +// inline int sizeOfPointField(int datatype) +// { +// switch (datatype) +// { +// case seerep::fb::Point_Field_Datatype_INT8: +// case seerep::fb::Point_Field_Datatype_UINT8: +// return 1; +// case seerep::fb::Point_Field_Datatype_INT16: +// case seerep::fb::Point_Field_Datatype_UINT16: +// return 2; +// case seerep::fb::Point_Field_Datatype_INT32: +// case seerep::fb::Point_Field_Datatype_UINT32: +// case seerep::fb::Point_Field_Datatype_FLOAT32: +// return 4; +// case seerep::fb::Point_Field_Datatype_FLOAT64: +// return 8; + +// default: +// std::stringstream err; +// err << "PointField of type " << datatype << " does not exist" << std::endl; +// throw std::runtime_error(err.str()); +// return -1; +// } +// } + +// /** Private function that adds a PointField to the "fields" member of a PointCloud2 +// * @param cloud_msg the PointCloud2 to add a field to +// * @param name the name of the field +// * @param count the number of elements in the PointField +// * @param datatype the datatype of the elements +// * @param offset the offset of that element +// * @return the offset of the next PointField that will be added to the PointCloud2 +// */ +// inline int addPointField(seerep::fb::PointCloud2& cloud_msg, const std::string& name, int count, int datatype, +// int offset) +// { +// // ToDO add point_field to point cloud if it's needed + +// // Update the offset +// return offset + count * sizeOfPointField(datatype); +// } +// } // namespace + +namespace seerep_hdf5_fb +{ +namespace impl +{ +/** + */ +template class V> +PointCloud2IteratorBase::PointCloud2IteratorBase() : data_char_(0), data_(0), data_end_(0) +{ +} + +/** + * @param cloud_msg The PointCloud2 to iterate upon + * @param field_name The field to iterate upon + */ +template class V> +PointCloud2IteratorBase::PointCloud2IteratorBase(C& cloud_msg, const std::string& field_name) +{ + (void)cloud_msg; // ignore that variable without causing warnings + (void)field_name; // ignore that variable without causing warnings +} + +/** Assignment operator + * @param iter the iterator to copy data from + * @return a reference to *this + */ +template class V> +V& PointCloud2IteratorBase::operator=(const V& iter) +{ + if (this != &iter) + { + point_step_ = iter.point_step_; + data_char_ = iter.data_char_; + data_ = iter.data_; + data_end_ = iter.data_end_; + is_bigendian_ = iter.is_bigendian_; + } + + return *this; +} + +/** Access the i th element starting at the current pointer (useful when a field has several elements of the same + * type) + * @param i + * @return a reference to the i^th value from the current position + */ +template class V> +TT& PointCloud2IteratorBase::operator[](size_t i) const +{ + return *(data_ + i); +} + +/** Dereference the iterator. Equivalent to accessing it through [0] + * @return the value to which the iterator is pointing + */ +template class V> +TT& PointCloud2IteratorBase::operator*() const +{ + return *data_; +} + +/** Increase the iterator to the next element + * @return a reference to the updated iterator + */ +template class V> +V& PointCloud2IteratorBase::operator++() +{ + data_char_ += point_step_; + data_ = reinterpret_cast(data_char_); + return *static_cast*>(this); +} + +/** Basic pointer addition + * @param i the amount to increase the iterator by + * @return an iterator with an increased position + */ +template class V> +V PointCloud2IteratorBase::operator+(int i) +{ + V res = *static_cast*>(this); + + res.data_char_ += i * point_step_; + res.data_ = reinterpret_cast(res.data_char_); + + return res; +} + +/** Increase the iterator by a certain amount + * @return a reference to the updated iterator + */ +template class V> +V& PointCloud2IteratorBase::operator+=(int i) +{ + data_char_ += i * point_step_; + data_ = reinterpret_cast(data_char_); + return *static_cast*>(this); +} + +/** Compare to another iterator + * @return whether the current iterator points to a different address than the other one + */ +template class V> +bool PointCloud2IteratorBase::operator!=(const V& iter) const +{ + return iter.data_ != data_; +} + +/** Return the end iterator + * @return the end iterator (useful when performing normal iterator processing with ++) + */ +template class V> +V PointCloud2IteratorBase::end() const +{ + V res = *static_cast*>(this); + res.data_ = data_end_; + return res; +} + +/** Common code to set the field of the PointCloud2 + * @param cloud_msg the PointCloud2 to modify + * @param field_name the name of the field to iterate upon + * @return the offset at which the field is found + */ +template class V> +int PointCloud2IteratorBase::set_field(const seerep::fb::PointCloud2& cloud_msg, + const std::string& field_name) +{ + is_bigendian_ = cloud_msg.is_bigendian(); + point_step_ = cloud_msg.point_step(); + // make sure the channel is valid + + for (size_t i = 0; i < cloud_msg.fields()->size(); i++) + { + const seerep::fb::PointField& field = cloud_msg.fields()->Get(i); + if (field.name()->str() == field_name) + { + return field.offset(); + } + } + + // // Handle the special case of r,g,b,a (we assume they are understood as the + // // channels of an rgb or rgba field) + + std::set field_names = { "r", "g", "b", "a" }; + + if (field_names.find(field_name) != field_names.end()) + { + for (int i = 0; i < cloud_msg.fields()->size(); i++) + { + const seerep::fb::PointField& field = cloud_msg.fields()->Get(i); + + if (field.name()->str() == "rgb" || field.name()->str() == "rgba") + { + if (field_name == "r") + { + if (is_bigendian_) + { + return field.offset() + 1; + } + else + { + return field.offset() + 2; + } + } + if (field_name == "g") + { + if (is_bigendian_) + { + return field.offset() + 2; + } + else + { + return field.offset() + 1; + } + } + if (field_name == "b") + { + if (is_bigendian_) + { + return field.offset() + 3; + } + else + { + return field.offset() + 0; + } + } + if (field_name == "a") + { + if (is_bigendian_) + { + return field.offset() + 0; + } + else + { + return field.offset() + 3; + } + } + } + } + } + throw std::runtime_error("Field " + field_name + " does not exist!"); +} + +} // namespace impl +} // namespace seerep_hdf5_fb + +#endif // seerep_hdf5_fb__IMPL__POINT_CLOUD2_ITERATOR_HPP_ From 3261cbe4cf8543f0a603a4df3e5fff0330c2e237 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 30 Aug 2022 11:24:09 +0000 Subject: [PATCH 06/54] add label with instances to fb point cloud --- seerep-msgs/fbs/point_cloud_2.fbs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/seerep-msgs/fbs/point_cloud_2.fbs b/seerep-msgs/fbs/point_cloud_2.fbs index 8e079f0e8..484e91920 100644 --- a/seerep-msgs/fbs/point_cloud_2.fbs +++ b/seerep-msgs/fbs/point_cloud_2.fbs @@ -2,6 +2,7 @@ include "header.fbs"; include "boundingbox_labeled.fbs"; include "point_field.fbs"; +include "label_with_instance.fbs"; namespace seerep.fb; @@ -15,6 +16,6 @@ table PointCloud2 { row_step:uint; data:[ubyte]; is_dense:bool; - labels_general:[string]; + labels_general:[seerep.fb.LabelWithInstance]; labels_bb:[seerep.fb.BoundingBoxLabeled]; } From 84d4526bf94fd9dbdbc7e2fd1831d43098a1709c Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 6 Sep 2022 14:47:18 +0000 Subject: [PATCH 07/54] WIP: fb point cloud --- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 117 ++++ .../impl/hdf5-fb-point-cloud2-iterator.hpp | 6 +- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 518 ++++++++++++++++++ 3 files changed, 638 insertions(+), 3 deletions(-) create mode 100644 seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h create mode 100644 seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h new file mode 100644 index 000000000..aa2ab9d18 --- /dev/null +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -0,0 +1,117 @@ +#ifndef SEEREP_HDF5_FB_HDF5_FB_POINT_CLOUD_H_ +#define SEEREP_HDF5_FB_HDF5_FB_POINT_CLOUD_H_ + +// highfive +#include + +// seerep-hdf5 +#include + +#include "seerep-hdf5-fb/hdf5-fb-general.h" +#include "seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h" + +// seerep-msgs +#include + +// std +#include +#include + +namespace seerep_hdf5_fb +{ +class Hdf5FbPointCloud : public Hdf5FbGeneral +{ +public: + Hdf5FbPointCloud(std::shared_ptr& file, std::shared_ptr& write_mtx); + + std::map getPointClouds(); + + std::shared_ptr writePointCloud2(const std::string& uuid, const seerep::fb::PointCloud2& pointcloud2); + + std::optional readPointCloud2(const std::string& uuid); + + std::vector loadBoundingBox(const std::string& uuid); + +private: + struct CloudInfo + { + bool has_points = false; + bool has_rgb = false; + bool has_rgba = false; + bool has_normals = false; + std::map other_fields; + }; + + template + void read(const std::string cloud_uuid, const std::string& field_name, seerep::fb::PointCloud2& cloud, size_t size) + { + const std::string id = seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX + "/" + cloud_uuid + "/" + field_name; + PointCloud2Iterator iter(cloud, field_name); + HighFive::DataSet dataset = m_file->getDataSet(id); + std::vector data; + data.reserve(size); + dataset.read(data); + + for (auto& value : data) + { + *iter = value; + ++iter; + } + } + + template + void write(const std::string cloud_uuid, const std::string& field_name, const seerep::fb ::PointCloud2& cloud, + size_t size) + { + const std::string id = seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX + "/" + cloud_uuid + "/" + field_name; + HighFive::DataSpace data_space(size); + + std::shared_ptr dataset_ptr; + if (!m_file->exist(id)) + dataset_ptr = std::make_shared(m_file->createDataSet(id, data_space)); + else + dataset_ptr = std::make_shared(m_file->getDataSet(id)); + + PointCloud2ConstIterator iter(cloud, field_name); + std::vector data; + data.reserve(size); + + for (size_t i = 0; i < size; i++) + { + data.push_back(*iter); + ++iter; + } + + dataset_ptr->write(data); + } + + CloudInfo getCloudInfo(const seerep::fb::PointCloud2& cloud); + + void writePoints(const std::string& uuid, const seerep::fb::PointCloud2& cloud); + + void writeColorsRGB(const std::string& uuid, const seerep::fb::PointCloud2& cloud); + + void writeColorsRGBA(const std::string& uuid, const seerep::fb::PointCloud2& cloud); + + void writeOtherFields(const std::string& uuid, const seerep::fb::PointCloud2& cloud, + const std::map& fields); + + void + writePointFieldAttributes(HighFive::Group& cloud_group, + const flatbuffers::Vector>& vectorPointField); + + void readPoints(const std::string& uuid, seerep::fb::PointCloud2& cloud); + + void readColorsRGB(const std::string& uuid, seerep::fb::PointCloud2& cloud); + + void readColorsRGBA(const std::string& uuid, seerep::fb::PointCloud2& cloud); + + void readOtherFields(const std::string& uuid, seerep::fb::PointCloud2& cloud, + const std::map& fields); + + flatbuffers::Vector> + readPointFieldAttributes(HighFive::Group& cloud_group); +}; +} // namespace seerep_hdf5_fb + +#endif /* SEEREP_HDF5_PB_HDF5_FB_POINT_CLOUD_H_ */ diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp index e31d333c7..674382b1c 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp @@ -221,7 +221,7 @@ int PointCloud2IteratorBase::set_field(const seerep::fb::PointCl for (size_t i = 0; i < cloud_msg.fields()->size(); i++) { - const seerep::fb::PointField& field = cloud_msg.fields()->Get(i); + const seerep::fb::PointField& field = *cloud_msg.fields()->Get(i); if (field.name()->str() == field_name) { return field.offset(); @@ -235,9 +235,9 @@ int PointCloud2IteratorBase::set_field(const seerep::fb::PointCl if (field_names.find(field_name) != field_names.end()) { - for (int i = 0; i < cloud_msg.fields()->size(); i++) + for (size_t i = 0; i < cloud_msg.fields()->size(); i++) { - const seerep::fb::PointField& field = cloud_msg.fields()->Get(i); + const seerep::fb::PointField& field = *cloud_msg.fields()->Get(i); if (field.name()->str() == "rgb" || field.name()->str() == "rgba") { diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp new file mode 100644 index 000000000..21646aebd --- /dev/null +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -0,0 +1,518 @@ +#include "seerep-hdf5-fb/hdf5-fb-pointcloud.h" + +#include + +namespace seerep_hdf5_fb +{ +Hdf5FbPointCloud::Hdf5FbPointCloud(std::shared_ptr& file, std::shared_ptr& write_mtx) + : Hdf5FbGeneral(file, write_mtx) +{ +} + +std::map Hdf5FbPointCloud::getPointClouds() +{ + // const std::scoped_lock lock(*m_write_mtx); + + // std::map map; + // if (m_file->exist(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD)) + // { + // const HighFive::Group& clouds_group = m_file->getGroup(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD); + // for (std::string& cloud_name : clouds_group.listObjectNames()) + // { + // map.insert(std::pair(cloud_name, clouds_group.getGroup(cloud_name))); + // } + // } + // return map; +} + +std::shared_ptr Hdf5FbPointCloud::writePointCloud2(const std::string& uuid, + const seerep::fb::PointCloud2& pointcloud2) +{ + // const std::scoped_lock lock(*m_write_mtx); + + // std::string cloud_group_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid; + + // std::shared_ptr data_group_ptr; + + // if (!m_file->exist(cloud_group_id)) + // { + // data_group_ptr = std::make_shared(m_file->createGroup(cloud_group_id)); + // data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT, pointcloud2.height()); + // data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH, pointcloud2.width()); + // data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN, pointcloud2.is_bigendian()); + // data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP, pointcloud2.point_step()); + // data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP, pointcloud2.row_step()); + // data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE, pointcloud2.is_dense()); + // } + // else + // { + // data_group_ptr = std::make_shared(m_file->getGroup(cloud_group_id)); + // data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT).write(pointcloud2.height()); + // data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH).write(pointcloud2.width()); + // data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN).write(pointcloud2.is_bigendian()); + // data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP).write(pointcloud2.point_step()); + // data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP).write(pointcloud2.row_step()); + // data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE).write(pointcloud2.is_dense()); + // } + + // writePointFieldAttributes(*data_group_ptr, pointcloud2.fields()); + // writeHeaderAttributes(*data_group_ptr, pointcloud2.header()); + + // writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, pointcloud2.labels_general()); + // writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, pointcloud2.labels_bb()); + + // CloudInfo info = getCloudInfo(pointcloud2); + + // if (info.has_points) + // writePoints(uuid, pointcloud2); + // if (info.has_rgb) + // writeColorsRGB(uuid, pointcloud2); + // if (info.has_rgba) + // writeColorsRGBA(uuid, pointcloud2); + + // // TODO normals + // if (!info.other_fields.empty()) + // writeOtherFields(uuid, pointcloud2, info.other_fields); + + // m_file->flush(); + // return data_group_ptr; +} + +void Hdf5FbPointCloud::writePoints(const std::string& uuid, const seerep::fb::PointCloud2& cloud) +{ + // std::string points_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; + // HighFive::DataSpace data_space({ cloud.height(), cloud.width(), 3 }); + + // std::shared_ptr points_dataset_ptr; + // if (!m_file->exist(points_id)) + // points_dataset_ptr = std::make_shared(m_file->createDataSet(points_id, data_space)); + // else + // points_dataset_ptr = std::make_shared(m_file->getDataSet(points_id)); + + // std::vector>> point_data; + // point_data.resize(cloud.height()); + + // seerep_hdf5_pb::PointCloud2ConstIterator x_iter(cloud, "x"); + // seerep_hdf5_pb::PointCloud2ConstIterator y_iter(cloud, "y"); + // seerep_hdf5_pb::PointCloud2ConstIterator z_iter(cloud, "z"); + + // std::array min, max; + // min[0] = min[1] = min[2] = std::numeric_limits::max(); + // max[0] = max[1] = max[2] = std::numeric_limits::min(); + + // for (unsigned int i = 0; i < cloud.height(); i++) + // { + // point_data[i].reserve(cloud.width()); + // for (unsigned int j = 0; j < cloud.width(); j++) + // { + // const float& x = *x_iter; + // const float& y = *y_iter; + // const float& z = *z_iter; + + // // compute bounding box + // if (x < min[0]) + // min[0] = x; + // if (x > max[0]) + // max[0] = x; + + // if (y < min[1]) + // min[1] = y; + // if (y > max[1]) + // max[1] = y; + + // if (z < min[2]) + // min[2] = z; + // if (z > max[2]) + // max[2] = z; + + // point_data[i].push_back(std::vector{ x, y, z }); + + // ++x_iter, ++y_iter, ++z_iter; + // } + // } + + // // write bounding box as attribute to dataset + // const std::vector boundingbox{ min[0], min[1], min[2], max[0], max[1], max[2] }; + // if (!points_dataset_ptr->hasAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX)) + // points_dataset_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, boundingbox); + // else + // points_dataset_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).write(boundingbox); + + // // write data to dataset + // points_dataset_ptr->write(point_data); +} + +void Hdf5FbPointCloud::writeColorsRGB(const std::string& uuid, const seerep::fb::PointCloud2& cloud) +{ + // const std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + + // "/colors"; HighFive::DataSpace data_space({ cloud.height(), cloud.width(), 3 }); + + // std::shared_ptr colors_dataset_ptr; + // if (!m_file->exist(colors_id)) + // colors_dataset_ptr = std::make_shared(m_file->createDataSet(colors_id, data_space)); + // else + // colors_dataset_ptr = std::make_shared(m_file->getDataSet(colors_id)); + + // std::vector>> colors_data; + // colors_data.resize(cloud.height()); + + // seerep_hdf5_pb::PointCloud2ConstIterator r_iter(cloud, "r"); + // seerep_hdf5_pb::PointCloud2ConstIterator g_iter(cloud, "g"); + // seerep_hdf5_pb::PointCloud2ConstIterator b_iter(cloud, "b"); + + // for (unsigned int i = 0; i < cloud.height(); i++) + // { + // colors_data[i].reserve(cloud.width()); + // for (unsigned int j = 0; j < cloud.width(); j++) + // { + // colors_data[i].push_back(std::vector{ *r_iter, *g_iter, *b_iter }); + // ++r_iter, ++g_iter, ++b_iter; + // } + // } + + // colors_dataset_ptr->write(colors_data); +} + +void Hdf5FbPointCloud::writeColorsRGBA(const std::string& uuid, const seerep::fb::PointCloud2& cloud) +{ + // const std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + + // "/colors"; HighFive::DataSpace data_space({ cloud.height(), cloud.width(), 4 }); + + // std::shared_ptr colors_dataset_ptr; + // if (!m_file->exist(colors_id)) + // colors_dataset_ptr = std::make_shared(m_file->createDataSet(colors_id, data_space)); + // else + // colors_dataset_ptr = std::make_shared(m_file->getDataSet(colors_id)); + + // std::vector>> colors_data; + // colors_data.resize(cloud.height()); + + // seerep_hdf5_pb::PointCloud2ConstIterator r_iter(cloud, "r"); + // seerep_hdf5_pb::PointCloud2ConstIterator g_iter(cloud, "g"); + // seerep_hdf5_pb::PointCloud2ConstIterator b_iter(cloud, "b"); + // seerep_hdf5_pb::PointCloud2ConstIterator a_iter(cloud, "a"); + + // for (unsigned int i = 0; i < cloud.height(); i++) + // { + // colors_data[i].reserve(cloud.width()); + // for (unsigned int j = 0; j < cloud.width(); j++) + // { + // colors_data[i].push_back(std::vector{ *r_iter, *g_iter, *b_iter, *a_iter }); + // ++r_iter; + // ++g_iter; + // ++b_iter; + // ++a_iter; + // } + // } + + // colors_dataset_ptr->write(colors_data); +} + +void Hdf5FbPointCloud::writeOtherFields(const std::string& uuid, const seerep::fb::PointCloud2& cloud, + const std::map& fields) +{ + // for (auto field_map_entry : fields) + // { + // const auto& field = field_map_entry.second; + // switch (field.datatype()) + // { + // case seerep::PointField::INT8: + // write(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::UINT8: + // write(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::INT16: + // write(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::UINT16: + // write(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::INT32: + // write(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::UINT32: + // write(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::FLOAT32: + // write(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::FLOAT64: + // write(uuid, field.name(), cloud, field.count()); + // break; + // default: + // std::cout << "datatype of pointcloud unknown" << std::endl; + // break; + // } + // } +} + +Hdf5FbPointCloud::CloudInfo Hdf5FbPointCloud::getCloudInfo(const seerep::fb::PointCloud2& cloud) +{ + // CloudInfo info; + // for (auto& field : cloud.fields()) + // { + // if (field.name() == "xyz") + // info.has_points = true; + // else if (field.name() == "rgb") + // info.has_rgb = true; + // else if (field.name() == "rgba") + // info.has_rgba = true; + // else if (field.name().find("normal") == 0) + // info.has_normals = true; + // else + // info.other_fields[field.name()] = field; + // } + // return info; +} + +// TODO read partial point cloud, e.g. only xyz without color, etc. +std::optional Hdf5FbPointCloud::readPointCloud2(const std::string& uuid) +{ + // const std::scoped_lock lock(*m_write_mtx); + + // if (!m_file->exist(uuid)) + // { + // return std::nullopt; + // } + // HighFive::Group cloud_group = m_file->getGroup(uuid); + + // seerep::PointCloud2 pointcloud2; + + // *pointcloud2.mutable_header() = readHeaderAttributes(cloud_group); + + // uint32_t height, width, point_step, row_step; + // bool is_bigendian, is_dense; + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT).read(height); + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH).read(width); + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN).read(is_bigendian); + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP).read(point_step); + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP).read(row_step); + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE).read(is_dense); + + // pointcloud2.set_height(height); + // pointcloud2.set_width(width); + // pointcloud2.set_is_bigendian(is_bigendian); + // pointcloud2.set_point_step(point_step); + // pointcloud2.set_row_step(row_step); + // pointcloud2.set_is_dense(is_dense); + + // *pointcloud2.mutable_fields() = readPointFieldAttributes(cloud_group); + + // // TODO build header and Point Fields + + // CloudInfo info = getCloudInfo(pointcloud2); + + // if (info.has_points) + // readPoints(uuid, pointcloud2); + + // if (info.has_rgb) + // readColorsRGB(uuid, pointcloud2); + + // if (info.has_rgba) + // readColorsRGBA(uuid, pointcloud2); + + // // TODO normals + + // if (!info.other_fields.empty()) + // readOtherFields(uuid, pointcloud2, info.other_fields); + + // return pointcloud2; +} + +void Hdf5FbPointCloud::readPoints(const std::string& uuid, seerep::fb::PointCloud2& cloud) +{ + // seerep_hdf5_pb::PointCloud2Iterator x_iter(cloud, "x"); + // seerep_hdf5_pb::PointCloud2Iterator y_iter(cloud, "y"); + // seerep_hdf5_pb::PointCloud2Iterator z_iter(cloud, "z"); + + // std::vector>> point_data; + // std::string points_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; + + // HighFive::DataSet points_dataset = m_file->getDataSet(points_id); + + // points_dataset.read(point_data); + + // for (auto column : point_data) + // { + // for (auto row : column) + // { + // *x_iter = row[0]; + // *y_iter = row[1]; + // *z_iter = row[2]; + // ++x_iter, ++y_iter, ++z_iter; + // } + // } +} + +void Hdf5FbPointCloud::readColorsRGB(const std::string& uuid, seerep::fb::PointCloud2& cloud) +{ + // seerep_hdf5_pb::PointCloud2Iterator r_iter(cloud, "r"); + // seerep_hdf5_pb::PointCloud2Iterator g_iter(cloud, "g"); + // seerep_hdf5_pb::PointCloud2Iterator b_iter(cloud, "b"); + + // std::vector>> color_data; + // std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + + // HighFive::DataSet colors_dataset = m_file->getDataSet(colors_id); + + // colors_dataset.read(color_data); + + // for (auto column : color_data) + // { + // for (auto row : column) + // { + // *r_iter = row[0]; + // *g_iter = row[1]; + // *b_iter = row[2]; + // ++r_iter, ++g_iter, ++b_iter; + // } + // } +} + +void Hdf5FbPointCloud::readColorsRGBA(const std::string& uuid, seerep::fb::PointCloud2& cloud) +{ + // seerep_hdf5_pb::PointCloud2Iterator r_iter(cloud, "r"); + // seerep_hdf5_pb::PointCloud2Iterator g_iter(cloud, "g"); + // seerep_hdf5_pb::PointCloud2Iterator b_iter(cloud, "b"); + // seerep_hdf5_pb::PointCloud2Iterator a_iter(cloud, "a"); + + // std::vector>> color_data; + // std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + + // HighFive::DataSet colors_dataset = m_file->getDataSet(colors_id); + + // colors_dataset.read(color_data); + + // for (auto column : color_data) + // { + // for (auto row : column) + // { + // *r_iter = row[0]; + // *g_iter = row[1]; + // *b_iter = row[2]; + // *a_iter = row[3]; + + // ++r_iter; + // ++g_iter; + // ++b_iter; + // ++a_iter; + // } + // } +} + +void Hdf5FbPointCloud::readOtherFields(const std::string& uuid, seerep::fb::PointCloud2& cloud, + const std::map& fields) +{ + // for (auto field_map_entry : fields) + // { + // const auto& field = field_map_entry.second; + // switch (field.datatype()) + // { + // case seerep::PointField::INT8: + // read(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::UINT8: + // read(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::INT16: + // read(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::UINT16: + // read(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::INT32: + // read(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::UINT32: + // read(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::FLOAT32: + // read(uuid, field.name(), cloud, field.count()); + // break; + // case seerep::PointField::FLOAT64: + // read(uuid, field.name(), cloud, field.count()); + // break; + // default: + // std::cout << "datatype of pointcloud unknown" << std::endl; + // break; + // } + // } +} + +void Hdf5FbPointCloud::writePointFieldAttributes( + HighFive::Group& cloud_group, + const flatbuffers::Vector>& vectorPointField) +{ + // std::vector names; + // std::vector offsets, counts; + // std::vector datatypes; + // for (int i = 0; i < repeatedPointField.size(); i++) + // { + // names.push_back(repeatedPointField.at(i).name()); + // offsets.push_back(repeatedPointField.at(i).offset()); + // datatypes.push_back(static_cast(repeatedPointField.at(i).datatype())); + // counts.push_back(repeatedPointField.at(i).count()); + // } + + // if (!cloud_group.hasAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME)) + // cloud_group.createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME, names); + // else + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME).write(names); + + // if (!cloud_group.hasAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET)) + // cloud_group.createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET, offsets); + // else + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET).write(offsets); + + // if (!cloud_group.hasAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX)) + // cloud_group.createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, datatypes); + // else + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).write(datatypes); + + // if (!cloud_group.hasAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX)) + // cloud_group.createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, counts); + // else + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).write(counts); +} + +flatbuffers::Vector> +Hdf5FbPointCloud::readPointFieldAttributes(HighFive::Group& cloud_group) +{ + // google::protobuf::RepeatedPtrField repeatedPointField; + + // std::vector names; + // std::vector offsets, counts; + // std::vector datatypes; + + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME).read(names); + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET).read(offsets); + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).read(datatypes); + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).read(counts); + + // for (long unsigned int i = 0; i < names.size(); i++) + // { + // seerep::PointField point_field; + + // point_field.set_name(names.at(i)); + // point_field.set_offset(offsets.at(i)); + // point_field.set_datatype(static_cast(datatypes.at(i))); + // point_field.set_count(counts.at(i)); + + // *repeatedPointField.Add() = point_field; + // } + + // return repeatedPointField; +} + +std::vector Hdf5FbPointCloud::loadBoundingBox(const std::string& uuid) +{ + // const std::scoped_lock lock(*m_write_mtx); + + // std::string hdf5DatasetPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid; + // std::shared_ptr group_ptr = + // std::make_shared(m_file->getGroup(hdf5DatasetPath)); std::vector bb; + // group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).write(bb); + // return bb; +} +} // namespace seerep_hdf5_fb From fe72315b35fd3becdbb949d31fbf6a658f2992d1 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 6 Sep 2022 14:48:05 +0000 Subject: [PATCH 08/54] refactor to only use reference where possible in hdf5 fb --- .../include/seerep-hdf5-fb/hdf5-fb-general.h | 27 ++++++- .../seerep-hdf5-fb/src/hdf5-fb-general.cpp | 77 +++++++++---------- .../seerep-hdf5-fb/src/hdf5-fb-image.cpp | 9 +++ .../seerep-hdf5-fb/src/hdf5-fb-point.cpp | 8 +- 4 files changed, 75 insertions(+), 46 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h index 2dcbab0be..0a0974e97 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h @@ -37,7 +37,7 @@ class Hdf5FbGeneral : public seerep_hdf5_core::Hdf5CoreGeneral // Attributes //################ void writeAttributeMap(const std::shared_ptr dataSetPtr, - const flatbuffers::Vector>* attributes); + const flatbuffers::Vector>& attributes); template flatbuffers::Offset>> readAttributeMap(HighFive::AnnotateTraits& object, flatbuffers::grpc::MessageBuilder& builder); @@ -54,17 +54,36 @@ class Hdf5FbGeneral : public seerep_hdf5_core::Hdf5CoreGeneral //################ void writeBoundingBoxLabeled( const std::string& datatypeGroup, const std::string& uuid, - const flatbuffers::Vector>* boundingboxLabeled); + const flatbuffers::Vector>& boundingboxLabeled); void writeBoundingBox2DLabeled( const std::string& datatypeGroup, const std::string& uuid, - const flatbuffers::Vector>* boundingbox2DLabeled); + const flatbuffers::Vector>& boundingbox2DLabeled); //################ // Labels General //################ void writeLabelsGeneral(const std::string& datatypeGroup, const std::string& uuid, - const flatbuffers::Vector>* labelsGeneral); + const flatbuffers::Vector>& labelsGeneral); + + void readLabelsGeneral(const std::string& datatypeGroup, const std::string& uuid, std::vector& labels, + std::vector& instances); + + //################ + // Project + //################ + void writeProjectname(const std::string& projectname); + + std::string readProjectname(); + + void writeProjectFrameId(const std::string& frameId); + + std::string readProjectFrameId(); + +protected: + std::shared_ptr m_file; + std::shared_ptr m_write_mtx; + boost::log::sources::severity_logger m_logger; }; } // namespace seerep_hdf5_fb diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp index 3f88a5834..2207ba69f 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp @@ -11,56 +11,53 @@ Hdf5FbGeneral::Hdf5FbGeneral(std::shared_ptr& file, std::shared_ void Hdf5FbGeneral::writeAttributeMap( const std::shared_ptr dataSetPtr, - const flatbuffers::Vector>* attributes) + const flatbuffers::Vector>& attributes) { - if (attributes) + for (auto attribute : attributes) { - for (auto attribute : *attributes) + if (attribute->value_type() == seerep::fb::Datatypes_Boolean) { - if (attribute->value_type() == seerep::fb::Datatypes_Boolean) - { - writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), - static_cast(attribute->value())->data()); - } - else if (attribute->value_type() == seerep::fb::Datatypes_Integer) - { - writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), - static_cast(attribute->value())->data()); - } - else if (attribute->value_type() == seerep::fb::Datatypes_Double) - { - writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), - static_cast(attribute->value())->data()); - } - else if (attribute->value_type() == seerep::fb::Datatypes_String) - { - writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), - static_cast(attribute->value())->data()->str()); - } - else - { - std::stringstream errorMsg; - errorMsg << "type " << attribute->value_type() << " of attribute " << attribute->key()->c_str() - << " not implemented in hdf5-io."; - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << errorMsg.str(); - throw std::invalid_argument(errorMsg.str()); - } + writeAttribute(dataSetPtr, attribute->key()->str(), + static_cast(attribute->value())->data()); + } + else if (attribute->value_type() == seerep::fb::Datatypes_Integer) + { + writeAttribute(dataSetPtr, attribute->key()->str(), + static_cast(attribute->value())->data()); + } + else if (attribute->value_type() == seerep::fb::Datatypes_Double) + { + writeAttribute(dataSetPtr, attribute->key()->str(), + static_cast(attribute->value())->data()); + } + else if (attribute->value_type() == seerep::fb::Datatypes_String) + { + writeAttribute(dataSetPtr, attribute->key()->str(), + static_cast(attribute->value())->data()->str()); + } + else + { + std::stringstream errorMsg; + errorMsg << "type " << attribute->value_type() << " of attribute " << attribute->key()->c_str() + << " not implemented in hdf5-io."; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << errorMsg.str(); + throw std::invalid_argument(errorMsg.str()); } } } void Hdf5FbGeneral::writeBoundingBoxLabeled( const std::string& datatypeGroup, const std::string& uuid, - const flatbuffers::Vector>* boundingboxLabeled) + const flatbuffers::Vector>& boundingboxLabeled) { - if (boundingboxLabeled && !boundingboxLabeled->size() == 0) + if (!boundingboxLabeled.size() == 0) { std::string id = datatypeGroup + "/" + uuid; std::vector labels; std::vector> boundingBoxes; std::vector instances; - for (auto label : *boundingboxLabeled) + for (auto label : boundingboxLabeled) { labels.push_back(label->labelWithInstance()->label()->str()); std::vector box{ label->bounding_box()->point_min()->x(), label->bounding_box()->point_min()->y(), @@ -89,16 +86,16 @@ void Hdf5FbGeneral::writeBoundingBoxLabeled( void Hdf5FbGeneral::writeBoundingBox2DLabeled( const std::string& datatypeGroup, const std::string& uuid, - const flatbuffers::Vector>* boundingbox2DLabeled) + const flatbuffers::Vector>& boundingbox2DLabeled) { std::string id = datatypeGroup + "/" + uuid; - if (boundingbox2DLabeled && !boundingbox2DLabeled->size() == 0) + if (!boundingbox2DLabeled.size() == 0) { std::vector labels; std::vector> boundingBoxes; std::vector instances; - for (auto label : *boundingbox2DLabeled) + for (auto label : boundingbox2DLabeled) { labels.push_back(label->labelWithInstance()->label()->str()); std::vector box{ label->bounding_box()->point_min()->x(), label->bounding_box()->point_min()->y(), @@ -126,15 +123,15 @@ void Hdf5FbGeneral::writeBoundingBox2DLabeled( void Hdf5FbGeneral::writeLabelsGeneral( const std::string& datatypeGroup, const std::string& uuid, - const flatbuffers::Vector>* labelsGeneral) + const flatbuffers::Vector>& labelsGeneral) { std::string id = datatypeGroup + "/" + uuid; - if (labelsGeneral && !labelsGeneral->size() == 0) + if (!labelsGeneral.size() == 0) { std::vector labels; std::vector instances; - for (auto label : *labelsGeneral) + for (auto label : labelsGeneral) { labels.push_back(label->label()->str()); diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp index fd8e38066..9476ef5c4 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp @@ -75,8 +75,13 @@ void Hdf5FbImage::writeImage(const std::string& id, const seerep::fb::Image& ima data_set_ptr->write(tmp); writeHeaderAttributes(*data_set_ptr, *image.header()); +<<<<<<< HEAD writeBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, image.labels_bb()); writeLabelsGeneral(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, image.labels_general()); +======= + writeBoundingBox2DLabeled(HDF5_GROUP_IMAGE, id, *image.labels_bb()); + writeLabelsGeneral(HDF5_GROUP_IMAGE, id, *image.labels_general()); +>>>>>>> refactor to only use reference where possible in hdf5 fb m_file->flush(); } @@ -86,7 +91,11 @@ void Hdf5FbImage::writeImageBoundingBox2DLabeled(const std::string& id, { const std::scoped_lock lock(*m_write_mtx); +<<<<<<< HEAD writeBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, bb2dLabeledStamped.labels_bb()); +======= + writeBoundingBox2DLabeled(HDF5_GROUP_IMAGE, id, *bb2dLabeledStamped.labels_bb()); +>>>>>>> refactor to only use reference where possible in hdf5 fb } std::optional> Hdf5FbImage::readImage(const std::string& id, diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp index fe1643cc1..385c48ce1 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp @@ -41,9 +41,13 @@ void Hdf5FbPoint::writePoint(const std::string& id, const seerep::fb::PointStamp data_set_ptr->write(data); writeHeaderAttributes(*data_set_ptr, *point->header()); - writeAttributeMap(data_set_ptr, point->attribute()); + writeAttributeMap(data_set_ptr, *point->attribute()); +<<<<<<< HEAD writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePoint::HDF5_GROUP_POINT, id, point->labels_general()); +======= + writeLabelsGeneral(HDF5_GROUP_POINT, id, *point->labels_general()); +>>>>>>> refactor to only use reference where possible in hdf5 fb m_file->flush(); } @@ -60,7 +64,7 @@ void Hdf5FbPoint::writeAdditionalPointAttributes(const seerep::fb::AttributesSta std::shared_ptr data_set_ptr = std::make_shared(m_file->getDataSet(hdf5DatasetRawDataPath)); - writeAttributeMap(data_set_ptr, attributeStamped.attribute()); + writeAttributeMap(data_set_ptr, *attributeStamped.attribute()); } catch (const std::exception& e) { From e037ca50f326e1adfec5ce8e6e7dee8c21818a36 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Thu, 8 Sep 2022 09:38:23 +0000 Subject: [PATCH 09/54] hdf5 fb write for point clouds compiling --- .../include/seerep-hdf5-fb/hdf5-fb-general.h | 19 - .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 19 +- .../seerep-hdf5-fb/src/hdf5-fb-general.cpp | 16 +- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 576 +++++++++--------- 4 files changed, 307 insertions(+), 323 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h index 0a0974e97..6a90c5a18 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h @@ -65,25 +65,6 @@ class Hdf5FbGeneral : public seerep_hdf5_core::Hdf5CoreGeneral //################ void writeLabelsGeneral(const std::string& datatypeGroup, const std::string& uuid, const flatbuffers::Vector>& labelsGeneral); - - void readLabelsGeneral(const std::string& datatypeGroup, const std::string& uuid, std::vector& labels, - std::vector& instances); - - //################ - // Project - //################ - void writeProjectname(const std::string& projectname); - - std::string readProjectname(); - - void writeProjectFrameId(const std::string& frameId); - - std::string readProjectFrameId(); - -protected: - std::shared_ptr m_file; - std::shared_ptr m_write_mtx; - boost::log::sources::severity_logger m_logger; }; } // namespace seerep_hdf5_fb diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index aa2ab9d18..fa3f76090 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -33,13 +33,24 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral std::vector loadBoundingBox(const std::string& uuid); private: + /* + Note: This struct is used to store the necessary information of a point field. + The generated seerep::fb::point struct can't be used since it doesn't have a copy + constructor resp. assignment operator and thus can't be used in maps (see CloudInfo struct). + */ + struct PointFieldInfo + { + uint8_t datatype; + uint32_t count; + }; + struct CloudInfo { bool has_points = false; bool has_rgb = false; bool has_rgba = false; bool has_normals = false; - std::map other_fields; + std::map other_fields; }; template @@ -94,7 +105,7 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral void writeColorsRGBA(const std::string& uuid, const seerep::fb::PointCloud2& cloud); void writeOtherFields(const std::string& uuid, const seerep::fb::PointCloud2& cloud, - const std::map& fields); + const std::map& fields); void writePointFieldAttributes(HighFive::Group& cloud_group, @@ -107,11 +118,11 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral void readColorsRGBA(const std::string& uuid, seerep::fb::PointCloud2& cloud); void readOtherFields(const std::string& uuid, seerep::fb::PointCloud2& cloud, - const std::map& fields); + const std::map& fields); flatbuffers::Vector> readPointFieldAttributes(HighFive::Group& cloud_group); }; } // namespace seerep_hdf5_fb -#endif /* SEEREP_HDF5_PB_HDF5_FB_POINT_CLOUD_H_ */ +#endif /* SEEREP_HDF5_FB_HDF5_FB_POINT_CLOUD_H_ */ diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp index 2207ba69f..2cccd8f3f 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp @@ -17,23 +17,23 @@ void Hdf5FbGeneral::writeAttributeMap( { if (attribute->value_type() == seerep::fb::Datatypes_Boolean) { - writeAttribute(dataSetPtr, attribute->key()->str(), - static_cast(attribute->value())->data()); + writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), + static_cast(attribute->value())->data()); } else if (attribute->value_type() == seerep::fb::Datatypes_Integer) { - writeAttribute(dataSetPtr, attribute->key()->str(), - static_cast(attribute->value())->data()); + writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), + static_cast(attribute->value())->data()); } else if (attribute->value_type() == seerep::fb::Datatypes_Double) { - writeAttribute(dataSetPtr, attribute->key()->str(), - static_cast(attribute->value())->data()); + writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), + static_cast(attribute->value())->data()); } else if (attribute->value_type() == seerep::fb::Datatypes_String) { - writeAttribute(dataSetPtr, attribute->key()->str(), - static_cast(attribute->value())->data()->str()); + writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), + static_cast(attribute->value())->data()->str()); } else { diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 21646aebd..4e3af86fb 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -11,297 +11,325 @@ Hdf5FbPointCloud::Hdf5FbPointCloud(std::shared_ptr& file, std::s std::map Hdf5FbPointCloud::getPointClouds() { - // const std::scoped_lock lock(*m_write_mtx); - - // std::map map; - // if (m_file->exist(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD)) - // { - // const HighFive::Group& clouds_group = m_file->getGroup(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD); - // for (std::string& cloud_name : clouds_group.listObjectNames()) - // { - // map.insert(std::pair(cloud_name, clouds_group.getGroup(cloud_name))); - // } - // } - // return map; + const std::scoped_lock lock(*m_write_mtx); + + std::map map; + if (m_file->exist(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD)) + { + const HighFive::Group& clouds_group = m_file->getGroup(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD); + for (std::string& cloud_name : clouds_group.listObjectNames()) + { + map.insert(std::pair(cloud_name, clouds_group.getGroup(cloud_name))); + } + } + return map; } std::shared_ptr Hdf5FbPointCloud::writePointCloud2(const std::string& uuid, const seerep::fb::PointCloud2& pointcloud2) { - // const std::scoped_lock lock(*m_write_mtx); - - // std::string cloud_group_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid; - - // std::shared_ptr data_group_ptr; - - // if (!m_file->exist(cloud_group_id)) - // { - // data_group_ptr = std::make_shared(m_file->createGroup(cloud_group_id)); - // data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT, pointcloud2.height()); - // data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH, pointcloud2.width()); - // data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN, pointcloud2.is_bigendian()); - // data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP, pointcloud2.point_step()); - // data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP, pointcloud2.row_step()); - // data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE, pointcloud2.is_dense()); - // } - // else - // { - // data_group_ptr = std::make_shared(m_file->getGroup(cloud_group_id)); - // data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT).write(pointcloud2.height()); - // data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH).write(pointcloud2.width()); - // data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN).write(pointcloud2.is_bigendian()); - // data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP).write(pointcloud2.point_step()); - // data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP).write(pointcloud2.row_step()); - // data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE).write(pointcloud2.is_dense()); - // } - - // writePointFieldAttributes(*data_group_ptr, pointcloud2.fields()); - // writeHeaderAttributes(*data_group_ptr, pointcloud2.header()); - - // writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, pointcloud2.labels_general()); - // writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, pointcloud2.labels_bb()); - - // CloudInfo info = getCloudInfo(pointcloud2); - - // if (info.has_points) - // writePoints(uuid, pointcloud2); - // if (info.has_rgb) - // writeColorsRGB(uuid, pointcloud2); - // if (info.has_rgba) - // writeColorsRGBA(uuid, pointcloud2); - - // // TODO normals - // if (!info.other_fields.empty()) - // writeOtherFields(uuid, pointcloud2, info.other_fields); - - // m_file->flush(); - // return data_group_ptr; + const std::scoped_lock lock(*m_write_mtx); + + std::string cloud_group_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid; + + std::shared_ptr data_group_ptr; + + if (!m_file->exist(cloud_group_id)) + { + data_group_ptr = std::make_shared(m_file->createGroup(cloud_group_id)); + data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT, pointcloud2.height()); + data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH, pointcloud2.width()); + data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN, pointcloud2.is_bigendian()); + data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP, pointcloud2.point_step()); + data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP, pointcloud2.row_step()); + data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE, pointcloud2.is_dense()); + } + else + { + data_group_ptr = std::make_shared(m_file->getGroup(cloud_group_id)); + data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT).write(pointcloud2.height()); + data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH).write(pointcloud2.width()); + data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN).write(pointcloud2.is_bigendian()); + data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP).write(pointcloud2.point_step()); + data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP).write(pointcloud2.row_step()); + data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE).write(pointcloud2.is_dense()); + } + + writePointFieldAttributes(*data_group_ptr, *pointcloud2.fields()); + writeHeaderAttributes(*data_group_ptr, *pointcloud2.header()); + + writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, *pointcloud2.labels_general()); + writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, *pointcloud2.labels_bb()); + + CloudInfo info = getCloudInfo(pointcloud2); + + if (info.has_points) + writePoints(uuid, pointcloud2); + if (info.has_rgb) + writeColorsRGB(uuid, pointcloud2); + if (info.has_rgba) + writeColorsRGBA(uuid, pointcloud2); + + // TODO normals + if (!info.other_fields.empty()) + writeOtherFields(uuid, pointcloud2, info.other_fields); + + m_file->flush(); + return data_group_ptr; } void Hdf5FbPointCloud::writePoints(const std::string& uuid, const seerep::fb::PointCloud2& cloud) { - // std::string points_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; - // HighFive::DataSpace data_space({ cloud.height(), cloud.width(), 3 }); - - // std::shared_ptr points_dataset_ptr; - // if (!m_file->exist(points_id)) - // points_dataset_ptr = std::make_shared(m_file->createDataSet(points_id, data_space)); - // else - // points_dataset_ptr = std::make_shared(m_file->getDataSet(points_id)); - - // std::vector>> point_data; - // point_data.resize(cloud.height()); - - // seerep_hdf5_pb::PointCloud2ConstIterator x_iter(cloud, "x"); - // seerep_hdf5_pb::PointCloud2ConstIterator y_iter(cloud, "y"); - // seerep_hdf5_pb::PointCloud2ConstIterator z_iter(cloud, "z"); - - // std::array min, max; - // min[0] = min[1] = min[2] = std::numeric_limits::max(); - // max[0] = max[1] = max[2] = std::numeric_limits::min(); - - // for (unsigned int i = 0; i < cloud.height(); i++) - // { - // point_data[i].reserve(cloud.width()); - // for (unsigned int j = 0; j < cloud.width(); j++) - // { - // const float& x = *x_iter; - // const float& y = *y_iter; - // const float& z = *z_iter; - - // // compute bounding box - // if (x < min[0]) - // min[0] = x; - // if (x > max[0]) - // max[0] = x; - - // if (y < min[1]) - // min[1] = y; - // if (y > max[1]) - // max[1] = y; - - // if (z < min[2]) - // min[2] = z; - // if (z > max[2]) - // max[2] = z; - - // point_data[i].push_back(std::vector{ x, y, z }); - - // ++x_iter, ++y_iter, ++z_iter; - // } - // } - - // // write bounding box as attribute to dataset - // const std::vector boundingbox{ min[0], min[1], min[2], max[0], max[1], max[2] }; - // if (!points_dataset_ptr->hasAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX)) - // points_dataset_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, boundingbox); - // else - // points_dataset_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).write(boundingbox); - - // // write data to dataset - // points_dataset_ptr->write(point_data); + std::string points_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; + HighFive::DataSpace data_space({ cloud.height(), cloud.width(), 3 }); + + std::shared_ptr points_dataset_ptr; + if (!m_file->exist(points_id)) + points_dataset_ptr = std::make_shared(m_file->createDataSet(points_id, data_space)); + else + points_dataset_ptr = std::make_shared(m_file->getDataSet(points_id)); + + std::vector>> point_data; + point_data.resize(cloud.height()); + + seerep_hdf5_fb::PointCloud2ConstIterator x_iter(cloud, "x"); + seerep_hdf5_fb::PointCloud2ConstIterator y_iter(cloud, "y"); + seerep_hdf5_fb::PointCloud2ConstIterator z_iter(cloud, "z"); + + std::array min, max; + min[0] = min[1] = min[2] = std::numeric_limits::max(); + max[0] = max[1] = max[2] = std::numeric_limits::min(); + + for (unsigned int i = 0; i < cloud.height(); i++) + { + point_data[i].reserve(cloud.width()); + for (unsigned int j = 0; j < cloud.width(); j++) + { + const float& x = *x_iter; + const float& y = *y_iter; + const float& z = *z_iter; + + // compute bounding box + if (x < min[0]) + min[0] = x; + if (x > max[0]) + max[0] = x; + + if (y < min[1]) + min[1] = y; + if (y > max[1]) + max[1] = y; + + if (z < min[2]) + min[2] = z; + if (z > max[2]) + max[2] = z; + + point_data[i].push_back(std::vector{ x, y, z }); + + ++x_iter, ++y_iter, ++z_iter; + } + } + + // write bounding box as attribute to dataset + const std::vector boundingbox{ min[0], min[1], min[2], max[0], max[1], max[2] }; + + writeAttributeToHdf5(*points_dataset_ptr, seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, boundingbox); + + // write data to dataset + points_dataset_ptr->write(point_data); } void Hdf5FbPointCloud::writeColorsRGB(const std::string& uuid, const seerep::fb::PointCloud2& cloud) { - // const std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + - // "/colors"; HighFive::DataSpace data_space({ cloud.height(), cloud.width(), 3 }); - - // std::shared_ptr colors_dataset_ptr; - // if (!m_file->exist(colors_id)) - // colors_dataset_ptr = std::make_shared(m_file->createDataSet(colors_id, data_space)); - // else - // colors_dataset_ptr = std::make_shared(m_file->getDataSet(colors_id)); - - // std::vector>> colors_data; - // colors_data.resize(cloud.height()); - - // seerep_hdf5_pb::PointCloud2ConstIterator r_iter(cloud, "r"); - // seerep_hdf5_pb::PointCloud2ConstIterator g_iter(cloud, "g"); - // seerep_hdf5_pb::PointCloud2ConstIterator b_iter(cloud, "b"); - - // for (unsigned int i = 0; i < cloud.height(); i++) - // { - // colors_data[i].reserve(cloud.width()); - // for (unsigned int j = 0; j < cloud.width(); j++) - // { - // colors_data[i].push_back(std::vector{ *r_iter, *g_iter, *b_iter }); - // ++r_iter, ++g_iter, ++b_iter; - // } - // } - - // colors_dataset_ptr->write(colors_data); + const std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + HighFive::DataSpace data_space({ cloud.height(), cloud.width(), 3 }); + + std::shared_ptr colors_dataset_ptr; + if (!m_file->exist(colors_id)) + colors_dataset_ptr = std::make_shared(m_file->createDataSet(colors_id, data_space)); + else + colors_dataset_ptr = std::make_shared(m_file->getDataSet(colors_id)); + + std::vector>> colors_data; + colors_data.resize(cloud.height()); + + seerep_hdf5_fb::PointCloud2ConstIterator r_iter(cloud, "r"); + seerep_hdf5_fb::PointCloud2ConstIterator g_iter(cloud, "g"); + seerep_hdf5_fb::PointCloud2ConstIterator b_iter(cloud, "b"); + + for (unsigned int i = 0; i < cloud.height(); i++) + { + colors_data[i].reserve(cloud.width()); + for (unsigned int j = 0; j < cloud.width(); j++) + { + colors_data[i].push_back(std::vector{ *r_iter, *g_iter, *b_iter }); + ++r_iter, ++g_iter, ++b_iter; + } + } + + colors_dataset_ptr->write(colors_data); } void Hdf5FbPointCloud::writeColorsRGBA(const std::string& uuid, const seerep::fb::PointCloud2& cloud) { - // const std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + - // "/colors"; HighFive::DataSpace data_space({ cloud.height(), cloud.width(), 4 }); - - // std::shared_ptr colors_dataset_ptr; - // if (!m_file->exist(colors_id)) - // colors_dataset_ptr = std::make_shared(m_file->createDataSet(colors_id, data_space)); - // else - // colors_dataset_ptr = std::make_shared(m_file->getDataSet(colors_id)); - - // std::vector>> colors_data; - // colors_data.resize(cloud.height()); - - // seerep_hdf5_pb::PointCloud2ConstIterator r_iter(cloud, "r"); - // seerep_hdf5_pb::PointCloud2ConstIterator g_iter(cloud, "g"); - // seerep_hdf5_pb::PointCloud2ConstIterator b_iter(cloud, "b"); - // seerep_hdf5_pb::PointCloud2ConstIterator a_iter(cloud, "a"); - - // for (unsigned int i = 0; i < cloud.height(); i++) - // { - // colors_data[i].reserve(cloud.width()); - // for (unsigned int j = 0; j < cloud.width(); j++) - // { - // colors_data[i].push_back(std::vector{ *r_iter, *g_iter, *b_iter, *a_iter }); - // ++r_iter; - // ++g_iter; - // ++b_iter; - // ++a_iter; - // } - // } - - // colors_dataset_ptr->write(colors_data); + const std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + HighFive::DataSpace data_space({ cloud.height(), cloud.width(), 4 }); + + std::shared_ptr colors_dataset_ptr; + if (!m_file->exist(colors_id)) + colors_dataset_ptr = std::make_shared(m_file->createDataSet(colors_id, data_space)); + else + colors_dataset_ptr = std::make_shared(m_file->getDataSet(colors_id)); + + std::vector>> colors_data; + colors_data.resize(cloud.height()); + + seerep_hdf5_fb::PointCloud2ConstIterator r_iter(cloud, "r"); + seerep_hdf5_fb::PointCloud2ConstIterator g_iter(cloud, "g"); + seerep_hdf5_fb::PointCloud2ConstIterator b_iter(cloud, "b"); + seerep_hdf5_fb::PointCloud2ConstIterator a_iter(cloud, "a"); + + for (unsigned int i = 0; i < cloud.height(); i++) + { + colors_data[i].reserve(cloud.width()); + for (unsigned int j = 0; j < cloud.width(); j++) + { + colors_data[i].push_back(std::vector{ *r_iter, *g_iter, *b_iter, *a_iter }); + ++r_iter; + ++g_iter; + ++b_iter; + ++a_iter; + } + } + + colors_dataset_ptr->write(colors_data); } void Hdf5FbPointCloud::writeOtherFields(const std::string& uuid, const seerep::fb::PointCloud2& cloud, - const std::map& fields) + const std::map& fields) { - // for (auto field_map_entry : fields) - // { - // const auto& field = field_map_entry.second; - // switch (field.datatype()) - // { - // case seerep::PointField::INT8: - // write(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::UINT8: - // write(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::INT16: - // write(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::UINT16: - // write(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::INT32: - // write(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::UINT32: - // write(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::FLOAT32: - // write(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::FLOAT64: - // write(uuid, field.name(), cloud, field.count()); - // break; - // default: - // std::cout << "datatype of pointcloud unknown" << std::endl; - // break; - // } - // } + for (auto map_entry : fields) + { + const std::string name = map_entry.first; + const PointFieldInfo info = map_entry.second; + switch (info.datatype) + { + case seerep::fb::Point_Field_Datatype_INT8: + write(uuid, name, cloud, info.count); + break; + case seerep::fb::Point_Field_Datatype_UINT8: + write(uuid, name, cloud, info.count); + break; + case seerep::fb::Point_Field_Datatype_INT16: + write(uuid, name, cloud, info.count); + break; + case seerep::fb::Point_Field_Datatype_UINT16: + write(uuid, name, cloud, info.count); + break; + case seerep::fb::Point_Field_Datatype_INT32: + write(uuid, name, cloud, info.count); + break; + case seerep::fb::Point_Field_Datatype_UINT32: + write(uuid, name, cloud, info.count); + break; + case seerep::fb::Point_Field_Datatype_FLOAT32: + write(uuid, name, cloud, info.count); + break; + case seerep::fb::Point_Field_Datatype_FLOAT64: + write(uuid, name, cloud, info.count); + break; + default: + std::cout << "datatype of pointcloud unknown" << std::endl; + break; + } + } +} + +void Hdf5FbPointCloud::writePointFieldAttributes( + HighFive::Group& cloud_group, + const flatbuffers::Vector>& vectorPointField) +{ + std::vector names; + std::vector offsets, counts; + std::vector datatypes; + for (size_t i = 0; i < vectorPointField.size(); i++) + { + names.push_back(vectorPointField.Get(i)->name()->str()); + offsets.push_back(vectorPointField.Get(i)->offset()); + datatypes.push_back(static_cast(vectorPointField.Get(i)->datatype())); + counts.push_back(vectorPointField.Get(i)->count()); + } + writeAttributeToHdf5>(cloud_group, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME, names); + + writeAttributeToHdf5>(cloud_group, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET, offsets); + + writeAttributeToHdf5>(cloud_group, seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, datatypes); + + writeAttributeToHdf5>(cloud_group, seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, counts); } Hdf5FbPointCloud::CloudInfo Hdf5FbPointCloud::getCloudInfo(const seerep::fb::PointCloud2& cloud) { - // CloudInfo info; - // for (auto& field : cloud.fields()) - // { - // if (field.name() == "xyz") - // info.has_points = true; - // else if (field.name() == "rgb") - // info.has_rgb = true; - // else if (field.name() == "rgba") - // info.has_rgba = true; - // else if (field.name().find("normal") == 0) - // info.has_normals = true; - // else - // info.other_fields[field.name()] = field; - // } - // return info; + bool hasFieldx = false; + bool hasFieldy = false; + bool hasFieldz = false; + + CloudInfo info; + for (size_t i = 0; i < cloud.fields()->size(); i++) + { + const std::string fieldName = cloud.fields()->Get(i)->name()->str(); + if (fieldName == "x") + hasFieldx = true; + else if (fieldName == "y") + hasFieldy = true; + else if (fieldName == "z") + hasFieldz = true; + else if (fieldName == "rgb") + info.has_rgb = true; + else if (fieldName == "rgba") + info.has_rgba = true; + else if (fieldName.find("normal") == 0) + info.has_normals = true; + else + { + info.other_fields[fieldName] = (struct PointFieldInfo){ .datatype = cloud.fields()->Get(i)->datatype(), + .count = cloud.fields()->Get(i)->count() }; + } + } + if (hasFieldx && hasFieldy && hasFieldz) + info.has_points = true; + return info; } // TODO read partial point cloud, e.g. only xyz without color, etc. std::optional Hdf5FbPointCloud::readPointCloud2(const std::string& uuid) { - // const std::scoped_lock lock(*m_write_mtx); - - // if (!m_file->exist(uuid)) - // { - // return std::nullopt; - // } - // HighFive::Group cloud_group = m_file->getGroup(uuid); - - // seerep::PointCloud2 pointcloud2; + // const std::scoped_lock lock(*m_write_mtx); - // *pointcloud2.mutable_header() = readHeaderAttributes(cloud_group); + // if (!m_file->exist(uuid)) + // { + // return std::nullopt; + // } + // HighFive::Group cloud_group = m_file->getGroup(uuid); - // uint32_t height, width, point_step, row_step; - // bool is_bigendian, is_dense; - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT).read(height); - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH).read(width); - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN).read(is_bigendian); - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP).read(point_step); - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP).read(row_step); - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE).read(is_dense); + // seerep::fb::PointCloud2 pointcloud2; - // pointcloud2.set_height(height); - // pointcloud2.set_width(width); - // pointcloud2.set_is_bigendian(is_bigendian); - // pointcloud2.set_point_step(point_step); - // pointcloud2.set_row_step(row_step); - // pointcloud2.set_is_dense(is_dense); + // uint32_t height, width, point_step, row_step; + // bool is_bigendian, is_dense; + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT).read(height); + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH).read(width); + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN).read(is_bigendian); + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP).read(point_step); + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP).read(row_step); + // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE).read(is_dense); - // *pointcloud2.mutable_fields() = readPointFieldAttributes(cloud_group); + // auto pointFieldAttributes = readPointFieldAttributes(cloud_group); + // auto headerAttributes = readHeaderAttributes(cloud_group); - // // TODO build header and Point Fields + // // TODO build header and Point Fields - // CloudInfo info = getCloudInfo(pointcloud2); + // CloudInfo info = getCloudInfo(pointcloud2); // if (info.has_points) // readPoints(uuid, pointcloud2); @@ -402,7 +430,7 @@ void Hdf5FbPointCloud::readColorsRGBA(const std::string& uuid, seerep::fb::Point } void Hdf5FbPointCloud::readOtherFields(const std::string& uuid, seerep::fb::PointCloud2& cloud, - const std::map& fields) + const std::map& fields) { // for (auto field_map_entry : fields) // { @@ -440,42 +468,6 @@ void Hdf5FbPointCloud::readOtherFields(const std::string& uuid, seerep::fb::Poin // } } -void Hdf5FbPointCloud::writePointFieldAttributes( - HighFive::Group& cloud_group, - const flatbuffers::Vector>& vectorPointField) -{ - // std::vector names; - // std::vector offsets, counts; - // std::vector datatypes; - // for (int i = 0; i < repeatedPointField.size(); i++) - // { - // names.push_back(repeatedPointField.at(i).name()); - // offsets.push_back(repeatedPointField.at(i).offset()); - // datatypes.push_back(static_cast(repeatedPointField.at(i).datatype())); - // counts.push_back(repeatedPointField.at(i).count()); - // } - - // if (!cloud_group.hasAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME)) - // cloud_group.createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME, names); - // else - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME).write(names); - - // if (!cloud_group.hasAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET)) - // cloud_group.createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET, offsets); - // else - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET).write(offsets); - - // if (!cloud_group.hasAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX)) - // cloud_group.createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, datatypes); - // else - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).write(datatypes); - - // if (!cloud_group.hasAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX)) - // cloud_group.createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, counts); - // else - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).write(counts); -} - flatbuffers::Vector> Hdf5FbPointCloud::readPointFieldAttributes(HighFive::Group& cloud_group) { From 75aa12c7fc8e16d19a5db6b9fdf416c6884b35ff Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Fri, 9 Sep 2022 11:50:31 +0000 Subject: [PATCH 10/54] add fb to core conversions for point clouds --- .../seerep-core-fb/core-fb-conversion.h | 18 ++++++- .../seerep-core-fb/src/core-fb-conversion.cpp | 49 +++++++++++++++++++ 2 files changed, 66 insertions(+), 1 deletion(-) diff --git a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-conversion.h b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-conversion.h index 4f0e61e7d..8e0bb944b 100644 --- a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-conversion.h +++ b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-conversion.h @@ -10,6 +10,7 @@ // seerep-msgs #include #include +#include #include #include #include @@ -67,11 +68,18 @@ class CoreFbConversion static seerep_core_msgs::DatasetIndexable fromFb(const seerep::fb::Image& img); /** * @brief converts the flatbuffer point message to seerep core specific message - * @param img the flatbuffer point message + * @param point the flatbuffer point message * @return the message in seerep core format for the data needed for the indices */ static seerep_core_msgs::DatasetIndexable fromFb(const seerep::fb::PointStamped* point); + /** + * @brief converts the flatbuffer point cloud message to seerep core specific message + * @param cloud the flatbuffer point cloud message + * @return the message in the seerep core format for the data needed for the indices + */ + static seerep_core_msgs::DatasetIndexable fromFb(const seerep::fb::PointCloud2& cloud); + /** * @brief converts the flatbuffer tf query message to seerep core specific message * @param query the flatbuffer tf query message @@ -165,6 +173,14 @@ class CoreFbConversion static void fromFbDataLabelsGeneral(const flatbuffers::Vector>* labelsBB2d, std::vector& labelWithInstance); + /** + * @brief converts the BoundingBoxLabeled with instances of the flatbuffer data message to seerep core specific message + * @param labelsBB the BoundingBoxLabeled with instances in the flatbuffer data message + * @param labelWithInstance the BoundingBoxLabeled with instances in the data message in seerep core format + */ + static void + fromFbDataLabelsGeneral(const flatbuffers::Vector>* labelsBB, + std::vector& labelWithInstance); }; } // namespace seerep_core_fb diff --git a/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp b/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp index 313373546..d0a836934 100644 --- a/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp +++ b/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp @@ -102,6 +102,31 @@ seerep_core_msgs::DatasetIndexable CoreFbConversion::fromFb(const seerep::fb::Po return dataForIndices; } +seerep_core_msgs::DatasetIndexable CoreFbConversion::fromFb(const seerep::fb::PointCloud2& cloud) +{ + seerep_core_msgs::DatasetIndexable dataForIndices; + fromFbDataHeader(cloud.header(), dataForIndices.header); + + // set bounding box for images to 0. assume no spatial extent + dataForIndices.boundingbox.min_corner().set<0>(0); + dataForIndices.boundingbox.min_corner().set<1>(0); + dataForIndices.boundingbox.min_corner().set<2>(0); + dataForIndices.boundingbox.max_corner().set<0>(0); + dataForIndices.boundingbox.max_corner().set<1>(0); + dataForIndices.boundingbox.max_corner().set<2>(0); + + // semantic + int labelSizeBB = 0; + if (cloud.labels_bb()) + { + labelSizeBB = cloud.labels_bb()->size(); + } + dataForIndices.labelsWithInstances.reserve(labelSizeBB); + fromFbDataLabelsGeneral(cloud.labels_bb(), dataForIndices.labelsWithInstances); + + return dataForIndices; +} + seerep_core_msgs::QueryTf CoreFbConversion::fromFb(const seerep::fb::TransformStampedQuery& query) { boost::uuids::string_generator gen; @@ -324,4 +349,28 @@ void CoreFbConversion::fromFbDataLabelsGeneral( } } } + +void CoreFbConversion::fromFbDataLabelsGeneral( + const flatbuffers::Vector>* labelsBB, + std::vector& labelWithInstance) +{ + if (labelsBB) + { + for (auto label : *labelsBB) + { + boost::uuids::string_generator gen; + boost::uuids::uuid uuidInstance; + try + { + uuidInstance = gen(label->labelWithInstance()->instanceUuid()->str()); + } + catch (std::runtime_error const& e) + { + uuidInstance = boost::uuids::nil_uuid(); + } + labelWithInstance.push_back(seerep_core_msgs::LabelWithInstance{ + .label = label->labelWithInstance()->label()->str(), .uuidInstance = uuidInstance }); + } + } +} } // namespace seerep_core_fb From a20c29943a68ad6e2517996f9ddf0d37396d3da8 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Fri, 9 Sep 2022 12:05:16 +0000 Subject: [PATCH 11/54] add service to write fb point clouds --- seerep-srv/seerep-core-fb/CMakeLists.txt | 1 + .../seerep-core-fb/core-fb-pointcloud.h | 50 ++++++++++++++++++ .../seerep-core-fb/src/core-fb-pointcloud.cpp | 52 +++++++++++++++++++ seerep-srv/seerep-server/CMakeLists.txt | 1 + .../seerep-server/fb-point-cloud-service.h | 34 ++++++++++++ .../src/fb-point-cloud-service.cpp | 1 + 6 files changed, 139 insertions(+) create mode 100644 seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h create mode 100644 seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp create mode 100644 seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h create mode 100644 seerep-srv/seerep-server/src/fb-point-cloud-service.cpp diff --git a/seerep-srv/seerep-core-fb/CMakeLists.txt b/seerep-srv/seerep-core-fb/CMakeLists.txt index 94097df84..770eeba4b 100644 --- a/seerep-srv/seerep-core-fb/CMakeLists.txt +++ b/seerep-srv/seerep-core-fb/CMakeLists.txt @@ -45,6 +45,7 @@ add_library(seerepcorefb src/core-fb-instance.cpp src/core-fb-point.cpp src/core-fb-tf.cpp + src/core-fb-pointcloud.cpp ) target_link_libraries(seerepcorefb diff --git a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h new file mode 100644 index 000000000..158adc3cc --- /dev/null +++ b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h @@ -0,0 +1,50 @@ +#ifndef SEEREP_CORE_FB_POINTCLOUD_H_ +#define SEEREP_CORE_FB_POINTCLOUD_H_ + +#include +#include + +#include "seerep-core-fb/core-fb-conversion.h" +#include "seerep-core-fb/core-fb-general.h" + +// seerep-msgs +#include +#include +// seerep-core-msgs +#include +#include +// seerep-hdf5-pb +#include +// seerep-core +#include + +// uuid +#include +#include +#include // uuid class +#include // generators +#include // streaming operators etc. + +namespace seerep_core_fb +{ +class CoreFbPointCloud +{ +public: + CoreFbPointCloud(std::shared_ptr seerepCore); + ~CoreFbPointCloud(); + + std::vector getData(const seerep::fb::Query& query); + boost::uuids::uuid addData(const seerep::fb::PointCloud2& pc); + +private: + std::shared_ptr getHdf5(boost::uuids::uuid project); + std::shared_ptr m_seerepCore; + std::unordered_map, + boost::hash> + m_hdf5IoMap; + boost::log::sources::severity_logger m_logger; +}; + +} // namespace seerep_core_fb + +#endif // SEEREP_CORE_FB_POINTCLOUD_H_ diff --git a/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp new file mode 100644 index 000000000..98c928f24 --- /dev/null +++ b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp @@ -0,0 +1,52 @@ +#include "seerep-core-fb/core-fb-pointcloud.h" + +namespace seerep_core_fb +{ +CoreFbPointCloud::CoreFbPointCloud(std::shared_ptr seerepCore) : m_seerepCore(seerepCore) +{ + CoreFbGeneral::getAllFileAccessorFromCore(m_seerepCore, m_hdf5IoMap); +} + +CoreFbPointCloud::~CoreFbPointCloud() +{ +} + +std::vector CoreFbPointCloud::getData(const seerep::fb::Query& query) +{ + // BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "loading point cloud from pointclouds/"; + // seerep_core_msgs::Query queryCore = CoreFbConversion::fromFb(query, seerep_core_msgs::Datatype::PointCloud); + // seerep_core_msgs::QueryResult resultCore = m_seerepCore->getDataset(queryCore); + + // std::vector resultPointClouds; + // for (auto project : resultCore.queryResultProjects) + // { + // BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) + // << "sending point cloud from project" << boost::lexical_cast(project.projectUuid); + // for (auto uuidPc : project.dataOrInstanceUuids) + // { + // auto img = CoreFbGeneral::getHdf5(project.projectUuid, m_seerepCore, m_hdf5IoMap) + // ->readPointCloud(boost::lexical_cast(uuidImg), queryCore.withoutData); + // if (pc) + // { + // BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) + // << "sending point cloud " << boost::lexical_cast(uuidImg); + // resultPointClouds.push_back(pc.value()); + // } + // } + // } + // return resultPointClouds; +} + +boost::uuids::uuid CoreFbPointCloud::addData(const seerep::fb::PointCloud2& pc) +{ + seerep_core_msgs::DatasetIndexable dataForIndices = CoreFbConversion::fromFb(pc); + + auto hdf5io = CoreFbGeneral::getHdf5(dataForIndices.header.uuidProject, m_seerepCore, m_hdf5IoMap); + hdf5io->writePointCloud2(boost::lexical_cast(dataForIndices.header.uuidData), pc); + + m_seerepCore->addDataset(dataForIndices); + + return dataForIndices.header.uuidData; +} + +} /* namespace seerep_core_fb */ diff --git a/seerep-srv/seerep-server/CMakeLists.txt b/seerep-srv/seerep-server/CMakeLists.txt index 56d8ce634..e79e25136 100644 --- a/seerep-srv/seerep-server/CMakeLists.txt +++ b/seerep-srv/seerep-server/CMakeLists.txt @@ -53,6 +53,7 @@ add_executable(${PROJECT_NAME}_server src/fb-tf-service.cpp src/fb-image-service.cpp src/fb-instance-service.cpp + src/fb-point-cloud-service.cpp ) diff --git a/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h b/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h new file mode 100644 index 000000000..264a49092 --- /dev/null +++ b/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h @@ -0,0 +1,34 @@ +#ifndef SEEREP_SERVER_FB_POINT_CLOUD_SERVICE_H_ +#define SEEREP_SERVER_FB_POINT_CLOUD_SERVICE_H_ + +// seerep +#include +#include +#include + +#include "util.hpp" + +// logging +#include +#include + +namespace seerep_server +{ +class FbPointCloudService final : public seerep::fb::PointCloudService::Service +{ +public: + FbPointCloudService(std::shared_ptr seerepCore); + grpc::Status GetPointCloud2(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request, + grpc::ServerWriter>* writer) override; + grpc::Status TransferPointCloud2(grpc::ServerContext* context, + grpc::ServerReader>* reader, + flatbuffers::grpc::Message* response) override; + void createResponse(std::string msg, seerep::fb::TRANSMISSION_STATE state, + flatbuffers::grpc::Message* response); + + std::shared_ptr imageFb; + boost::log::sources::severity_logger m_logger; +}; +} /* namespace seerep_server */ +#endif // SEEREP_SERVER_FB_POINT_CLOUD_SERVICE_H_ diff --git a/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp b/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp new file mode 100644 index 000000000..9e5293c2f --- /dev/null +++ b/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp @@ -0,0 +1 @@ +#include "seerep-server/fb-point-cloud-service.h" From 2dd979a7980b2b9426dfffe445383ce6ce72e593 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 12 Sep 2022 07:50:53 +0000 Subject: [PATCH 12/54] add flatbuffer point cloud service implementation --- .../hdf5-fb-point-cloud2-iterator.h | 6 +- .../hdf5-pb-point-cloud2-iterator.h | 6 +- .../seerep-server/fb-point-cloud-service.h | 2 +- .../include/seerep-server/server.h | 3 + .../src/fb-point-cloud-service.cpp | 60 +++++++++++++++++++ seerep-srv/seerep-server/src/server.cpp | 2 + 6 files changed, 72 insertions(+), 7 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h index 3398b72ad..f476b3b24 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h @@ -29,8 +29,8 @@ // This file is originally from: // https://github.com/ros/common_msgs/blob/275b09a/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h -#ifndef SENSOR_MSGS__POINT_CLOUD2_ITERATOR_HPP_ -#define SENSOR_MSGS__POINT_CLOUD2_ITERATOR_HPP_ +#ifndef SENSOR_MSGS__POINT_CLOUD2_ITERATOR_FB_HPP_ +#define SENSOR_MSGS__POINT_CLOUD2_ITERATOR_FB_HPP_ #include #include @@ -331,4 +331,4 @@ class PointCloud2ConstIterator #include "impl/hdf5-fb-point-cloud2-iterator.hpp" // NOLINT -#endif // SENSOR_MSGS__POINT_CLOUD2_ITERATOR_HPP_ +#endif // SENSOR_MSGS__POINT_CLOUD2_ITERATOR_FB_HPP_ diff --git a/seerep-hdf5/seerep-hdf5-pb/include/seerep-hdf5-pb/hdf5-pb-point-cloud2-iterator.h b/seerep-hdf5/seerep-hdf5-pb/include/seerep-hdf5-pb/hdf5-pb-point-cloud2-iterator.h index 705cac0e4..67398aa91 100644 --- a/seerep-hdf5/seerep-hdf5-pb/include/seerep-hdf5-pb/hdf5-pb-point-cloud2-iterator.h +++ b/seerep-hdf5/seerep-hdf5-pb/include/seerep-hdf5-pb/hdf5-pb-point-cloud2-iterator.h @@ -29,8 +29,8 @@ // This file is originally from: // https://github.com/ros/common_msgs/blob/275b09a/sensor_msgs/include/sensor_msgs/point_cloud2_iterator.h -#ifndef SENSOR_MSGS__POINT_CLOUD2_ITERATOR_HPP_ -#define SENSOR_MSGS__POINT_CLOUD2_ITERATOR_HPP_ +#ifndef SENSOR_MSGS__POINT_CLOUD2_ITERATOR_PB_HPP_ +#define SENSOR_MSGS__POINT_CLOUD2_ITERATOR_PB_HPP_ #include @@ -329,4 +329,4 @@ class PointCloud2ConstIterator #include "impl/hdf5-pb-point-cloud2-iterator.hpp" // NOLINT -#endif // SENSOR_MSGS__POINT_CLOUD2_ITERATOR_HPP_ +#endif // SENSOR_MSGS__POINT_CLOUD2_ITERATOR_PB_HPP_ diff --git a/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h b/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h index 264a49092..d17b546a3 100644 --- a/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h +++ b/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h @@ -27,7 +27,7 @@ class FbPointCloudService final : public seerep::fb::PointCloudService::Service void createResponse(std::string msg, seerep::fb::TRANSMISSION_STATE state, flatbuffers::grpc::Message* response); - std::shared_ptr imageFb; + std::shared_ptr pointCloudFb; boost::log::sources::severity_logger m_logger; }; } /* namespace seerep_server */ diff --git a/seerep-srv/seerep-server/include/seerep-server/server.h b/seerep-srv/seerep-server/include/seerep-server/server.h index 05207dfa1..4e608f3a8 100644 --- a/seerep-srv/seerep-server/include/seerep-server/server.h +++ b/seerep-srv/seerep-server/include/seerep-server/server.h @@ -18,6 +18,7 @@ #include "seerep-server/fb-image-service.h" #include "seerep-server/fb-instance-service.h" #include "seerep-server/fb-meta-operations.h" +#include "seerep-server/fb-point-cloud-service.h" #include "seerep-server/fb-point-service.h" #include "seerep-server/fb-tf-service.h" #include "seerep-server/pb-image-service.h" @@ -156,6 +157,8 @@ class server std::shared_ptr m_pointServiceFb; /** @brief the flatbuffer service for instances related queries*/ std::shared_ptr m_instanceServiceFb; + /** @brief the flatbuffer service for point clouds */ + // std::shared_ptr m_pointCloudServiceFb; /** @brief the logger object for logging to file and stdout*/ boost::log::sources::severity_logger m_logger; diff --git a/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp b/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp index 9e5293c2f..23d784d82 100644 --- a/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp +++ b/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp @@ -1 +1,61 @@ #include "seerep-server/fb-point-cloud-service.h" + +namespace seerep_server +{ +FbPointCloudService::FbPointCloudService(std::shared_ptr seerepCore) + : pointCloudFb(std::make_shared(seerepCore)) +{ +} + +grpc::Status +FbPointCloudService::GetPointCloud2(grpc::ServerContext* context, + const flatbuffers::grpc::Message* request, + grpc::ServerWriter>* writer) +{ +} + +grpc::Status FbPointCloudService::TransferPointCloud2( + grpc::ServerContext* context, grpc::ServerReader>* reader, + flatbuffers::grpc::Message* response) +{ + (void)context; // ignore + std::string answer = "everything stored!"; + + flatbuffers::grpc::Message pointCloudMsg; + while (reader->Read(&pointCloudMsg)) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "received point cloud ..."; + auto pointCloud = pointCloudMsg.GetRoot(); + + std::string uuidProject = pointCloud->header()->uuid_project()->str(); + if (!uuidProject.empty()) + { + try + { + pointCloudFb->addData(*pointCloud); + } + catch (std::runtime_error const& e) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + + seerep_server_util::createResponseFb(std::string(e.what()), seerep::fb::TRANSMISSION_STATE_FAILURE, response); + + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + } + else + { + answer = "a msg had no project uuid!"; + } + } + seerep_server_util::createResponseFb(answer, seerep::fb::TRANSMISSION_STATE_SUCCESS, response); + + return grpc::Status::OK; +} + +void FbPointCloudService::createResponse(std::string msg, seerep::fb::TRANSMISSION_STATE state, + flatbuffers::grpc::Message* response) +{ +} + +} /* namespace seerep_server */ diff --git a/seerep-srv/seerep-server/src/server.cpp b/seerep-srv/seerep-server/src/server.cpp index eb9fd52f3..4c25b9a17 100644 --- a/seerep-srv/seerep-server/src/server.cpp +++ b/seerep-srv/seerep-server/src/server.cpp @@ -238,6 +238,7 @@ void server::addServicesFb(grpc::ServerBuilder& server_builder) server_builder.RegisterService(&*m_instanceServiceFb); server_builder.RegisterService(&*m_imageServiceFb); server_builder.RegisterService(&*m_pointServiceFb); + // server_builder.RegisterService(&*m_pointCloudServiceFb); } void server::createServicesPb() @@ -255,6 +256,7 @@ void server::createServicesFb() m_instanceServiceFb = std::make_shared(m_seerepCore); m_imageServiceFb = std::make_shared(m_seerepCore); m_pointServiceFb = std::make_shared(m_seerepCore); + // pointCloudServiceFb = std::make_shared(m_seerepCore); } } /* namespace seerep_server */ From 42ced15320dcdebcb5f1b6bbe74846dab063becf Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 13 Sep 2022 11:15:15 +0000 Subject: [PATCH 13/54] WIP first working fb point cloud example script --- .../gRPC/pointcloud/gRPC_fb_pointCloud.py | 108 ++++++++++++++++++ .../include/seerep-server/fb-image-service.h | 3 - .../seerep-server/fb-point-cloud-service.h | 3 +- .../seerep-server/pb-point-cloud-service.h | 6 +- .../include/seerep-server/server.h | 2 +- .../src/fb-point-cloud-service.cpp | 5 - seerep-srv/seerep-server/src/server.cpp | 4 +- 7 files changed, 115 insertions(+), 16 deletions(-) create mode 100755 examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py new file mode 100755 index 000000000..facaa370c --- /dev/null +++ b/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py @@ -0,0 +1,108 @@ +#!/user/bin/env python3 + +import os +import sys +import time + +import flatbuffers +import numpy as np +from fb import Header, PointCloud2, PointField, Timestamp +from fb import point_cloud_service_grpc_fb as pointCloudService + +script_dir = os.path.dirname(__file__) +util_dir = os.path.join(script_dir, '..') +sys.path.append(util_dir) +import util +import util_fb + + +def createTimeStamp(builder, time): + '''Create a flatbuffers time stamp''' + Timestamp.Start(builder) + Timestamp.AddSeconds(builder, time) + Timestamp.AddNanos(builder, 0) + return Timestamp.End(builder) + + +# ToDo add doc for input parameter +def createPointField(builder, name, offset, datatype, count): + '''Creates point field to describe a channel in the point cloud''' + nameStr = builder.CreateString(name) + PointField.Start(builder) + PointField.AddName(builder, nameStr) + PointField.AddOffset(builder, offset) + PointField.AddDatatype(builder, datatype) + PointField.AddCount(builder, count) + return PointField.End(builder) + + +def createPointFields(builder, fields="xyz", datatype=7, count=1): + '''Creates fields for all channels of the point cloud''' + fieldsList = [] + offset = 0 + for field in fields: + fieldsList.append(createPointField(builder, field, offset, datatype, count)) + offset += 4 + return fieldsList + + +def listToPointFieldVector(builder, pointFieldList): + '''Creates flatbuffers vector from a list of point fields''' + PointCloud2.StartFieldsVector(builder, len(pointFieldList)) + for pointField in pointFieldList: + builder.PrependUOffsetTRelative(pointField) + return builder.EndVector() + + +def createHeader(builder, time, frame, projectUuid): + '''Creates a flatbuffers header''' + frameIdStr = builder.CreateString(frame) + projectUuidStr = builder.CreateString(projectUuid) + timeStamp = createTimeStamp(builder, time) + Header.Start(builder) + Header.AddFrameId(builder, frameIdStr) + Header.AddStamp(builder, timeStamp) + Header.AddUuidProject(builder, projectUuidStr) + return Header.End(builder) + + +def createPointCloud(builder, header): + '''Creates the point cloud message''' + pointFields = createPointFields(builder) + pointFieldVector = listToPointFieldVector(builder, pointFields) + + # generate the actual point cloud data + points = np.random.randn(10, 3).astype(np.float32) + pointsVector = builder.CreateByteVector(points.tobytes()) + + PointCloud2.Start(builder) + PointCloud2.AddHeader(builder, header) + PointCloud2.AddHeight(builder, points.shape[0]) + PointCloud2.AddWidth(builder, points.shape[1]) + PointCloud2.AddIsBigendian(builder, False) + PointCloud2.AddPointStep(builder, 8) + PointCloud2.AddRowStep(builder, points.shape[0] * points.shape[1]) + PointCloud2.AddFields(builder, pointFieldVector) + PointCloud2.AddData(builder, pointsVector) + return PointCloud2.End(builder) + + +def createPointClouds(projectUuid, numOf=10): + '''Creates 'numOf' point cloud messages''' + theTime = int(time.time()) + for i in range(numOf): + print(f"Send point cloud: {str(i)}") + builder = flatbuffers.Builder(1024) + + header = createHeader(builder, theTime + i, "camera", projectUuid) + pointCloudMsg = createPointCloud(builder, header) + builder.Finish(pointCloudMsg) + yield bytes(builder.Output()) + + +channel = util.get_gRPC_channel() +stub = pointCloudService.PointCloudServiceStub(channel) + +projectUuid = util_fb.get_or_create_project(channel, "testproject", True) + +responseBuf = stub.TransferPointCloud2(createPointClouds(projectUuid)) diff --git a/seerep-srv/seerep-server/include/seerep-server/fb-image-service.h b/seerep-srv/seerep-server/include/seerep-server/fb-image-service.h index 58f91362d..aec36903c 100644 --- a/seerep-srv/seerep-server/include/seerep-server/fb-image-service.h +++ b/seerep-srv/seerep-server/include/seerep-server/fb-image-service.h @@ -30,9 +30,6 @@ class FbImageService final : public seerep::fb::ImageService::Service flatbuffers::grpc::Message* response) override; private: - void createResponse(std::string msg, seerep::fb::TRANSMISSION_STATE state, - flatbuffers::grpc::Message* response); - std::shared_ptr imageFb; boost::log::sources::severity_logger m_logger; }; diff --git a/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h b/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h index d17b546a3..33dcc724f 100644 --- a/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h +++ b/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h @@ -24,9 +24,8 @@ class FbPointCloudService final : public seerep::fb::PointCloudService::Service grpc::Status TransferPointCloud2(grpc::ServerContext* context, grpc::ServerReader>* reader, flatbuffers::grpc::Message* response) override; - void createResponse(std::string msg, seerep::fb::TRANSMISSION_STATE state, - flatbuffers::grpc::Message* response); +private: std::shared_ptr pointCloudFb; boost::log::sources::severity_logger m_logger; }; diff --git a/seerep-srv/seerep-server/include/seerep-server/pb-point-cloud-service.h b/seerep-srv/seerep-server/include/seerep-server/pb-point-cloud-service.h index 7116f3a82..9e5122c04 100644 --- a/seerep-srv/seerep-server/include/seerep-server/pb-point-cloud-service.h +++ b/seerep-srv/seerep-server/include/seerep-server/pb-point-cloud-service.h @@ -1,5 +1,5 @@ -#ifndef SEEREP_SERVER_POINT_CLOUD_SERVICE_H_ -#define SEEREP_SERVER_POINT_CLOUD_SERVICE_H_ +#ifndef SEEREP_SERVER_PB_POINT_CLOUD_SERVICE_H_ +#define SEEREP_SERVER_PB_POINT_CLOUD_SERVICE_H_ // seerep #include @@ -26,4 +26,4 @@ class PbPointCloudService final : public seerep::PointCloudService::Service }; } /* namespace seerep_server */ -#endif // SEEREP_SERVER_POINT_CLOUD_SERVICE_H_ +#endif // SEEREP_SERVER_PB_POINT_CLOUD_SERVICE_H_ diff --git a/seerep-srv/seerep-server/include/seerep-server/server.h b/seerep-srv/seerep-server/include/seerep-server/server.h index 4e608f3a8..ab2a51897 100644 --- a/seerep-srv/seerep-server/include/seerep-server/server.h +++ b/seerep-srv/seerep-server/include/seerep-server/server.h @@ -158,7 +158,7 @@ class server /** @brief the flatbuffer service for instances related queries*/ std::shared_ptr m_instanceServiceFb; /** @brief the flatbuffer service for point clouds */ - // std::shared_ptr m_pointCloudServiceFb; + std::shared_ptr m_pointCloudServiceFb; /** @brief the logger object for logging to file and stdout*/ boost::log::sources::severity_logger m_logger; diff --git a/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp b/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp index 23d784d82..5cacf9c76 100644 --- a/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp +++ b/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp @@ -53,9 +53,4 @@ grpc::Status FbPointCloudService::TransferPointCloud2( return grpc::Status::OK; } -void FbPointCloudService::createResponse(std::string msg, seerep::fb::TRANSMISSION_STATE state, - flatbuffers::grpc::Message* response) -{ -} - } /* namespace seerep_server */ diff --git a/seerep-srv/seerep-server/src/server.cpp b/seerep-srv/seerep-server/src/server.cpp index 4c25b9a17..83c5e825e 100644 --- a/seerep-srv/seerep-server/src/server.cpp +++ b/seerep-srv/seerep-server/src/server.cpp @@ -238,7 +238,7 @@ void server::addServicesFb(grpc::ServerBuilder& server_builder) server_builder.RegisterService(&*m_instanceServiceFb); server_builder.RegisterService(&*m_imageServiceFb); server_builder.RegisterService(&*m_pointServiceFb); - // server_builder.RegisterService(&*m_pointCloudServiceFb); + server_builder.RegisterService(&*m_pointCloudServiceFb); } void server::createServicesPb() @@ -256,7 +256,7 @@ void server::createServicesFb() m_instanceServiceFb = std::make_shared(m_seerepCore); m_imageServiceFb = std::make_shared(m_seerepCore); m_pointServiceFb = std::make_shared(m_seerepCore); - // pointCloudServiceFb = std::make_shared(m_seerepCore); + m_pointCloudServiceFb = std::make_shared(m_seerepCore); } } /* namespace seerep_server */ From 0fbeb5fa52c9e43e8623856e81f98a563794afde Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 13 Sep 2022 16:36:14 +0000 Subject: [PATCH 14/54] write fb point clouds with general labels --- .../gRPC/pointcloud/gRPC_fb_pointCloud.py | 44 +++++++++++--- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 5 +- .../seerep-hdf5-fb/src/hdf5-fb-general.cpp | 2 +- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 58 ++++++++++--------- .../seerep-core-fb/src/core-fb-conversion.cpp | 1 + .../seerep-core-fb/src/core-fb-pointcloud.cpp | 2 +- 6 files changed, 75 insertions(+), 37 deletions(-) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py index facaa370c..163abe041 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py @@ -6,7 +6,7 @@ import flatbuffers import numpy as np -from fb import Header, PointCloud2, PointField, Timestamp +from fb import Header, LabelWithInstance, PointCloud2, PointField, Timestamp from fb import point_cloud_service_grpc_fb as pointCloudService script_dir = os.path.dirname(__file__) @@ -47,9 +47,9 @@ def createPointFields(builder, fields="xyz", datatype=7, count=1): def listToPointFieldVector(builder, pointFieldList): - '''Creates flatbuffers vector from a list of point fields''' + '''Creates fields vector from a list of point fields''' PointCloud2.StartFieldsVector(builder, len(pointFieldList)) - for pointField in pointFieldList: + for pointField in reversed(pointFieldList): builder.PrependUOffsetTRelative(pointField) return builder.EndVector() @@ -66,13 +66,42 @@ def createHeader(builder, time, frame, projectUuid): return Header.End(builder) -def createPointCloud(builder, header): +def createGeneralLabel(builder, label="testlabel", instanceUuid="testUuid"): + '''Creates a label with an instance uuid''' + labelStr = builder.CreateString(label) + instanceUuidStr = builder.CreateString(instanceUuid) + LabelWithInstance.Start(builder) + LabelWithInstance.AddLabel(builder, labelStr) + LabelWithInstance.AddInstanceUuid(builder, instanceUuidStr) + return LabelWithInstance.End(builder) + + +def createGeneralLabels(builder, numOf=10): + '''Creates 'numOf' general labels''' + labelsGeneral = [] + for i in range(numOf): + labelsGeneral.append(createGeneralLabel(builder, "testlabel" + str(i), "testUuid" + str(i))) + return labelsGeneral + + +def listToGeneralLabelsVector(builder, generalLabelList): + '''Creates labels general vector from a list of labels''' + PointCloud2.StartLabelsGeneralVector(builder, len(generalLabelList)) + for label in reversed(generalLabelList): + builder.PrependUOffsetTRelative(label) + return builder.EndVector() + + +def createPointCloud(builder, header, height=960, width=1280): '''Creates the point cloud message''' pointFields = createPointFields(builder) pointFieldVector = listToPointFieldVector(builder, pointFields) - # generate the actual point cloud data - points = np.random.randn(10, 3).astype(np.float32) + labelsGeneral = createGeneralLabels(builder) + labelsGeneralVector = listToGeneralLabelsVector(builder, labelsGeneral) + + # generate ordered point cloud with dim (height, width, 3) + points = np.random.randn(height, width, 3).astype(np.float32) pointsVector = builder.CreateByteVector(points.tobytes()) PointCloud2.Start(builder) @@ -80,10 +109,11 @@ def createPointCloud(builder, header): PointCloud2.AddHeight(builder, points.shape[0]) PointCloud2.AddWidth(builder, points.shape[1]) PointCloud2.AddIsBigendian(builder, False) - PointCloud2.AddPointStep(builder, 8) + PointCloud2.AddPointStep(builder, 12) PointCloud2.AddRowStep(builder, points.shape[0] * points.shape[1]) PointCloud2.AddFields(builder, pointFieldVector) PointCloud2.AddData(builder, pointsVector) + PointCloud2.AddLabelsGeneral(builder, labelsGeneralVector) return PointCloud2.End(builder) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index fa3f76090..a6a10e937 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -26,7 +26,7 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral std::map getPointClouds(); - std::shared_ptr writePointCloud2(const std::string& uuid, const seerep::fb::PointCloud2& pointcloud2); + std::shared_ptr writePointCloud2(const std::string& uuid, const seerep::fb::PointCloud2* pointcloud2); std::optional readPointCloud2(const std::string& uuid); @@ -98,7 +98,8 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral CloudInfo getCloudInfo(const seerep::fb::PointCloud2& cloud); - void writePoints(const std::string& uuid, const seerep::fb::PointCloud2& cloud); + void writePoints(const std::string& uuid, const std::shared_ptr& data_group_ptr, + const seerep::fb::PointCloud2& cloud); void writeColorsRGB(const std::string& uuid, const seerep::fb::PointCloud2& cloud); diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp index 2cccd8f3f..25e37271b 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp @@ -127,7 +127,7 @@ void Hdf5FbGeneral::writeLabelsGeneral( { std::string id = datatypeGroup + "/" + uuid; - if (!labelsGeneral.size() == 0) + if (labelsGeneral.size() != 0) { std::vector labels; std::vector instances; diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 4e3af86fb..483b1ac44 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -26,7 +26,7 @@ std::map Hdf5FbPointCloud::getPointClouds() } std::shared_ptr Hdf5FbPointCloud::writePointCloud2(const std::string& uuid, - const seerep::fb::PointCloud2& pointcloud2) + const seerep::fb::PointCloud2* pointcloud2) { const std::scoped_lock lock(*m_write_mtx); @@ -37,48 +37,53 @@ std::shared_ptr Hdf5FbPointCloud::writePointCloud2(const std::s if (!m_file->exist(cloud_group_id)) { data_group_ptr = std::make_shared(m_file->createGroup(cloud_group_id)); - data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT, pointcloud2.height()); - data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH, pointcloud2.width()); - data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN, pointcloud2.is_bigendian()); - data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP, pointcloud2.point_step()); - data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP, pointcloud2.row_step()); - data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE, pointcloud2.is_dense()); + data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT, pointcloud2->height()); + data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH, pointcloud2->width()); + data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN, pointcloud2->is_bigendian()); + data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP, pointcloud2->point_step()); + data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP, pointcloud2->row_step()); + data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE, pointcloud2->is_dense()); } else { data_group_ptr = std::make_shared(m_file->getGroup(cloud_group_id)); - data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT).write(pointcloud2.height()); - data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH).write(pointcloud2.width()); - data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN).write(pointcloud2.is_bigendian()); - data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP).write(pointcloud2.point_step()); - data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP).write(pointcloud2.row_step()); - data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE).write(pointcloud2.is_dense()); + data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT).write(pointcloud2->height()); + data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH).write(pointcloud2->width()); + data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN).write(pointcloud2->is_bigendian()); + data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP).write(pointcloud2->point_step()); + data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP).write(pointcloud2->row_step()); + data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE).write(pointcloud2->is_dense()); } - writePointFieldAttributes(*data_group_ptr, *pointcloud2.fields()); - writeHeaderAttributes(*data_group_ptr, *pointcloud2.header()); + writePointFieldAttributes(*data_group_ptr, *pointcloud2->fields()); + writeHeaderAttributes(*data_group_ptr, *pointcloud2->header()); - writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, *pointcloud2.labels_general()); - writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, *pointcloud2.labels_bb()); + if (flatbuffers::IsFieldPresent(pointcloud2, seerep::fb::PointCloud2::VT_LABELS_GENERAL)) + writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, + *pointcloud2->labels_general()); + if (flatbuffers::IsFieldPresent(pointcloud2, seerep::fb::PointCloud2::VT_LABELS_BB)) + writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, + *pointcloud2->labels_bb()); - CloudInfo info = getCloudInfo(pointcloud2); + CloudInfo info = getCloudInfo(*pointcloud2); if (info.has_points) - writePoints(uuid, pointcloud2); + writePoints(uuid, data_group_ptr, *pointcloud2); if (info.has_rgb) - writeColorsRGB(uuid, pointcloud2); + writeColorsRGB(uuid, *pointcloud2); if (info.has_rgba) - writeColorsRGBA(uuid, pointcloud2); + writeColorsRGBA(uuid, *pointcloud2); // TODO normals if (!info.other_fields.empty()) - writeOtherFields(uuid, pointcloud2, info.other_fields); + writeOtherFields(uuid, *pointcloud2, info.other_fields); m_file->flush(); return data_group_ptr; } -void Hdf5FbPointCloud::writePoints(const std::string& uuid, const seerep::fb::PointCloud2& cloud) +void Hdf5FbPointCloud::writePoints(const std::string& uuid, const std::shared_ptr& data_group_ptr, + const seerep::fb::PointCloud2& cloud) { std::string points_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; HighFive::DataSpace data_space({ cloud.height(), cloud.width(), 3 }); @@ -134,7 +139,7 @@ void Hdf5FbPointCloud::writePoints(const std::string& uuid, const seerep::fb::Po // write bounding box as attribute to dataset const std::vector boundingbox{ min[0], min[1], min[2], max[0], max[1], max[2] }; - writeAttributeToHdf5(*points_dataset_ptr, seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, boundingbox); + writeAttributeToHdf5(*data_group_ptr, seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, boundingbox); // write data to dataset points_dataset_ptr->write(point_data); @@ -264,9 +269,10 @@ void Hdf5FbPointCloud::writePointFieldAttributes( writeAttributeToHdf5>(cloud_group, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET, offsets); - writeAttributeToHdf5>(cloud_group, seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, datatypes); + writeAttributeToHdf5>(cloud_group, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_DATATYPE, + datatypes); - writeAttributeToHdf5>(cloud_group, seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, counts); + writeAttributeToHdf5>(cloud_group, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_COUNT, counts); } Hdf5FbPointCloud::CloudInfo Hdf5FbPointCloud::getCloudInfo(const seerep::fb::PointCloud2& cloud) diff --git a/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp b/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp index d0a836934..b1739bece 100644 --- a/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp +++ b/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp @@ -277,6 +277,7 @@ void CoreFbConversion::fromFbDataHeader(const seerep::fb::Header* header, seerep coreHeader.uuidData = boost::uuids::random_generator()(); } + // ToDo change datatype accordingly coreHeader.datatype = seerep_core_msgs::Datatype::Image; coreHeader.frameId = header->frame_id()->str(); coreHeader.timestamp.seconds = header->stamp()->seconds(); diff --git a/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp index 98c928f24..e73539d1c 100644 --- a/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp +++ b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp @@ -42,7 +42,7 @@ boost::uuids::uuid CoreFbPointCloud::addData(const seerep::fb::PointCloud2& pc) seerep_core_msgs::DatasetIndexable dataForIndices = CoreFbConversion::fromFb(pc); auto hdf5io = CoreFbGeneral::getHdf5(dataForIndices.header.uuidProject, m_seerepCore, m_hdf5IoMap); - hdf5io->writePointCloud2(boost::lexical_cast(dataForIndices.header.uuidData), pc); + hdf5io->writePointCloud2(boost::lexical_cast(dataForIndices.header.uuidData), &pc); m_seerepCore->addDataset(dataForIndices); From e8b9b580b09e93fd018eaa20749aeb2396339a93 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Wed, 14 Sep 2022 08:25:25 +0000 Subject: [PATCH 15/54] add boudingBoxLabeled into fb point cloud script --- .../gRPC/pointcloud/gRPC_fb_pointCloud.py | 73 +++++++++++++++++-- 1 file changed, 66 insertions(+), 7 deletions(-) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py index 163abe041..dc0adb2d0 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py @@ -6,7 +6,16 @@ import flatbuffers import numpy as np -from fb import Header, LabelWithInstance, PointCloud2, PointField, Timestamp +from fb import ( + Boundingbox, + BoundingBoxLabeled, + Header, + LabelWithInstance, + Point, + PointCloud2, + PointField, + Timestamp, +) from fb import point_cloud_service_grpc_fb as pointCloudService script_dir = os.path.dirname(__file__) @@ -66,7 +75,7 @@ def createHeader(builder, time, frame, projectUuid): return Header.End(builder) -def createGeneralLabel(builder, label="testlabel", instanceUuid="testUuid"): +def createLabelWithInstance(builder, label="testlabel", instanceUuid="testUuid"): '''Creates a label with an instance uuid''' labelStr = builder.CreateString(label) instanceUuidStr = builder.CreateString(instanceUuid) @@ -80,10 +89,56 @@ def createGeneralLabels(builder, numOf=10): '''Creates 'numOf' general labels''' labelsGeneral = [] for i in range(numOf): - labelsGeneral.append(createGeneralLabel(builder, "testlabel" + str(i), "testUuid" + str(i))) + labelsGeneral.append(createLabelWithInstance(builder, "testlabel" + str(i), "testUuid" + str(i))) return labelsGeneral +def createPoint(builder, x=np.random.rand(), y=np.random.rand(), z=np.random.rand()): + Point.Start(builder) + Point.AddX(builder, x) + Point.AddY(builder, y) + Point.AddZ(builder, z) + return Point.End(builder) + + +def createBoundingBox(builder, time, frame, projectUuid): + header = createHeader(builder, time, frame, projectUuid) + pointMin = createPoint(builder) + pointMax = createPoint(builder) + Boundingbox.Start(builder) + Boundingbox.AddHeader(builder, header) + Boundingbox.AddPointMin(builder, pointMin) + Boundingbox.AddPointMax(builder, pointMax) + return Boundingbox.End(builder) + + +def createBoundingBoxLabeled(builder, labels, instanceUuid, time, frame, projectUuid): + labelWithInstance = createLabelWithInstance(builder, labels, instanceUuid) + boundingBox = createBoundingBox(builder, time, frame, projectUuid) + BoundingBoxLabeled.Start(builder) + BoundingBoxLabeled.AddLabelWithInstance(builder, labelWithInstance) + BoundingBoxLabeled.AddBoundingBox(builder, boundingBox) + return BoundingBoxLabeled.End(builder) + + +def createBoundingBoxesLabeled(builder, projectUuid, numOf=10): + boundingBoxLabeled = [] + for i in range(numOf): + boundingBoxLabeled.append( + createBoundingBoxLabeled( + builder, "testlabel" + str(i), "testUuid" + str(i), int(time.time()), "camera", projectUuid + ) + ) + return boundingBoxLabeled + + +def listToBoudingBoxLabeledVector(builder, boudingBoxLabeledList): + PointCloud2.StartLabelsBbVector(builder, len(boudingBoxLabeledList)) + for bb in reversed(boudingBoxLabeledList): + builder.PrependUOffsetTRelative(bb) + return builder.EndVector() + + def listToGeneralLabelsVector(builder, generalLabelList): '''Creates labels general vector from a list of labels''' PointCloud2.StartLabelsGeneralVector(builder, len(generalLabelList)) @@ -92,7 +147,7 @@ def listToGeneralLabelsVector(builder, generalLabelList): return builder.EndVector() -def createPointCloud(builder, header, height=960, width=1280): +def createPointCloud(builder, header, projectUuid, height=960, width=1280): '''Creates the point cloud message''' pointFields = createPointFields(builder) pointFieldVector = listToPointFieldVector(builder, pointFields) @@ -100,6 +155,9 @@ def createPointCloud(builder, header, height=960, width=1280): labelsGeneral = createGeneralLabels(builder) labelsGeneralVector = listToGeneralLabelsVector(builder, labelsGeneral) + labelsBb = createBoundingBoxesLabeled(builder, projectUuid) + labelsBbVector = listToBoudingBoxLabeledVector(builder, labelsBb) + # generate ordered point cloud with dim (height, width, 3) points = np.random.randn(height, width, 3).astype(np.float32) pointsVector = builder.CreateByteVector(points.tobytes()) @@ -110,10 +168,11 @@ def createPointCloud(builder, header, height=960, width=1280): PointCloud2.AddWidth(builder, points.shape[1]) PointCloud2.AddIsBigendian(builder, False) PointCloud2.AddPointStep(builder, 12) - PointCloud2.AddRowStep(builder, points.shape[0] * points.shape[1]) + PointCloud2.AddRowStep(builder, points.shape[1], 12) PointCloud2.AddFields(builder, pointFieldVector) PointCloud2.AddData(builder, pointsVector) PointCloud2.AddLabelsGeneral(builder, labelsGeneralVector) + PointCloud2.AddLabelsBb(builder, labelsBbVector) return PointCloud2.End(builder) @@ -121,11 +180,11 @@ def createPointClouds(projectUuid, numOf=10): '''Creates 'numOf' point cloud messages''' theTime = int(time.time()) for i in range(numOf): - print(f"Send point cloud: {str(i)}") + print(f"Send point cloud: {str(i+1)}") builder = flatbuffers.Builder(1024) header = createHeader(builder, theTime + i, "camera", projectUuid) - pointCloudMsg = createPointCloud(builder, header) + pointCloudMsg = createPointCloud(builder, header, projectUuid) builder.Finish(pointCloudMsg) yield bytes(builder.Output()) From fb45b1d377d83f7dab8811347dd0a478b6cee5c7 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Wed, 14 Sep 2022 11:57:09 +0000 Subject: [PATCH 16/54] refactor: fb point cloud example to use fb util --- .../gRPC/pointcloud/gRPC_fb_pointCloud.py | 214 +++++------------- examples/python/gRPC/util_fb.py | 149 +++++++++++- 2 files changed, 207 insertions(+), 156 deletions(-) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py index dc0adb2d0..27694ee78 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py @@ -6,185 +6,91 @@ import flatbuffers import numpy as np -from fb import ( - Boundingbox, - BoundingBoxLabeled, - Header, - LabelWithInstance, - Point, - PointCloud2, - PointField, - Timestamp, -) +from fb import PointCloud2 from fb import point_cloud_service_grpc_fb as pointCloudService script_dir = os.path.dirname(__file__) util_dir = os.path.join(script_dir, '..') sys.path.append(util_dir) -import util -import util_fb - - -def createTimeStamp(builder, time): - '''Create a flatbuffers time stamp''' - Timestamp.Start(builder) - Timestamp.AddSeconds(builder, time) - Timestamp.AddNanos(builder, 0) - return Timestamp.End(builder) - - -# ToDo add doc for input parameter -def createPointField(builder, name, offset, datatype, count): - '''Creates point field to describe a channel in the point cloud''' - nameStr = builder.CreateString(name) - PointField.Start(builder) - PointField.AddName(builder, nameStr) - PointField.AddOffset(builder, offset) - PointField.AddDatatype(builder, datatype) - PointField.AddCount(builder, count) - return PointField.End(builder) - - -def createPointFields(builder, fields="xyz", datatype=7, count=1): - '''Creates fields for all channels of the point cloud''' - fieldsList = [] - offset = 0 - for field in fields: - fieldsList.append(createPointField(builder, field, offset, datatype, count)) - offset += 4 - return fieldsList - - -def listToPointFieldVector(builder, pointFieldList): - '''Creates fields vector from a list of point fields''' - PointCloud2.StartFieldsVector(builder, len(pointFieldList)) - for pointField in reversed(pointFieldList): - builder.PrependUOffsetTRelative(pointField) - return builder.EndVector() - - -def createHeader(builder, time, frame, projectUuid): - '''Creates a flatbuffers header''' - frameIdStr = builder.CreateString(frame) - projectUuidStr = builder.CreateString(projectUuid) - timeStamp = createTimeStamp(builder, time) - Header.Start(builder) - Header.AddFrameId(builder, frameIdStr) - Header.AddStamp(builder, timeStamp) - Header.AddUuidProject(builder, projectUuidStr) - return Header.End(builder) - - -def createLabelWithInstance(builder, label="testlabel", instanceUuid="testUuid"): - '''Creates a label with an instance uuid''' - labelStr = builder.CreateString(label) - instanceUuidStr = builder.CreateString(instanceUuid) - LabelWithInstance.Start(builder) - LabelWithInstance.AddLabel(builder, labelStr) - LabelWithInstance.AddInstanceUuid(builder, instanceUuidStr) - return LabelWithInstance.End(builder) - - -def createGeneralLabels(builder, numOf=10): - '''Creates 'numOf' general labels''' - labelsGeneral = [] - for i in range(numOf): - labelsGeneral.append(createLabelWithInstance(builder, "testlabel" + str(i), "testUuid" + str(i))) - return labelsGeneral - - -def createPoint(builder, x=np.random.rand(), y=np.random.rand(), z=np.random.rand()): - Point.Start(builder) - Point.AddX(builder, x) - Point.AddY(builder, y) - Point.AddZ(builder, z) - return Point.End(builder) - - -def createBoundingBox(builder, time, frame, projectUuid): - header = createHeader(builder, time, frame, projectUuid) - pointMin = createPoint(builder) - pointMax = createPoint(builder) - Boundingbox.Start(builder) - Boundingbox.AddHeader(builder, header) - Boundingbox.AddPointMin(builder, pointMin) - Boundingbox.AddPointMax(builder, pointMax) - return Boundingbox.End(builder) - - -def createBoundingBoxLabeled(builder, labels, instanceUuid, time, frame, projectUuid): - labelWithInstance = createLabelWithInstance(builder, labels, instanceUuid) - boundingBox = createBoundingBox(builder, time, frame, projectUuid) - BoundingBoxLabeled.Start(builder) - BoundingBoxLabeled.AddLabelWithInstance(builder, labelWithInstance) - BoundingBoxLabeled.AddBoundingBox(builder, boundingBox) - return BoundingBoxLabeled.End(builder) - - -def createBoundingBoxesLabeled(builder, projectUuid, numOf=10): - boundingBoxLabeled = [] - for i in range(numOf): - boundingBoxLabeled.append( - createBoundingBoxLabeled( - builder, "testlabel" + str(i), "testUuid" + str(i), int(time.time()), "camera", projectUuid - ) - ) - return boundingBoxLabeled - - -def listToBoudingBoxLabeledVector(builder, boudingBoxLabeledList): - PointCloud2.StartLabelsBbVector(builder, len(boudingBoxLabeledList)) - for bb in reversed(boudingBoxLabeledList): - builder.PrependUOffsetTRelative(bb) - return builder.EndVector() - - -def listToGeneralLabelsVector(builder, generalLabelList): - '''Creates labels general vector from a list of labels''' - PointCloud2.StartLabelsGeneralVector(builder, len(generalLabelList)) - for label in reversed(generalLabelList): - builder.PrependUOffsetTRelative(label) - return builder.EndVector() - -def createPointCloud(builder, header, projectUuid, height=960, width=1280): - '''Creates the point cloud message''' - pointFields = createPointFields(builder) - pointFieldVector = listToPointFieldVector(builder, pointFields) - - labelsGeneral = createGeneralLabels(builder) - labelsGeneralVector = listToGeneralLabelsVector(builder, labelsGeneral) - - labelsBb = createBoundingBoxesLabeled(builder, projectUuid) - labelsBbVector = listToBoudingBoxLabeledVector(builder, labelsBb) +import util +from util_fb import ( + addToBoundingBoxLabeledVector, + addToGeneralLabelsVector, + addToPointFieldVector, + createBoundingBoxes, + createBoundingBoxesLabeled, + createHeader, + createLabelsWithInstance, + createPoint, + createPointFields, + createTimeStamp, + getOrCreateProject, +) - # generate ordered point cloud with dim (height, width, 3) +NUM_GENERAL_LABELS = 10 +NUM_BB_LABELS = 10 +NUM_POINT_CLOUDS = 10 + + +def createPointCloud(builder, header, height=960, width=1280): + '''Creates a flatbuffers point cloud message''' + pointFields = createPointFields(builder, "xyz", 7, 4, 1) + pointFieldsVector = addToPointFieldVector(builder, pointFields) + + # create general labels + labelsGeneral = createLabelsWithInstance( + builder, + ["GeneralLabel" + str(i) for i in range(NUM_GENERAL_LABELS)], + ["InstanceUuid" + str(i) for i in range(NUM_GENERAL_LABELS)], + ) + labelsGeneralVector = addToGeneralLabelsVector(builder, labelsGeneral) + + # create bounding box labels + boundingBoxes = createBoundingBoxes( + builder, + header, + [createPoint(builder, np.random.rand(), np.random.rand(), np.random.rand()) for i in range(NUM_BB_LABELS)], + [createPoint(builder, np.random.rand(), np.random.rand(), np.random.rand()) for i in range(NUM_BB_LABELS)], + ) + labelWithInstances = createLabelsWithInstance( + builder, + ["BoundingBoxLabel" + str(i) for i in range(NUM_BB_LABELS)], + ["InstanceUuid" + str(i) for i in range(NUM_BB_LABELS)], + ) + labelsBb = createBoundingBoxesLabeled(builder, labelWithInstances, boundingBoxes) + labelsBbVector = addToBoundingBoxLabeledVector(builder, labelsBb) + + # create ordered point cloud with dim (height, width, 3) points = np.random.randn(height, width, 3).astype(np.float32) pointsVector = builder.CreateByteVector(points.tobytes()) + # add all data into the flatbuffers point cloud message PointCloud2.Start(builder) PointCloud2.AddHeader(builder, header) PointCloud2.AddHeight(builder, points.shape[0]) PointCloud2.AddWidth(builder, points.shape[1]) PointCloud2.AddIsBigendian(builder, False) PointCloud2.AddPointStep(builder, 12) - PointCloud2.AddRowStep(builder, points.shape[1], 12) - PointCloud2.AddFields(builder, pointFieldVector) + PointCloud2.AddRowStep(builder, points.shape[1] * 12) + PointCloud2.AddFields(builder, pointFieldsVector) PointCloud2.AddData(builder, pointsVector) PointCloud2.AddLabelsGeneral(builder, labelsGeneralVector) PointCloud2.AddLabelsBb(builder, labelsBbVector) return PointCloud2.End(builder) -def createPointClouds(projectUuid, numOf=10): - '''Creates 'numOf' point cloud messages''' +def createPointClouds(projectUuid, numOf): + '''Creates numOf pointcloud2 messages as a generator function''' theTime = int(time.time()) for i in range(numOf): print(f"Send point cloud: {str(i+1)}") builder = flatbuffers.Builder(1024) - header = createHeader(builder, theTime + i, "camera", projectUuid) - pointCloudMsg = createPointCloud(builder, header, projectUuid) + timeStamp = createTimeStamp(builder, theTime + i) + header = createHeader(builder, timeStamp, "camera", projectUuid) + + pointCloudMsg = createPointCloud(builder, header) builder.Finish(pointCloudMsg) yield bytes(builder.Output()) @@ -192,6 +98,6 @@ def createPointClouds(projectUuid, numOf=10): channel = util.get_gRPC_channel() stub = pointCloudService.PointCloudServiceStub(channel) -projectUuid = util_fb.get_or_create_project(channel, "testproject", True) +projectUuid = getOrCreateProject(channel, "testproject", True) -responseBuf = stub.TransferPointCloud2(createPointClouds(projectUuid)) +responseBuf = stub.TransferPointCloud2(createPointClouds(projectUuid, NUM_POINT_CLOUDS)) diff --git a/examples/python/gRPC/util_fb.py b/examples/python/gRPC/util_fb.py index a6360ec61..bb8e910cf 100644 --- a/examples/python/gRPC/util_fb.py +++ b/examples/python/gRPC/util_fb.py @@ -1,11 +1,25 @@ import sys +import time import flatbuffers -from fb import Empty, ProjectCreation, ProjectInfo, ProjectInfos +from fb import ( + Boundingbox, + BoundingBoxLabeled, + Empty, + Header, + LabelWithInstance, + Point, + PointCloud2, + PointField, + ProjectCreation, + ProjectInfo, + ProjectInfos, + Timestamp, +) from fb import meta_operations_grpc_fb as metaOperations -def get_or_create_project(channel, name, create=True, mapFrameId="map"): +def getOrCreateProject(channel, name, create=True, mapFrameId="map"): stubMeta = metaOperations.MetaOperationsStub(channel) builder = flatbuffers.Builder(1024) @@ -42,3 +56,134 @@ def get_or_create_project(channel, name, create=True, mapFrameId="map"): sys.exit() return projectuuid + + +# Header +def createTimeStamp(builder, seconds, nanoseconds=0): + '''Create a time stamp in flatbuffers''' + Timestamp.Start(builder) + Timestamp.AddSeconds(builder, seconds) + Timestamp.AddNanos(builder, nanoseconds) + return Timestamp.End(builder) + + +def createHeader(builder, timeStamp, frame, projectUuid): + '''Creates a message header in flatbuffers''' + frameStr = builder.CreateString(frame) + projectUuidStr = builder.CreateString(projectUuid) + Header.Start(builder) + Header.AddFrameId(builder, frameStr) + Header.AddStamp(builder, timeStamp) + Header.AddUuidProject(builder, projectUuidStr) + return Header.End(builder) + + +# Point clouds +def createPointField(builder, name, offset, datatype, count): + '''Creates a point field in flatbuffers to describe a channel in the point cloud''' + nameStr = builder.CreateString(name) + PointField.Start(builder) + PointField.AddName(builder, nameStr) + PointField.AddOffset(builder, offset) + PointField.AddDatatype(builder, datatype) + PointField.AddCount(builder, count) + return PointField.End(builder) + + +def createPointFields(builder, channels, datatype, dataTypeOffset, count): + '''Creates point fields for all specified channels''' + pointFieldsList = [] + offset = 0 + for channel in channels: + pointFieldsList.append(createPointField(builder, channel, offset, datatype, count)) + offset += dataTypeOffset + return pointFieldsList + + +def createLabelWithInstance(builder, label, instanceUuid): + '''Creates a label with an associated instance uuid in flatbuffers''' + labelStr = builder.CreateString(label) + instanceUuidStr = builder.CreateString(instanceUuid) + LabelWithInstance.Start(builder) + LabelWithInstance.AddLabel(builder, labelStr) + LabelWithInstance.AddInstanceUuid(builder, instanceUuidStr) + return LabelWithInstance.End(builder) + + +def createLabelsWithInstance(builder, labels, instanceUuids): + '''Creates multiple general labels''' + assert len(labels) == len(instanceUuids) + labelsGeneral = [] + for label, uuid in zip(labels, instanceUuids): + labelsGeneral.append(createLabelWithInstance(builder, label, uuid)) + return labelsGeneral + + +def createPoint(builder, x, y, z): + '''Creates a 3D point in flatbuffers''' + Point.Start(builder) + Point.AddX(builder, x) + Point.AddY(builder, y) + Point.AddZ(builder, z) + return Point.End(builder) + + +def createBoundingBox(builder, header, pointMin, pointMax): + '''Creates a 3D bounding box in flatbuffers''' + Boundingbox.Start(builder) + Boundingbox.AddHeader(builder, header) + Boundingbox.AddPointMin(builder, pointMin) + Boundingbox.AddPointMax(builder, pointMax) + return Boundingbox.End(builder) + + +def createBoundingBoxes(builder, header, minPoints, maxPoints): + assert len(minPoints) == len(maxPoints) + boundingBoxes = [] + for pointMin, pointMax in zip(minPoints, maxPoints): + boundingBoxes.append(createBoundingBox(builder, header, pointMin, pointMax)) + return boundingBoxes + + +def createBoundingBoxLabeled(builder, instance, boundingBox): + '''Creates a labeled bounding box in flatbuffers''' + BoundingBoxLabeled.Start(builder) + BoundingBoxLabeled.AddLabelWithInstance(builder, instance) + BoundingBoxLabeled.AddBoundingBox(builder, boundingBox) + return BoundingBoxLabeled.End(builder) + + +def createBoundingBoxesLabeled(builder, instances, boundingBoxes): + '''Creates multiple labeled bounding boxes''' + assert len(instances) == len(boundingBoxes) + boundingBoxesLabeled = [] + for instance, boundingBox in zip(instances, boundingBoxes): + boundingBoxesLabeled.append(createBoundingBoxLabeled(builder, instance, boundingBox)) + return boundingBoxesLabeled + + +def addToBoundingBoxLabeledVector(builder, boundingBoxLabeledList): + '''Adds list of boudingBoxLabeled into the labelsBbVector of a flatbuffers pointcloud2''' + PointCloud2.StartLabelsBbVector(builder, len(boundingBoxLabeledList)) + # Note: reverse because we prepend + for bb in reversed(boundingBoxLabeledList): + builder.PrependUOffsetTRelative(bb) + return builder.EndVector() + + +def addToGeneralLabelsVector(builder, generalLabelList): + '''Adds list of generalLabels into the labelsGeneralVector of a flatbuffers pointcloud2''' + PointCloud2.StartLabelsGeneralVector(builder, len(generalLabelList)) + # Note: reverse because we prepend + for label in reversed(generalLabelList): + builder.PrependUOffsetTRelative(label) + return builder.EndVector() + + +def addToPointFieldVector(builder, pointFieldList): + '''Adds a list of pointFields into the fieldsVector of a flatbuffers pointcloud2''' + PointCloud2.StartFieldsVector(builder, len(pointFieldList)) + # Note: reverse because we prepend + for pointField in reversed(pointFieldList): + builder.PrependUOffsetTRelative(pointField) + return builder.EndVector() From ba9ddec419445867573cb9901f9edbf20584bfb8 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Thu, 15 Sep 2022 11:44:49 +0000 Subject: [PATCH 17/54] hdf5 fb point cloud read compiling --- .../seerep-hdf5-core/hdf5-core-general.h | 1 + .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 30 +- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 383 ++++++++++-------- 3 files changed, 247 insertions(+), 167 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-general.h b/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-general.h index f323f3220..a747c6b37 100644 --- a/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-general.h +++ b/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-general.h @@ -112,6 +112,7 @@ class Hdf5CoreGeneral inline static const std::string LABELBB = "labelBB"; inline static const std::string LABELBBBOXES = "labelBBBoxes"; inline static const std::string LABELBBINSTANCES = "labelBBInstances"; + inline static const std::string POINTS = "points"; protected: std::shared_ptr m_file; diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index a6a10e937..29c532504 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -28,7 +28,7 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral std::shared_ptr writePointCloud2(const std::string& uuid, const seerep::fb::PointCloud2* pointcloud2); - std::optional readPointCloud2(const std::string& uuid); + std::optional> readPointCloud2(const std::string& uuid); std::vector loadBoundingBox(const std::string& uuid); @@ -96,6 +96,12 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral dataset_ptr->write(data); } + template + bool sameVectorSize(T const& first, U const&... rest) + { + return ((first.size() == rest.size()) && ...); + } + CloudInfo getCloudInfo(const seerep::fb::PointCloud2& cloud); void writePoints(const std::string& uuid, const std::shared_ptr& data_group_ptr, @@ -121,8 +127,26 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral void readOtherFields(const std::string& uuid, seerep::fb::PointCloud2& cloud, const std::map& fields); - flatbuffers::Vector> - readPointFieldAttributes(HighFive::Group& cloud_group); + std::optional>>> + readPointFieldAttributes(const std::string& id, std::shared_ptr& data_set_ptr); + + // pointcloud attribute keys + inline static const std::string HEIGHT = "height"; + inline static const std::string WIDTH = "width"; + inline static const std::string ENCODING = "encoding"; + inline static const std::string IS_BIGENDIAN = "is_bigendian"; + inline static const std::string ROW_STEP = "row_step"; + inline static const std::string POINT_STEP = "point_step"; + inline static const std::string IS_DENSE = "is_dense"; + + // point field attribute keys + inline static const std::string FIELD_NAME = "name"; + inline static const std::string FIELD_OFFSET = "offset"; + inline static const std::string FIELD_DATATYPE = "datatype"; + inline static const std::string FIELD_COUNT = "counts"; + +public: + inline static const std::string HDF5_GROUP_POINTCLOUD = "pointclouds"; }; } // namespace seerep_hdf5_fb diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 483b1ac44..b2e4988a4 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -309,208 +309,263 @@ Hdf5FbPointCloud::CloudInfo Hdf5FbPointCloud::getCloudInfo(const seerep::fb::Poi } // TODO read partial point cloud, e.g. only xyz without color, etc. -std::optional Hdf5FbPointCloud::readPointCloud2(const std::string& uuid) +std::optional> +Hdf5FbPointCloud::readPointCloud2(const std::string& id) { - // const std::scoped_lock lock(*m_write_mtx); + const std::scoped_lock lock(*m_write_mtx); + + const std::string hdf5DatasetPath = HDF5_GROUP_POINTCLOUD + "/" + id; + + if (!m_file->exist(hdf5DatasetPath)) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << hdf5DatasetPath << "does not exist"; + return std::nullopt; + } + + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "loading " << hdf5DatasetPath; + + std::shared_ptr data_set_ptr = + std::make_shared(m_file->getDataSet(hdf5DatasetPath)); + + flatbuffers::grpc::MessageBuilder builder; + + HighFive::Group cloud_group = m_file->getGroup(id); + + uint32_t height, width, point_step, row_step; + bool is_bigendian, is_dense; + + try + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "loading attributes"; + height = readAttributeFromHdf5(id, *data_set_ptr, HEIGHT); + width = readAttributeFromHdf5(id, *data_set_ptr, WIDTH); + point_step = readAttributeFromHdf5(id, *data_set_ptr, POINT_STEP); + row_step = readAttributeFromHdf5(id, *data_set_ptr, ROW_STEP); + is_bigendian = readAttributeFromHdf5(id, *data_set_ptr, IS_BIGENDIAN); + is_dense = readAttributeFromHdf5(id, *data_set_ptr, IS_DENSE); + } + catch (const std::invalid_argument& e) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << "error: " << e.what(); + return std::nullopt; + } - // if (!m_file->exist(uuid)) - // { - // return std::nullopt; - // } - // HighFive::Group cloud_group = m_file->getGroup(uuid); + auto headerAttributes = readHeaderAttributes(cloud_group, id, builder); + auto pointFields = readPointFieldAttributes(id, data_set_ptr); - // seerep::fb::PointCloud2 pointcloud2; + if (!pointFields.has_value()) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << "could not read point fields"; + return std::nullopt; + } - // uint32_t height, width, point_step, row_step; - // bool is_bigendian, is_dense; - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT).read(height); - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH).read(width); - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN).read(is_bigendian); - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP).read(point_step); - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP).read(row_step); - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE).read(is_dense); + seerep::fb::PointCloud2Builder pointCloudBuilder(builder); + pointCloudBuilder.add_header(headerAttributes); + pointCloudBuilder.add_height(height); + pointCloudBuilder.add_width(width); + pointCloudBuilder.add_point_step(point_step); + pointCloudBuilder.add_row_step(row_step); + pointCloudBuilder.add_is_bigendian(is_bigendian); + pointCloudBuilder.add_is_dense(is_dense); + pointCloudBuilder.add_fields(pointFields.value()); + auto pointCloud = pointCloudBuilder.Finish(); - // auto pointFieldAttributes = readPointFieldAttributes(cloud_group); - // auto headerAttributes = readHeaderAttributes(cloud_group); + auto data_ptr = builder.GetBufferPointer(); - // // TODO build header and Point Fields + auto pointCloud2 = reinterpret_cast(builder.GetBufferPointer()); - // CloudInfo info = getCloudInfo(pointcloud2); + CloudInfo info = getCloudInfo(*pointCloud2); - // if (info.has_points) - // readPoints(uuid, pointcloud2); + if (info.has_points) + readPoints(id, *pointCloud2); - // if (info.has_rgb) - // readColorsRGB(uuid, pointcloud2); + if (info.has_rgb) + readColorsRGB(id, *pointCloud2); - // if (info.has_rgba) - // readColorsRGBA(uuid, pointcloud2); + if (info.has_rgba) + readColorsRGBA(id, *pointCloud2); - // // TODO normals + // // TODO normals - // if (!info.other_fields.empty()) - // readOtherFields(uuid, pointcloud2, info.other_fields); + if (!info.other_fields.empty()) + readOtherFields(id, *pointCloud2, info.other_fields); - // return pointcloud2; + // return pointCloud2; } void Hdf5FbPointCloud::readPoints(const std::string& uuid, seerep::fb::PointCloud2& cloud) { - // seerep_hdf5_pb::PointCloud2Iterator x_iter(cloud, "x"); - // seerep_hdf5_pb::PointCloud2Iterator y_iter(cloud, "y"); - // seerep_hdf5_pb::PointCloud2Iterator z_iter(cloud, "z"); - - // std::vector>> point_data; - // std::string points_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; - - // HighFive::DataSet points_dataset = m_file->getDataSet(points_id); - - // points_dataset.read(point_data); - - // for (auto column : point_data) - // { - // for (auto row : column) - // { - // *x_iter = row[0]; - // *y_iter = row[1]; - // *z_iter = row[2]; - // ++x_iter, ++y_iter, ++z_iter; - // } - // } + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "reading points"; + + seerep_hdf5_fb::PointCloud2Iterator x_iter(cloud, "x"); + seerep_hdf5_fb::PointCloud2Iterator y_iter(cloud, "y"); + seerep_hdf5_fb::PointCloud2Iterator z_iter(cloud, "z"); + + std::vector>> point_data; + std::string hdf5DatasetPointsPath = + seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; + + HighFive::DataSet points_dataset = m_file->getDataSet(hdf5DatasetPointsPath); + + points_dataset.read(point_data); + + for (auto column : point_data) + { + for (auto row : column) + { + *x_iter = row[0]; + *y_iter = row[1]; + *z_iter = row[2]; + ++x_iter, ++y_iter, ++z_iter; + } + } } void Hdf5FbPointCloud::readColorsRGB(const std::string& uuid, seerep::fb::PointCloud2& cloud) { - // seerep_hdf5_pb::PointCloud2Iterator r_iter(cloud, "r"); - // seerep_hdf5_pb::PointCloud2Iterator g_iter(cloud, "g"); - // seerep_hdf5_pb::PointCloud2Iterator b_iter(cloud, "b"); - - // std::vector>> color_data; - // std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; - - // HighFive::DataSet colors_dataset = m_file->getDataSet(colors_id); - - // colors_dataset.read(color_data); - - // for (auto column : color_data) - // { - // for (auto row : column) - // { - // *r_iter = row[0]; - // *g_iter = row[1]; - // *b_iter = row[2]; - // ++r_iter, ++g_iter, ++b_iter; - // } - // } + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "reading rgb "; + seerep_hdf5_fb::PointCloud2Iterator r_iter(cloud, "r"); + seerep_hdf5_fb::PointCloud2Iterator g_iter(cloud, "g"); + seerep_hdf5_fb::PointCloud2Iterator b_iter(cloud, "b"); + + std::vector>> color_data; + std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + + HighFive::DataSet colors_dataset = m_file->getDataSet(colors_id); + + colors_dataset.read(color_data); + + for (auto column : color_data) + { + for (auto row : column) + { + *r_iter = row[0]; + *g_iter = row[1]; + *b_iter = row[2]; + ++r_iter, ++g_iter, ++b_iter; + } + } } void Hdf5FbPointCloud::readColorsRGBA(const std::string& uuid, seerep::fb::PointCloud2& cloud) { - // seerep_hdf5_pb::PointCloud2Iterator r_iter(cloud, "r"); - // seerep_hdf5_pb::PointCloud2Iterator g_iter(cloud, "g"); - // seerep_hdf5_pb::PointCloud2Iterator b_iter(cloud, "b"); - // seerep_hdf5_pb::PointCloud2Iterator a_iter(cloud, "a"); - - // std::vector>> color_data; - // std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; - - // HighFive::DataSet colors_dataset = m_file->getDataSet(colors_id); - - // colors_dataset.read(color_data); - - // for (auto column : color_data) - // { - // for (auto row : column) - // { - // *r_iter = row[0]; - // *g_iter = row[1]; - // *b_iter = row[2]; - // *a_iter = row[3]; - - // ++r_iter; - // ++g_iter; - // ++b_iter; - // ++a_iter; - // } - // } + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "reading rgb "; + + seerep_hdf5_fb::PointCloud2Iterator r_iter(cloud, "r"); + seerep_hdf5_fb::PointCloud2Iterator g_iter(cloud, "g"); + seerep_hdf5_fb::PointCloud2Iterator b_iter(cloud, "b"); + seerep_hdf5_fb::PointCloud2Iterator a_iter(cloud, "a"); + + std::vector>> color_data; + std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + + HighFive::DataSet colors_dataset = m_file->getDataSet(colors_id); + + colors_dataset.read(color_data); + + for (auto column : color_data) + { + for (auto row : column) + { + *r_iter = row[0]; + *g_iter = row[1]; + *b_iter = row[2]; + *a_iter = row[3]; + + ++r_iter; + ++g_iter; + ++b_iter; + ++a_iter; + } + } } void Hdf5FbPointCloud::readOtherFields(const std::string& uuid, seerep::fb::PointCloud2& cloud, const std::map& fields) { - // for (auto field_map_entry : fields) - // { - // const auto& field = field_map_entry.second; - // switch (field.datatype()) - // { - // case seerep::PointField::INT8: - // read(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::UINT8: - // read(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::INT16: - // read(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::UINT16: - // read(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::INT32: - // read(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::UINT32: - // read(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::FLOAT32: - // read(uuid, field.name(), cloud, field.count()); - // break; - // case seerep::PointField::FLOAT64: - // read(uuid, field.name(), cloud, field.count()); - // break; - // default: - // std::cout << "datatype of pointcloud unknown" << std::endl; - // break; - // } - // } + for (auto field_map_entry : fields) + { + const std::string& name = field_map_entry.first; + const PointFieldInfo pointFieldInfo = field_map_entry.second; + switch (pointFieldInfo.datatype) + { + case seerep::fb::Point_Field_Datatype_INT8: + read(uuid, name, cloud, pointFieldInfo.count); + break; + case seerep::fb::Point_Field_Datatype_UINT8: + read(uuid, name, cloud, pointFieldInfo.count); + break; + case seerep::fb::Point_Field_Datatype_INT16: + read(uuid, name, cloud, pointFieldInfo.count); + break; + case seerep::fb::Point_Field_Datatype_UINT16: + read(uuid, name, cloud, pointFieldInfo.count); + break; + case seerep::fb::Point_Field_Datatype_INT32: + read(uuid, name, cloud, pointFieldInfo.count); + break; + case seerep::fb::Point_Field_Datatype_UINT32: + read(uuid, name, cloud, pointFieldInfo.count); + break; + case seerep::fb::Point_Field_Datatype_FLOAT32: + read(uuid, name, cloud, pointFieldInfo.count); + break; + case seerep::fb::Point_Field_Datatype_FLOAT64: + read(uuid, name, cloud, pointFieldInfo.count); + break; + default: + std::cout << "datatype of pointcloud unknown" << std::endl; + break; + } + } } -flatbuffers::Vector> -Hdf5FbPointCloud::readPointFieldAttributes(HighFive::Group& cloud_group) +std::optional>>> +Hdf5FbPointCloud::readPointFieldAttributes(const std::string& id, std::shared_ptr& data_set_ptr) { - // google::protobuf::RepeatedPtrField repeatedPointField; - - // std::vector names; - // std::vector offsets, counts; - // std::vector datatypes; + std::vector names; + std::vector offsets, counts; + std::vector datatypes; - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME).read(names); - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET).read(offsets); - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).read(datatypes); - // cloud_group.getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).read(counts); + // try to read the point fields attributes from the hdf5 file + try + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "loading point field attributes"; + names = readAttributeFromHdf5>(id, *data_set_ptr, FIELD_NAME); + offsets = readAttributeFromHdf5>(id, *data_set_ptr, FIELD_OFFSET); + counts = readAttributeFromHdf5>(id, *data_set_ptr, FIELD_COUNT); + datatypes = readAttributeFromHdf5>(id, *data_set_ptr, FIELD_DATATYPE); + } + catch (const std::invalid_argument& e) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << "error: " << e.what(); + return std::nullopt; + } - // for (long unsigned int i = 0; i < names.size(); i++) - // { - // seerep::PointField point_field; + // verify that the same number of point fields were read + if (!sameVectorSize(names, offsets, counts, datatypes)) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << "error: " + << "length of point fields differ"; + return std::nullopt; + } - // point_field.set_name(names.at(i)); - // point_field.set_offset(offsets.at(i)); - // point_field.set_datatype(static_cast(datatypes.at(i))); - // point_field.set_count(counts.at(i)); + std::vector> pointFields; + pointFields.reserve(names.size()); - // *repeatedPointField.Add() = point_field; - // } + flatbuffers::grpc::MessageBuilder builder; - // return repeatedPointField; + for (long unsigned int i = 0; i < names.size(); i++) + { + auto nameStr = builder.CreateString(names.at(i)); + seerep::fb::PointFieldBuilder pointField(builder); + pointField.add_name(nameStr); + pointField.add_offset(offsets.at(i)); + pointField.add_datatype(static_cast(datatypes.at(i))); + pointField.add_count(counts.at(i)); + pointFields.push_back(pointField.Finish()); + } + return builder.CreateVector(pointFields); } -std::vector Hdf5FbPointCloud::loadBoundingBox(const std::string& uuid) -{ - // const std::scoped_lock lock(*m_write_mtx); - - // std::string hdf5DatasetPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid; - // std::shared_ptr group_ptr = - // std::make_shared(m_file->getGroup(hdf5DatasetPath)); std::vector bb; - // group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).write(bb); - // return bb; -} } // namespace seerep_hdf5_fb From db2ff37a420f6384b3e4e657b28fcd9b06b487b6 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 20 Sep 2022 11:57:45 +0000 Subject: [PATCH 18/54] read xyz of pointcloud2 in flatbuffers --- .../pointcloud/gRPC_fb_queryPointCloud.py | 78 +++++ ...ointCloud.py => gRPC_fb_sendPointCloud.py} | 9 +- examples/python/gRPC/util_fb.py | 83 +++-- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 17 +- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 289 +++++++++++------- .../seerep-core-fb/core-fb-conversion.h | 4 +- .../seerep-core-fb/core-fb-pointcloud.h | 10 +- .../seerep-core-fb/src/core-fb-conversion.cpp | 11 +- .../seerep-core-fb/src/core-fb-pointcloud.cpp | 47 +-- .../src/fb-point-cloud-service.cpp | 37 +++ 10 files changed, 413 insertions(+), 172 deletions(-) create mode 100644 examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py rename examples/python/gRPC/pointcloud/{gRPC_fb_pointCloud.py => gRPC_fb_sendPointCloud.py} (95%) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py new file mode 100644 index 000000000..f09721c68 --- /dev/null +++ b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py @@ -0,0 +1,78 @@ +import os +import sys + +import flatbuffers +from fb import point_cloud_service_grpc_fb as pointCloudService +from fb.PointCloud2 import PointCloud2 + +script_dir = os.path.dirname(__file__) +util_dir = os.path.join(script_dir, '..') +sys.path.append(util_dir) +import util +import util_fb + +channel = util.get_gRPC_channel() + +stubPointCloud = pointCloudService.PointCloudServiceStub(channel) +builder = flatbuffers.Builder(1024) + +PROJECTNAME = "testproject" +projectUuid = util_fb.getProject(builder, channel, PROJECTNAME) + +if projectUuid is None: + print(f"Project: {PROJECTNAME} does not exist") + sys.exit() + + +builder = flatbuffers.Builder(1024) + +# cerate time interval + +timeMin = util_fb.createTimeStamp(builder, 1610549273, 0) +timeMax = util_fb.createTimeStamp(builder, 1938549273, 0) +timeInterval = util_fb.createTimeInterval(builder, timeMin, timeMax) + +queryMsg = util_fb.createQuery( + builder, [builder.CreateString(projectUuid)], timeInterval, [builder.CreateString("BoundingBoxLabel0")] +) +builder.Finish(queryMsg) +buf = builder.Output() + +for responseBuf in stubPointCloud.GetPointCloud2(bytes(buf)): + response = PointCloud2.GetRootAs(responseBuf) + + print("---Header---") + print(f"Message UUID: {response.Header().UuidMsgs().decode('utf-8')}") + print(f"Project UUID: {response.Header().UuidProject().decode('utf-8')}") + print(f"Frame ID: {response.Header().FrameId().decode('utf-8')}") + + print("---Point Fields---") + for i in range(response.FieldsLength()): + print(f"Field Name: {response.Fields(i).Name().decode('utf-8')}") + print(f"Datatype: {response.Fields(i).Datatype()}") + print(f"Offset: {response.Fields(i).Offset()}") + print(f"Count: {response.Fields(i).Count()}") + + print("---Bounding Box Labels---") + for i in range(response.LabelsBbLength()): + print(f"Label {i}: {response.LabelsBb(i).LabelWithInstance().Label().decode('utf-8')}") + print(f"Instance {i}: {response.LabelsBb(i).LabelWithInstance().InstanceUuid().decode('utf-8')}") + print( + f"Bounding Box Min {i}: " + f"{response.LabelsBb(i).BoundingBox().PointMin().X()}," + f"{response.LabelsBb(i).BoundingBox().PointMin().Y()}," + f"{response.LabelsBb(i).BoundingBox().PointMin().Z()} " + f"(x,y,z)" + ) + print( + f"Bounding Box Min {i}: " + f"{response.LabelsBb(i).BoundingBox().PointMax().X()}," + f"{response.LabelsBb(i).BoundingBox().PointMax().Y()}," + f"{response.LabelsBb(i).BoundingBox().PointMax().Z()} " + f"(x,y,z)" + ) + + print("---General Labels----") + for i in range(response.LabelsGeneralLength()): + print(f"Label {i}: {response.LabelsGeneral(i).Label().decode('utf-8')}") + print(f"Instance {i}: {response.LabelsGeneral(i).InstanceUuid().decode('utf-8')}") diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py similarity index 95% rename from examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py rename to examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py index 27694ee78..997f43f2d 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_pointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py @@ -28,9 +28,9 @@ getOrCreateProject, ) -NUM_GENERAL_LABELS = 10 -NUM_BB_LABELS = 10 -NUM_POINT_CLOUDS = 10 +NUM_GENERAL_LABELS = 1 +NUM_BB_LABELS = 1 +NUM_POINT_CLOUDS = 1 def createPointCloud(builder, header, height=960, width=1280): @@ -97,7 +97,8 @@ def createPointClouds(projectUuid, numOf): channel = util.get_gRPC_channel() stub = pointCloudService.PointCloudServiceStub(channel) +builder = flatbuffers.Builder(1024) -projectUuid = getOrCreateProject(channel, "testproject", True) +projectUuid = getOrCreateProject(builder, channel, "testproject") responseBuf = stub.TransferPointCloud2(createPointClouds(projectUuid, NUM_POINT_CLOUDS)) diff --git a/examples/python/gRPC/util_fb.py b/examples/python/gRPC/util_fb.py index bb8e910cf..dcab71cb7 100644 --- a/examples/python/gRPC/util_fb.py +++ b/examples/python/gRPC/util_fb.py @@ -14,15 +14,17 @@ ProjectCreation, ProjectInfo, ProjectInfos, + Query, + TimeInterval, Timestamp, ) from fb import meta_operations_grpc_fb as metaOperations -def getOrCreateProject(channel, name, create=True, mapFrameId="map"): +def getProject(builder, channel, name): + '''Retrieve a project by name''' stubMeta = metaOperations.MetaOperationsStub(channel) - builder = flatbuffers.Builder(1024) Empty.Start(builder) emptyMsg = Empty.End(builder) builder.Finish(emptyMsg) @@ -31,31 +33,44 @@ def getOrCreateProject(channel, name, create=True, mapFrameId="map"): responseBuf = stubMeta.GetProjects(bytes(buf)) response = ProjectInfos.ProjectInfos.GetRootAs(responseBuf) - projectuuid = "" for i in range(response.ProjectsLength()): - print(response.Projects(i).Name().decode("utf-8") + " " + response.Projects(i).Uuid().decode("utf-8") + "\n") if response.Projects(i).Name().decode("utf-8") == name: - projectuuid = response.Projects(i).Uuid().decode("utf-8") + return response.Projects(i).Uuid().decode("utf-8") + return None - if projectuuid == "": + +def createProject(channel, builder, name, frameId): + '''Create a project from the parameters''' + stubMeta = metaOperations.MetaOperationsStub(channel) + + frameIdBuf = builder.CreateString(frameId) + nameBuf = builder.CreateString(name) + + ProjectCreation.Start(builder) + ProjectCreation.AddMapFrameId(builder, frameIdBuf) + ProjectCreation.AddName(builder, nameBuf) + projectCreationMsg = ProjectCreation.End(builder) + builder.Finish(projectCreationMsg) + + buf = builder.Output() + + responseBuf = stubMeta.CreateProject(bytes(buf)) + response = ProjectInfo.ProjectInfo.GetRootAs(responseBuf) + + return response.Uuid().decode("utf-8") + + +def getOrCreateProject(builder, channel, name, create=True, mapFrameId="map"): + '''Get the project,, or if not present, create one''' + projectUuid = getProject(builder, channel, name) + + if projectUuid is None: if create: - mapFrameIdBuf = builder.CreateString(mapFrameId) - nameBuf = builder.CreateString(name) - ProjectCreation.Start(builder) - ProjectCreation.AddMapFrameId(builder, mapFrameIdBuf) - ProjectCreation.AddName(builder, nameBuf) - projectCreationMsg = ProjectCreation.End(builder) - builder.Finish(projectCreationMsg) - buf = builder.Output() - - responseBuf = stubMeta.CreateProject(bytes(buf)) - response = ProjectInfo.ProjectInfo.GetRootAs(responseBuf) - - projectuuid = response.Uuid().decode("utf-8") + projectUuid = createProject(channel, builder, name, mapFrameId) else: sys.exit() - return projectuuid + return projectUuid # Header @@ -187,3 +202,31 @@ def addToPointFieldVector(builder, pointFieldList): for pointField in reversed(pointFieldList): builder.PrependUOffsetTRelative(pointField) return builder.EndVector() + + +def createQuery(builder, projectUuids, timeInterval, generalLabels): + # add project uuids + Query.StartProjectuuidVector(builder, len(projectUuids)) + for projectUuid in reversed(projectUuids): + builder.PrependUOffsetTRelative(projectUuid) + projectUuidsOffset = builder.EndVector() + + Query.StartLabelVector(builder, len(generalLabels)) + for label in reversed(generalLabels): + builder.PrependUOffsetTRelative(label) + labelsOffset = builder.EndVector() + + Query.Start(builder) + Query.AddProjectuuid(builder, projectUuidsOffset) + Query.AddTimeinterval(builder, timeInterval) + Query.AddLabel(builder, labelsOffset) + + return Query.End(builder) + + +def createTimeInterval(builder, timeMin, timeMax): + '''Create a time time interval in flatbuffers''' + TimeInterval.Start(builder) + TimeInterval.AddTimeMin(builder, timeMin) + TimeInterval.AddTimeMax(builder, timeMax) + return TimeInterval.End(builder) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index 29c532504..67647c038 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -40,7 +40,7 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral */ struct PointFieldInfo { - uint8_t datatype; + int32_t datatype; uint32_t count; }; @@ -104,6 +104,8 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral CloudInfo getCloudInfo(const seerep::fb::PointCloud2& cloud); + CloudInfo getCloudInfo(const std::vector& names); + void writePoints(const std::string& uuid, const std::shared_ptr& data_group_ptr, const seerep::fb::PointCloud2& cloud); @@ -118,7 +120,7 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral writePointFieldAttributes(HighFive::Group& cloud_group, const flatbuffers::Vector>& vectorPointField); - void readPoints(const std::string& uuid, seerep::fb::PointCloud2& cloud); + void readPoints(const std::string& uuid, std::vector>>& pointData); void readColorsRGB(const std::string& uuid, seerep::fb::PointCloud2& cloud); @@ -127,9 +129,6 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral void readOtherFields(const std::string& uuid, seerep::fb::PointCloud2& cloud, const std::map& fields); - std::optional>>> - readPointFieldAttributes(const std::string& id, std::shared_ptr& data_set_ptr); - // pointcloud attribute keys inline static const std::string HEIGHT = "height"; inline static const std::string WIDTH = "width"; @@ -140,10 +139,10 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral inline static const std::string IS_DENSE = "is_dense"; // point field attribute keys - inline static const std::string FIELD_NAME = "name"; - inline static const std::string FIELD_OFFSET = "offset"; - inline static const std::string FIELD_DATATYPE = "datatype"; - inline static const std::string FIELD_COUNT = "counts"; + inline static const std::string FIELD_NAME = "field_name"; + inline static const std::string FIELD_OFFSET = "field_offset"; + inline static const std::string FIELD_DATATYPE = "field_datatype"; + inline static const std::string FIELD_COUNT = "field_count"; public: inline static const std::string HDF5_GROUP_POINTCLOUD = "pointclouds"; diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index b2e4988a4..f6a7fd541 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -308,6 +308,37 @@ Hdf5FbPointCloud::CloudInfo Hdf5FbPointCloud::getCloudInfo(const seerep::fb::Poi return info; } +Hdf5FbPointCloud::CloudInfo Hdf5FbPointCloud::getCloudInfo(const std::vector& names) +{ + bool hasFieldx = false; + bool hasFieldy = false; + bool hasFieldz = false; + + CloudInfo info; + for (auto fieldName : names) + { + if (fieldName == "x") + hasFieldx = true; + else if (fieldName == "y") + hasFieldy = true; + else if (fieldName == "z") + hasFieldz = true; + else if (fieldName == "rgb") + info.has_rgb = true; + else if (fieldName == "rgba") + info.has_rgba = true; + else if (fieldName.find("normal") == 0) + info.has_normals = true; + else + { + info.other_fields[fieldName] = (struct PointFieldInfo){}; + } + } + if (hasFieldx && hasFieldy && hasFieldz) + info.has_points = true; + return info; +} + // TODO read partial point cloud, e.g. only xyz without color, etc. std::optional> Hdf5FbPointCloud::readPointCloud2(const std::string& id) @@ -316,33 +347,31 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) const std::string hdf5DatasetPath = HDF5_GROUP_POINTCLOUD + "/" + id; + // check if the point cloud exists if (!m_file->exist(hdf5DatasetPath)) { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << hdf5DatasetPath << "does not exist"; return std::nullopt; } - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "loading " << hdf5DatasetPath; - - std::shared_ptr data_set_ptr = - std::make_shared(m_file->getDataSet(hdf5DatasetPath)); + std::shared_ptr data_group_ptr = + std::make_shared(m_file->getGroup(hdf5DatasetPath)); flatbuffers::grpc::MessageBuilder builder; - HighFive::Group cloud_group = m_file->getGroup(id); - + // read general attributes uint32_t height, width, point_step, row_step; bool is_bigendian, is_dense; try { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "loading attributes"; - height = readAttributeFromHdf5(id, *data_set_ptr, HEIGHT); - width = readAttributeFromHdf5(id, *data_set_ptr, WIDTH); - point_step = readAttributeFromHdf5(id, *data_set_ptr, POINT_STEP); - row_step = readAttributeFromHdf5(id, *data_set_ptr, ROW_STEP); - is_bigendian = readAttributeFromHdf5(id, *data_set_ptr, IS_BIGENDIAN); - is_dense = readAttributeFromHdf5(id, *data_set_ptr, IS_DENSE); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "loading general attributes"; + height = readAttributeFromHdf5(id, *data_group_ptr, HEIGHT); + width = readAttributeFromHdf5(id, *data_group_ptr, WIDTH); + point_step = readAttributeFromHdf5(id, *data_group_ptr, POINT_STEP); + row_step = readAttributeFromHdf5(id, *data_group_ptr, ROW_STEP); + is_bigendian = readAttributeFromHdf5(id, *data_group_ptr, IS_BIGENDIAN); + is_dense = readAttributeFromHdf5(id, *data_group_ptr, IS_DENSE); } catch (const std::invalid_argument& e) { @@ -350,75 +379,169 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) return std::nullopt; } - auto headerAttributes = readHeaderAttributes(cloud_group, id, builder); - auto pointFields = readPointFieldAttributes(id, data_set_ptr); + // read header attributes + auto headerAttributes = readHeaderAttributes(*data_group_ptr, id, builder); + + // read point field attributes + std::vector names; + std::vector offsets, counts; + std::vector datatypes; - if (!pointFields.has_value()) + try { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << "could not read point fields"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "loading point field attributes"; + names = readAttributeFromHdf5>(id, *data_group_ptr, FIELD_NAME); + offsets = readAttributeFromHdf5>(id, *data_group_ptr, FIELD_OFFSET); + counts = readAttributeFromHdf5>(id, *data_group_ptr, FIELD_COUNT); + datatypes = readAttributeFromHdf5>(id, *data_group_ptr, FIELD_DATATYPE); + } + catch (const std::invalid_argument& e) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << "error: " << e.what(); return std::nullopt; } - seerep::fb::PointCloud2Builder pointCloudBuilder(builder); - pointCloudBuilder.add_header(headerAttributes); - pointCloudBuilder.add_height(height); - pointCloudBuilder.add_width(width); - pointCloudBuilder.add_point_step(point_step); - pointCloudBuilder.add_row_step(row_step); - pointCloudBuilder.add_is_bigendian(is_bigendian); - pointCloudBuilder.add_is_dense(is_dense); - pointCloudBuilder.add_fields(pointFields.value()); - auto pointCloud = pointCloudBuilder.Finish(); + if (!sameVectorSize(names, offsets, counts, datatypes)) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) + << "number of read point field attributes does not match"; + return std::nullopt; + } - auto data_ptr = builder.GetBufferPointer(); + std::vector> pointFields; + for (size_t i = 0; i < names.size(); i++) + { + auto nameStr = builder.CreateString(names.at(i)); + seerep::fb::PointFieldBuilder pointField(builder); + pointField.add_name(nameStr); + pointField.add_offset(offsets.at(i)); + pointField.add_datatype(static_cast(datatypes.at(i))); + pointField.add_count(counts.at(i)); + pointFields.push_back(pointField.Finish()); + } + auto pointFieldsVector = builder.CreateVector(pointFields); - auto pointCloud2 = reinterpret_cast(builder.GetBufferPointer()); + // combine read points fields + CloudInfo info = getCloudInfo(names); - CloudInfo info = getCloudInfo(*pointCloud2); + std::vector>> points; + std::vector>> pointsRGB; + std::vector>> pointsRGBA; if (info.has_points) - readPoints(id, *pointCloud2); + readPoints(id, points); - if (info.has_rgb) - readColorsRGB(id, *pointCloud2); + // if (info.has_rgb) + // readColorsRGB(id, *pointCloud2); - if (info.has_rgba) - readColorsRGBA(id, *pointCloud2); + // if (info.has_rgba) + // readColorsRGBA(id, *pointCloud2); - // // TODO normals + // TODO normals - if (!info.other_fields.empty()) - readOtherFields(id, *pointCloud2, info.other_fields); + // if (!info.other_fields.empty()) + // readOtherFields(id, *pointCloud2, info.other_fields); - // return pointCloud2; -} + // TODO extra fields other than x,y,z + // converting the point cloud data into a 1D array + std::vector data; + data.reserve(height * width * point_step); + for (uint32_t row = 0; row < height; row++) + { + for (uint32_t col = 0; col < width; col++) + { + data.insert(data.end(), std::make_move_iterator(points.at(row).at(col).begin()), + std::make_move_iterator(points.at(row).at(col).end())); + } + } + auto dataVector = builder.CreateVector(data); -void Hdf5FbPointCloud::readPoints(const std::string& uuid, seerep::fb::PointCloud2& cloud) -{ - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "reading points"; + // read labeled bounding box + std::vector boundingBoxesLabels; + std::vector> boundingBoxes; + std::vector boundingBoxesInstances; + readBoundingBoxLabeled(HDF5_GROUP_POINTCLOUD, id, boundingBoxesLabels, boundingBoxes, boundingBoxesInstances); - seerep_hdf5_fb::PointCloud2Iterator x_iter(cloud, "x"); - seerep_hdf5_fb::PointCloud2Iterator y_iter(cloud, "y"); - seerep_hdf5_fb::PointCloud2Iterator z_iter(cloud, "z"); + std::vector> boundingBoxLabeled; + for (size_t i = 0; i < boundingBoxes.size(); i++) + { + auto instance = builder.CreateString(boundingBoxesInstances.at(i)); + auto label = builder.CreateString(boundingBoxesLabels.at(i)); - std::vector>> point_data; - std::string hdf5DatasetPointsPath = - seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; + seerep::fb::LabelWithInstanceBuilder labelBuilder(builder); + labelBuilder.add_instanceUuid(instance); + labelBuilder.add_label(label); + auto labelWithInstance = labelBuilder.Finish(); + + auto pointMin = seerep::fb::CreatePoint(builder, boundingBoxes.at(i).at(0), boundingBoxes.at(i).at(1), + boundingBoxes.at(i).at(2)); + auto pointMax = seerep::fb::CreatePoint(builder, boundingBoxes.at(i).at(3), boundingBoxes.at(i).at(4), + boundingBoxes.at(i).at(5)); + + seerep::fb::BoundingboxBuilder bbBuilder(builder); + bbBuilder.add_point_min(pointMin); + bbBuilder.add_point_max(pointMax); + auto bb = bbBuilder.Finish(); - HighFive::DataSet points_dataset = m_file->getDataSet(hdf5DatasetPointsPath); + seerep::fb::BoundingBoxLabeledBuilder bblabeledBuilder(builder); + bblabeledBuilder.add_bounding_box(bb); + bblabeledBuilder.add_labelWithInstance(labelWithInstance); - points_dataset.read(point_data); + boundingBoxLabeled.push_back(bblabeledBuilder.Finish()); + } + auto boundingBoxLabeledVector = builder.CreateVector(boundingBoxLabeled); + + // read labels general + std::vector labelsGeneral; + std::vector labelsGeneralInstances; + readLabelsGeneral(HDF5_GROUP_POINTCLOUD, id, labelsGeneral, labelsGeneralInstances); - for (auto column : point_data) + // TODO add this to a higher level, it's the same for images and point clouds + std::vector> labelGeneral; + labelGeneral.reserve(labelsGeneral.size()); + for (size_t i = 0; i < labelsGeneral.size(); i++) { - for (auto row : column) - { - *x_iter = row[0]; - *y_iter = row[1]; - *z_iter = row[2]; - ++x_iter, ++y_iter, ++z_iter; - } + auto labelOffset = builder.CreateString(labelsGeneral.at(i)); + auto instanceOffset = builder.CreateString(labelsGeneralInstances.at(i)); + + seerep::fb::LabelWithInstanceBuilder labelBuilder(builder); + labelBuilder.add_label(labelOffset); + labelBuilder.add_instanceUuid(instanceOffset); + labelGeneral.push_back(labelBuilder.Finish()); } + + auto labelsGeneralVector = builder.CreateVector>(labelGeneral); + + // build the point cloud message + seerep::fb::PointCloud2Builder pointCloudBuilder(builder); + pointCloudBuilder.add_header(headerAttributes); + pointCloudBuilder.add_height(height); + pointCloudBuilder.add_width(width); + pointCloudBuilder.add_fields(pointFieldsVector); + pointCloudBuilder.add_is_bigendian(is_bigendian); + pointCloudBuilder.add_point_step(point_step); + pointCloudBuilder.add_row_step(row_step); + pointCloudBuilder.add_data(dataVector); + pointCloudBuilder.add_is_dense(is_dense); + pointCloudBuilder.add_labels_general(labelsGeneralVector); + pointCloudBuilder.add_labels_bb(boundingBoxLabeledVector); + auto pointCloudOffset = pointCloudBuilder.Finish(); + builder.Finish(pointCloudOffset); + + auto grpcPointCloud = builder.ReleaseMessage(); + return grpcPointCloud; +} + +// ToDo Exception safety? +void Hdf5FbPointCloud::readPoints(const std::string& uuid, std::vector>>& pointData) +{ + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "reading points from hdf5 file"; + + std::string hdf5DatasetPointsPath = + seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; + + HighFive::DataSet pointsDataset = m_file->getDataSet(hdf5DatasetPointsPath); + pointsDataset.read(pointData); } void Hdf5FbPointCloud::readColorsRGB(const std::string& uuid, seerep::fb::PointCloud2& cloud) @@ -520,52 +643,4 @@ void Hdf5FbPointCloud::readOtherFields(const std::string& uuid, seerep::fb::Poin } } -std::optional>>> -Hdf5FbPointCloud::readPointFieldAttributes(const std::string& id, std::shared_ptr& data_set_ptr) -{ - std::vector names; - std::vector offsets, counts; - std::vector datatypes; - - // try to read the point fields attributes from the hdf5 file - try - { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "loading point field attributes"; - names = readAttributeFromHdf5>(id, *data_set_ptr, FIELD_NAME); - offsets = readAttributeFromHdf5>(id, *data_set_ptr, FIELD_OFFSET); - counts = readAttributeFromHdf5>(id, *data_set_ptr, FIELD_COUNT); - datatypes = readAttributeFromHdf5>(id, *data_set_ptr, FIELD_DATATYPE); - } - catch (const std::invalid_argument& e) - { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << "error: " << e.what(); - return std::nullopt; - } - - // verify that the same number of point fields were read - if (!sameVectorSize(names, offsets, counts, datatypes)) - { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << "error: " - << "length of point fields differ"; - return std::nullopt; - } - - std::vector> pointFields; - pointFields.reserve(names.size()); - - flatbuffers::grpc::MessageBuilder builder; - - for (long unsigned int i = 0; i < names.size(); i++) - { - auto nameStr = builder.CreateString(names.at(i)); - seerep::fb::PointFieldBuilder pointField(builder); - pointField.add_name(nameStr); - pointField.add_offset(offsets.at(i)); - pointField.add_datatype(static_cast(datatypes.at(i))); - pointField.add_count(counts.at(i)); - pointFields.push_back(pointField.Finish()); - } - return builder.CreateVector(pointFields); -} - } // namespace seerep_hdf5_fb diff --git a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-conversion.h b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-conversion.h index 8e0bb944b..905f3dcb2 100644 --- a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-conversion.h +++ b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-conversion.h @@ -149,8 +149,10 @@ class CoreFbConversion * @brief converts the header of the flatbuffer data message to seerep core specific message * @param header the header in the flatbuffer data message * @param coreHeader the header in the data message in seerep core format + * @param datatype of the associated message */ - static void fromFbDataHeader(const seerep::fb::Header* header, seerep_core_msgs::Header& coreHeader); + static void fromFbDataHeader(const seerep::fb::Header* header, seerep_core_msgs::Header& coreHeader, + seerep_core_msgs::Datatype&& datatype); /** * @brief converts (or generates if not set) the msg uuid of the flatbuffer data message to seerep core specific message diff --git a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h index 158adc3cc..4c89388a0 100644 --- a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h +++ b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h @@ -4,8 +4,8 @@ #include #include -#include "seerep-core-fb/core-fb-conversion.h" -#include "seerep-core-fb/core-fb-general.h" +#include "core-fb-conversion.h" +#include "core-fb-general.h" // seerep-msgs #include @@ -18,7 +18,10 @@ // seerep-core #include +// add as temporary fix +#include // uuid + #include #include #include // uuid class @@ -33,7 +36,8 @@ class CoreFbPointCloud CoreFbPointCloud(std::shared_ptr seerepCore); ~CoreFbPointCloud(); - std::vector getData(const seerep::fb::Query& query); + void getData(const seerep::fb::Query* query, + grpc::ServerWriter>* const writer); boost::uuids::uuid addData(const seerep::fb::PointCloud2& pc); private: diff --git a/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp b/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp index b1739bece..01a402e64 100644 --- a/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp +++ b/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp @@ -48,7 +48,7 @@ seerep_core_msgs::DatasetIndexable CoreFbConversion::fromFb(const seerep::fb::Im { seerep_core_msgs::DatasetIndexable dataForIndices; - fromFbDataHeader(img.header(), dataForIndices.header); + fromFbDataHeader(img.header(), dataForIndices.header, seerep_core_msgs::Datatype::Image); // set bounding box for images to 0. assume no spatial extent dataForIndices.boundingbox.min_corner().set<0>(0); @@ -79,7 +79,7 @@ seerep_core_msgs::DatasetIndexable CoreFbConversion::fromFb(const seerep::fb::Po { seerep_core_msgs::DatasetIndexable dataForIndices; - fromFbDataHeader(point->header(), dataForIndices.header); + fromFbDataHeader(point->header(), dataForIndices.header, seerep_core_msgs::Datatype::Point); // set bounding box for point to point coordinates. assume no spatial extent dataForIndices.boundingbox.min_corner().set<0>(point->point()->x()); @@ -105,7 +105,7 @@ seerep_core_msgs::DatasetIndexable CoreFbConversion::fromFb(const seerep::fb::Po seerep_core_msgs::DatasetIndexable CoreFbConversion::fromFb(const seerep::fb::PointCloud2& cloud) { seerep_core_msgs::DatasetIndexable dataForIndices; - fromFbDataHeader(cloud.header(), dataForIndices.header); + fromFbDataHeader(cloud.header(), dataForIndices.header, seerep_core_msgs::Datatype::PointCloud); // set bounding box for images to 0. assume no spatial extent dataForIndices.boundingbox.min_corner().set<0>(0); @@ -266,7 +266,8 @@ bool CoreFbConversion::fromFbQueryWithoutData(const seerep::fb::Query* query) return false; } -void CoreFbConversion::fromFbDataHeader(const seerep::fb::Header* header, seerep_core_msgs::Header& coreHeader) +void CoreFbConversion::fromFbDataHeader(const seerep::fb::Header* header, seerep_core_msgs::Header& coreHeader, + seerep_core_msgs::Datatype&& datatype) { if (flatbuffers::IsFieldPresent(header, seerep::fb::Header::VT_UUID_MSGS)) { @@ -278,7 +279,7 @@ void CoreFbConversion::fromFbDataHeader(const seerep::fb::Header* header, seerep } // ToDo change datatype accordingly - coreHeader.datatype = seerep_core_msgs::Datatype::Image; + coreHeader.datatype = datatype; coreHeader.frameId = header->frame_id()->str(); coreHeader.timestamp.seconds = header->stamp()->seconds(); coreHeader.timestamp.nanos = header->stamp()->nanos(); diff --git a/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp index e73539d1c..6b862032c 100644 --- a/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp +++ b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp @@ -11,30 +11,31 @@ CoreFbPointCloud::~CoreFbPointCloud() { } -std::vector CoreFbPointCloud::getData(const seerep::fb::Query& query) +void CoreFbPointCloud::getData(const seerep::fb::Query* query, + grpc::ServerWriter>* const writer) { - // BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "loading point cloud from pointclouds/"; - // seerep_core_msgs::Query queryCore = CoreFbConversion::fromFb(query, seerep_core_msgs::Datatype::PointCloud); - // seerep_core_msgs::QueryResult resultCore = m_seerepCore->getDataset(queryCore); - - // std::vector resultPointClouds; - // for (auto project : resultCore.queryResultProjects) - // { - // BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) - // << "sending point cloud from project" << boost::lexical_cast(project.projectUuid); - // for (auto uuidPc : project.dataOrInstanceUuids) - // { - // auto img = CoreFbGeneral::getHdf5(project.projectUuid, m_seerepCore, m_hdf5IoMap) - // ->readPointCloud(boost::lexical_cast(uuidImg), queryCore.withoutData); - // if (pc) - // { - // BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) - // << "sending point cloud " << boost::lexical_cast(uuidImg); - // resultPointClouds.push_back(pc.value()); - // } - // } - // } - // return resultPointClouds; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "loading point cloud from points/" << std::endl; + seerep_core_msgs::Query queryCore = + seerep_core_fb::CoreFbConversion::fromFb(query, seerep_core_msgs::Datatype::PointCloud); + + seerep_core_msgs::QueryResult resultCore = m_seerepCore->getDataset(queryCore); + + for (auto project : resultCore.queryResultProjects) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) + << "sending point cloud from project" << boost::lexical_cast(project.projectUuid); + for (auto uuidPc : project.dataOrInstanceUuids) + { + auto pc = CoreFbGeneral::getHdf5(project.projectUuid, m_seerepCore, m_hdf5IoMap) + ->readPointCloud2(boost::lexical_cast(uuidPc)); + if (pc) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) + << "sending point cloud " << boost::lexical_cast(uuidPc); + writer->Write(pc.value()); + } + } + } } boost::uuids::uuid CoreFbPointCloud::addData(const seerep::fb::PointCloud2& pc) diff --git a/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp b/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp index 5cacf9c76..e757b1891 100644 --- a/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp +++ b/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp @@ -12,6 +12,43 @@ FbPointCloudService::GetPointCloud2(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, grpc::ServerWriter>* writer) { + (void)context; + auto requestRoot = request->GetRoot(); + std::stringstream debuginfo; + debuginfo << "sending point clouds with this query parameters:"; + if (requestRoot->boundingbox() != NULL) + { + debuginfo << "bounding box min(" << requestRoot->boundingbox()->point_min()->x() << "/" + << requestRoot->boundingbox()->point_min()->y() << "/" << requestRoot->boundingbox()->point_min()->z() + << "), max(" << requestRoot->boundingbox()->point_max()->x() << "/" + << requestRoot->boundingbox()->point_max()->y() << "/" << requestRoot->boundingbox()->point_max()->z() + << ")"; + } + if (requestRoot->timeinterval() != NULL) + { + debuginfo << "time interval (" << requestRoot->timeinterval()->time_min()->seconds() << "/" + << requestRoot->timeinterval()->time_max()->seconds() << ")"; + } + if (requestRoot->label() != NULL) + { + debuginfo << "labels general"; + for (auto label : *requestRoot->label()) + { + debuginfo << "'" << label->str() << "' "; + } + } + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << debuginfo.rdbuf(); + + try + { + pointCloudFb->getData(requestRoot, writer); + } + catch (std::runtime_error const& e) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); + } + return grpc::Status::OK; } grpc::Status FbPointCloudService::TransferPointCloud2( From 74954f00e575f2b2887dd7834662f54f0a015ffc Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Wed, 21 Sep 2022 08:52:20 +0000 Subject: [PATCH 19/54] add rgb and rgba read for flatbuffers point clouds --- .../pointcloud/gRPC_fb_queryPointCloud.py | 5 +- .../gRPC/pointcloud/gRPC_fb_sendPointCloud.py | 16 +- examples/python/gRPC/util.py | 7 +- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 19 +- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 174 +++++++++--------- 5 files changed, 116 insertions(+), 105 deletions(-) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py index f09721c68..925d27a32 100644 --- a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py @@ -65,7 +65,7 @@ f"(x,y,z)" ) print( - f"Bounding Box Min {i}: " + f"Bounding Box Max {i}: " f"{response.LabelsBb(i).BoundingBox().PointMax().X()}," f"{response.LabelsBb(i).BoundingBox().PointMax().Y()}," f"{response.LabelsBb(i).BoundingBox().PointMax().Z()} " @@ -76,3 +76,6 @@ for i in range(response.LabelsGeneralLength()): print(f"Label {i}: {response.LabelsGeneral(i).Label().decode('utf-8')}") print(f"Instance {i}: {response.LabelsGeneral(i).InstanceUuid().decode('utf-8')}") + + print("---Points---") + print(response.DataAsNumpy()) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py index 997f43f2d..843278693 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py @@ -35,7 +35,7 @@ def createPointCloud(builder, header, height=960, width=1280): '''Creates a flatbuffers point cloud message''' - pointFields = createPointFields(builder, "xyz", 7, 4, 1) + pointFields = createPointFields(builder, ['x', 'y', 'z', 'rgb'], 7, 4, 1) pointFieldsVector = addToPointFieldVector(builder, pointFields) # create general labels @@ -50,8 +50,8 @@ def createPointCloud(builder, header, height=960, width=1280): boundingBoxes = createBoundingBoxes( builder, header, - [createPoint(builder, np.random.rand(), np.random.rand(), np.random.rand()) for i in range(NUM_BB_LABELS)], - [createPoint(builder, np.random.rand(), np.random.rand(), np.random.rand()) for i in range(NUM_BB_LABELS)], + [createPoint(builder, np.random.rand(), np.random.rand(), np.random.rand()) for _ in range(NUM_BB_LABELS)], + [createPoint(builder, np.random.rand(), np.random.rand(), np.random.rand()) for _ in range(NUM_BB_LABELS)], ) labelWithInstances = createLabelsWithInstance( builder, @@ -61,8 +61,10 @@ def createPointCloud(builder, header, height=960, width=1280): labelsBb = createBoundingBoxesLabeled(builder, labelWithInstances, boundingBoxes) labelsBbVector = addToBoundingBoxLabeledVector(builder, labelsBb) - # create ordered point cloud with dim (height, width, 3) - points = np.random.randn(height, width, 3).astype(np.float32) + # create ordered point cloud with dim (height, width, 6) + points = np.random.randn(height, width, 6).astype(np.float32) + print(f"Data shape: {points.shape}") + pointsVector = builder.CreateByteVector(points.tobytes()) # add all data into the flatbuffers point cloud message @@ -71,8 +73,8 @@ def createPointCloud(builder, header, height=960, width=1280): PointCloud2.AddHeight(builder, points.shape[0]) PointCloud2.AddWidth(builder, points.shape[1]) PointCloud2.AddIsBigendian(builder, False) - PointCloud2.AddPointStep(builder, 12) - PointCloud2.AddRowStep(builder, points.shape[1] * 12) + PointCloud2.AddPointStep(builder, 24) + PointCloud2.AddRowStep(builder, points.shape[1] * 24) PointCloud2.AddFields(builder, pointFieldsVector) PointCloud2.AddData(builder, pointsVector) PointCloud2.AddLabelsGeneral(builder, labelsGeneralVector) diff --git a/examples/python/gRPC/util.py b/examples/python/gRPC/util.py index d089de140..cd2cddfb7 100644 --- a/examples/python/gRPC/util.py +++ b/examples/python/gRPC/util.py @@ -23,6 +23,11 @@ def get_gRPC_channel(target="local"): elif target == "local": # server without certs server = "localhost:9090" - channel = grpc.insecure_channel(server) + # set the max message size to 1GB (half the size of the server) + options = [ + ('grpc.max_send_message_length', 1 * 1024 * 1024 * 1024), + ('grpc.max_receive_message_length', 1 * 1024 * 1024 * 1024), + ] + channel = grpc.insecure_channel(server, options=options) return channel diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index 67647c038..c732cdd0e 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -14,6 +14,7 @@ #include // std +#include #include #include @@ -54,20 +55,12 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral }; template - void read(const std::string cloud_uuid, const std::string& field_name, seerep::fb::PointCloud2& cloud, size_t size) + void read(const std::string cloud_uuid, const std::string& field_name, std::vector& data, size_t size) { const std::string id = seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX + "/" + cloud_uuid + "/" + field_name; - PointCloud2Iterator iter(cloud, field_name); HighFive::DataSet dataset = m_file->getDataSet(id); - std::vector data; data.reserve(size); dataset.read(data); - - for (auto& value : data) - { - *iter = value; - ++iter; - } } template @@ -122,12 +115,12 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral void readPoints(const std::string& uuid, std::vector>>& pointData); - void readColorsRGB(const std::string& uuid, seerep::fb::PointCloud2& cloud); + void readColorsRGB(const std::string& uuid, std::vector>>& colorData); - void readColorsRGBA(const std::string& uuid, seerep::fb::PointCloud2& cloud); + void readColorsRGBA(const std::string& uuid, std::vector>>& colorData); - void readOtherFields(const std::string& uuid, seerep::fb::PointCloud2& cloud, - const std::map& fields); + void readOtherFields(const std::string& uuid, const std::map& fields, + std::vector otherFieldsData); // pointcloud attribute keys inline static const std::string HEIGHT = "height"; diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index f6a7fd541..63f5408d8 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -425,35 +425,50 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) CloudInfo info = getCloudInfo(names); std::vector>> points; - std::vector>> pointsRGB; - std::vector>> pointsRGBA; + std::vector>> pointsRGB; + std::vector>> pointsRGBA; + std::vector otherFields; if (info.has_points) readPoints(id, points); - // if (info.has_rgb) - // readColorsRGB(id, *pointCloud2); + if (info.has_rgb) + readColorsRGB(id, pointsRGB); - // if (info.has_rgba) - // readColorsRGBA(id, *pointCloud2); + if (info.has_rgba) + readColorsRGBA(id, pointsRGBA); - // TODO normals + if (!info.other_fields.empty()) + readOtherFields(id, info.other_fields, otherFields); - // if (!info.other_fields.empty()) - // readOtherFields(id, *pointCloud2, info.other_fields); + // TODO add normals - // TODO extra fields other than x,y,z - // converting the point cloud data into a 1D array std::vector data; - data.reserve(height * width * point_step); - for (uint32_t row = 0; row < height; row++) + if (!points.empty()) { - for (uint32_t col = 0; col < width; col++) + // converting the point cloud data into a 1D array + data.reserve(height * width); + for (size_t row = 0; row < height; row++) { - data.insert(data.end(), std::make_move_iterator(points.at(row).at(col).begin()), - std::make_move_iterator(points.at(row).at(col).end())); + for (size_t col = 0; col < width; col++) + { + data.insert(data.end(), std::make_move_iterator(points.at(row).at(col).begin()), + std::make_move_iterator(points.at(row).at(col).end())); + if (info.has_rgb) + { + data.insert(data.end(), std::make_move_iterator(pointsRGB.at(row).at(col).begin()), + std::make_move_iterator(pointsRGB.at(row).at(col).end())); + } + if (info.has_rgba) + { + data.insert(data.end(), std::make_move_iterator(pointsRGBA.at(row).at(col).begin()), + std::make_move_iterator(pointsRGBA.at(row).at(col).end())); + } + // TODO add support for other fields like laser scanner info + } } } + int vectorSize = sizeof(std::vector) + (sizeof(uint8_t) * data.size()); auto dataVector = builder.CreateVector(data); // read labeled bounding box @@ -532,7 +547,6 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) return grpcPointCloud; } -// ToDo Exception safety? void Hdf5FbPointCloud::readPoints(const std::string& uuid, std::vector>>& pointData) { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "reading points from hdf5 file"; @@ -544,67 +558,28 @@ void Hdf5FbPointCloud::readPoints(const std::string& uuid, std::vector>>& colorData) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "reading rgb "; - seerep_hdf5_fb::PointCloud2Iterator r_iter(cloud, "r"); - seerep_hdf5_fb::PointCloud2Iterator g_iter(cloud, "g"); - seerep_hdf5_fb::PointCloud2Iterator b_iter(cloud, "b"); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "reading rgb from hdf5 file "; - std::vector>> color_data; - std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + std::string colorsId = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + HighFive::DataSet colorsDataset = m_file->getDataSet(colorsId); - HighFive::DataSet colors_dataset = m_file->getDataSet(colors_id); - - colors_dataset.read(color_data); - - for (auto column : color_data) - { - for (auto row : column) - { - *r_iter = row[0]; - *g_iter = row[1]; - *b_iter = row[2]; - ++r_iter, ++g_iter, ++b_iter; - } - } + colorsDataset.read(colorData); } -void Hdf5FbPointCloud::readColorsRGBA(const std::string& uuid, seerep::fb::PointCloud2& cloud) +void Hdf5FbPointCloud::readColorsRGBA(const std::string& uuid, std::vector>>& colorData) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "reading rgb "; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "reading rgba from hdf5 file "; - seerep_hdf5_fb::PointCloud2Iterator r_iter(cloud, "r"); - seerep_hdf5_fb::PointCloud2Iterator g_iter(cloud, "g"); - seerep_hdf5_fb::PointCloud2Iterator b_iter(cloud, "b"); - seerep_hdf5_fb::PointCloud2Iterator a_iter(cloud, "a"); - - std::vector>> color_data; std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; - HighFive::DataSet colors_dataset = m_file->getDataSet(colors_id); - colors_dataset.read(color_data); - - for (auto column : color_data) - { - for (auto row : column) - { - *r_iter = row[0]; - *g_iter = row[1]; - *b_iter = row[2]; - *a_iter = row[3]; - - ++r_iter; - ++g_iter; - ++b_iter; - ++a_iter; - } - } + colors_dataset.read(colorData); } -void Hdf5FbPointCloud::readOtherFields(const std::string& uuid, seerep::fb::PointCloud2& cloud, - const std::map& fields) +void Hdf5FbPointCloud::readOtherFields(const std::string& uuid, const std::map& fields, + std::vector otherFieldsData) { for (auto field_map_entry : fields) { @@ -613,31 +588,64 @@ void Hdf5FbPointCloud::readOtherFields(const std::string& uuid, seerep::fb::Poin switch (pointFieldInfo.datatype) { case seerep::fb::Point_Field_Datatype_INT8: - read(uuid, name, cloud, pointFieldInfo.count); - break; + { + std::vector data; + read(uuid, name, data, pointFieldInfo.count); + otherFieldsData.push_back(std::move(data)); + } + break; case seerep::fb::Point_Field_Datatype_UINT8: - read(uuid, name, cloud, pointFieldInfo.count); - break; + { + std::vector data; + read(uuid, name, data, pointFieldInfo.count); + otherFieldsData.push_back(std::move(data)); + } + break; case seerep::fb::Point_Field_Datatype_INT16: - read(uuid, name, cloud, pointFieldInfo.count); - break; + { + std::vector data; + read(uuid, name, data, pointFieldInfo.count); + otherFieldsData.push_back(std::move(data)); + } + break; case seerep::fb::Point_Field_Datatype_UINT16: - read(uuid, name, cloud, pointFieldInfo.count); - break; + { + std::vector data; + read(uuid, name, data, pointFieldInfo.count); + otherFieldsData.push_back(std::move(data)); + } + break; case seerep::fb::Point_Field_Datatype_INT32: - read(uuid, name, cloud, pointFieldInfo.count); - break; + { + std::vector data; + read(uuid, name, data, pointFieldInfo.count); + otherFieldsData.push_back(std::move(data)); + } + break; case seerep::fb::Point_Field_Datatype_UINT32: - read(uuid, name, cloud, pointFieldInfo.count); - break; + { + std::vector data; + read(uuid, name, data, pointFieldInfo.count); + otherFieldsData.push_back(std::move(data)); + } + break; case seerep::fb::Point_Field_Datatype_FLOAT32: - read(uuid, name, cloud, pointFieldInfo.count); - break; + { + std::vector data; + read(uuid, name, data, pointFieldInfo.count); + otherFieldsData.push_back(std::move(data)); + } + break; case seerep::fb::Point_Field_Datatype_FLOAT64: - read(uuid, name, cloud, pointFieldInfo.count); - break; + { + std::vector data; + read(uuid, name, data, pointFieldInfo.count); + otherFieldsData.push_back(std::move(data)); + } + break; default: - std::cout << "datatype of pointcloud unknown" << std::endl; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) + << "datatype of field in point cloud unknown"; break; } } From 4e1e98225e426d85011fa134591d4fcb3d1ceff5 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Wed, 21 Sep 2022 14:55:19 +0000 Subject: [PATCH 20/54] fix: point cloud indicies on server startup --- examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py | 2 +- examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py | 5 +++-- seerep-hdf5/seerep-hdf5-core/src/hdf5-core-point-cloud.cpp | 7 ++----- seerep-srv/seerep-core/src/core-project.cpp | 2 +- seerep-srv/seerep-core/src/core.cpp | 3 +-- 5 files changed, 8 insertions(+), 11 deletions(-) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py index 925d27a32..832769165 100644 --- a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py @@ -78,4 +78,4 @@ print(f"Instance {i}: {response.LabelsGeneral(i).InstanceUuid().decode('utf-8')}") print("---Points---") - print(response.DataAsNumpy()) + # TODO check if the read point cloud matches diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py index 843278693..bdb5535b3 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py @@ -3,6 +3,7 @@ import os import sys import time +import uuid import flatbuffers import numpy as np @@ -42,7 +43,7 @@ def createPointCloud(builder, header, height=960, width=1280): labelsGeneral = createLabelsWithInstance( builder, ["GeneralLabel" + str(i) for i in range(NUM_GENERAL_LABELS)], - ["InstanceUuid" + str(i) for i in range(NUM_GENERAL_LABELS)], + [str(uuid.uuid4()) for _ in range(NUM_GENERAL_LABELS)], ) labelsGeneralVector = addToGeneralLabelsVector(builder, labelsGeneral) @@ -56,7 +57,7 @@ def createPointCloud(builder, header, height=960, width=1280): labelWithInstances = createLabelsWithInstance( builder, ["BoundingBoxLabel" + str(i) for i in range(NUM_BB_LABELS)], - ["InstanceUuid" + str(i) for i in range(NUM_BB_LABELS)], + [str(uuid.uuid4()) for _ in range(NUM_BB_LABELS)], ) labelsBb = createBoundingBoxesLabeled(builder, labelWithInstances, boundingBoxes) labelsBbVector = addToBoundingBoxLabeledVector(builder, labelsBb) diff --git a/seerep-hdf5/seerep-hdf5-core/src/hdf5-core-point-cloud.cpp b/seerep-hdf5/seerep-hdf5-core/src/hdf5-core-point-cloud.cpp index 3526fe71e..0a3b9f171 100644 --- a/seerep-hdf5/seerep-hdf5-core/src/hdf5-core-point-cloud.cpp +++ b/seerep-hdf5/seerep-hdf5-core/src/hdf5-core-point-cloud.cpp @@ -19,14 +19,11 @@ std::optional Hdf5CorePointCloud::readDatase const std::scoped_lock lock(*m_write_mtx); std::string hdf5DatasetPath = HDF5_GROUP_POINTCLOUD + "/" + uuid; - std::string hdf5DatasetRawDataPath = hdf5DatasetPath + "/" + seerep_hdf5_core::Hdf5CorePointCloud::RAWDATA; - if (!m_file->exist(hdf5DatasetPath) || !m_file->exist(hdf5DatasetRawDataPath)) + if (!m_file->exist(hdf5DatasetPath)) return std::nullopt; std::shared_ptr group_ptr = std::make_shared(m_file->getGroup(hdf5DatasetPath)); - std::shared_ptr data_set_ptr = - std::make_shared(m_file->getDataSet(hdf5DatasetRawDataPath)); seerep_core_msgs::DatasetIndexable data; @@ -38,7 +35,7 @@ std::optional Hdf5CorePointCloud::readDatase group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEADER_STAMP_NANOS).read(data.header.timestamp.nanos); std::vector bb; - group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).write(bb); + group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX).read(bb); data.boundingbox.min_corner().set<0>(bb.at(0)); data.boundingbox.min_corner().set<1>(bb.at(1)); data.boundingbox.min_corner().set<2>(bb.at(2)); diff --git a/seerep-srv/seerep-core/src/core-project.cpp b/seerep-srv/seerep-core/src/core-project.cpp index b13b76aee..5d454ae32 100644 --- a/seerep-srv/seerep-core/src/core-project.cpp +++ b/seerep-srv/seerep-core/src/core-project.cpp @@ -110,7 +110,7 @@ void CoreProject::recreateDatatypes() for (auto datatypeName : datatypeNames) { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) - << "found datatype" << datatypeName << " in HDF5 file."; + << "found datatype " << datatypeName << " in HDF5 file."; } } diff --git a/seerep-srv/seerep-core/src/core.cpp b/seerep-srv/seerep-core/src/core.cpp index ba861cdc2..4e3163bee 100644 --- a/seerep-srv/seerep-core/src/core.cpp +++ b/seerep-srv/seerep-core/src/core.cpp @@ -68,8 +68,7 @@ void Core::recreateProjects() { if (entry.path().filename().extension() == ".h5") { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) - << "found " << entry.path().string() << " in HDF5 file."; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "found " << entry.path().string(); try { From e330e085c9bdaa7ba7957896280f238e50880486 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Thu, 22 Sep 2022 14:38:23 +0000 Subject: [PATCH 21/54] refactor hdf5 write of flatbuffer point clouds --- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 36 +- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 331 ++++++++---------- .../seerep-core-fb/src/core-fb-pointcloud.cpp | 2 +- 3 files changed, 178 insertions(+), 191 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index c732cdd0e..d55ecd4d5 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -25,9 +25,7 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral public: Hdf5FbPointCloud(std::shared_ptr& file, std::shared_ptr& write_mtx); - std::map getPointClouds(); - - std::shared_ptr writePointCloud2(const std::string& uuid, const seerep::fb::PointCloud2* pointcloud2); + void writePointCloud2(const std::string& uuid, const seerep::fb::PointCloud2& pointcloud2); std::optional> readPointCloud2(const std::string& uuid); @@ -90,14 +88,36 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral } template - bool sameVectorSize(T const& first, U const&... rest) + bool equalVectorLength(T const& first, U const&... rest) { return ((first.size() == rest.size()) && ...); } - CloudInfo getCloudInfo(const seerep::fb::PointCloud2& cloud); + template + void writePointFieldAttributes(HighFive::AnnotateTraits& object, + const flatbuffers::Vector>& pointFields) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "writing point field attributes to hdf5"; + + std::vector names; + std::vector offsets, counts; + std::vector datatypes; + + for (auto pointField : pointFields) + { + names.push_back(pointField->name()->str()); + offsets.push_back(pointField->offset()); + datatypes.push_back(static_cast(pointField->datatype())); + counts.push_back(pointField->count()); + } - CloudInfo getCloudInfo(const std::vector& names); + writeAttributeToHdf5>(object, FIELD_NAME, names); + writeAttributeToHdf5>(object, FIELD_OFFSET, offsets); + writeAttributeToHdf5>(object, FIELD_DATATYPE, datatypes); + writeAttributeToHdf5>(object, FIELD_COUNT, counts); + } + + CloudInfo getCloudInfo(const flatbuffers::Vector>& pointFields); void writePoints(const std::string& uuid, const std::shared_ptr& data_group_ptr, const seerep::fb::PointCloud2& cloud); @@ -109,10 +129,6 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral void writeOtherFields(const std::string& uuid, const seerep::fb::PointCloud2& cloud, const std::map& fields); - void - writePointFieldAttributes(HighFive::Group& cloud_group, - const flatbuffers::Vector>& vectorPointField); - void readPoints(const std::string& uuid, std::vector>>& pointData); void readColorsRGB(const std::string& uuid, std::vector>>& colorData); diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 63f5408d8..9d419a41a 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -4,117 +4,118 @@ namespace seerep_hdf5_fb { -Hdf5FbPointCloud::Hdf5FbPointCloud(std::shared_ptr& file, std::shared_ptr& write_mtx) - : Hdf5FbGeneral(file, write_mtx) +Hdf5FbPointCloud::Hdf5FbPointCloud(std::shared_ptr& file, std::shared_ptr& writeMtx) + : Hdf5FbGeneral(file, writeMtx) { } -std::map Hdf5FbPointCloud::getPointClouds() +void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb::PointCloud2& pointcloud2) { const std::scoped_lock lock(*m_write_mtx); - std::map map; - if (m_file->exist(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD)) + std::string hdf5GroupPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "writing flatbuffers point cloud to: " << hdf5GroupPath; + + std::shared_ptr dataGroupPtr; + + try { - const HighFive::Group& clouds_group = m_file->getGroup(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD); - for (std::string& cloud_name : clouds_group.listObjectNames()) - { - map.insert(std::pair(cloud_name, clouds_group.getGroup(cloud_name))); - } + checkExists(hdf5GroupPath); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) + << "hdf5 group" << hdf5GroupPath << " already exists!"; + dataGroupPtr = std::make_shared(m_file->getGroup(hdf5GroupPath)); + } + catch (std::invalid_argument const& e) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) + << "hdf5 group " << hdf5GroupPath << " does not exist! Creating a new group"; + dataGroupPtr = std::make_shared(m_file->createGroup(hdf5GroupPath)); } - return map; -} -std::shared_ptr Hdf5FbPointCloud::writePointCloud2(const std::string& uuid, - const seerep::fb::PointCloud2* pointcloud2) -{ - const std::scoped_lock lock(*m_write_mtx); + writeAttributeToHdf5(*dataGroupPtr, HEIGHT, pointcloud2.height()); + writeAttributeToHdf5(*dataGroupPtr, WIDTH, pointcloud2.width()); + writeAttributeToHdf5(*dataGroupPtr, IS_BIGENDIAN, pointcloud2.is_bigendian()); + writeAttributeToHdf5(*dataGroupPtr, POINT_STEP, pointcloud2.point_step()); + writeAttributeToHdf5(*dataGroupPtr, ROW_STEP, pointcloud2.row_step()); + writeAttributeToHdf5(*dataGroupPtr, IS_DENSE, pointcloud2.is_dense()); - std::string cloud_group_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid; + writeHeaderAttributes(*dataGroupPtr, *pointcloud2.header()); - std::shared_ptr data_group_ptr; + writePointFieldAttributes(*dataGroupPtr, *pointcloud2.fields()); - if (!m_file->exist(cloud_group_id)) + if (pointcloud2.labels_bb() != nullptr) { - data_group_ptr = std::make_shared(m_file->createGroup(cloud_group_id)); - data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT, pointcloud2->height()); - data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH, pointcloud2->width()); - data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN, pointcloud2->is_bigendian()); - data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP, pointcloud2->point_step()); - data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP, pointcloud2->row_step()); - data_group_ptr->createAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE, pointcloud2->is_dense()); + writeBoundingBoxLabeled(HDF5_GROUP_POINTCLOUD, id, *pointcloud2.labels_bb()); } - else + + if (pointcloud2.labels_general() != nullptr) { - data_group_ptr = std::make_shared(m_file->getGroup(cloud_group_id)); - data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT).write(pointcloud2->height()); - data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::WIDTH).write(pointcloud2->width()); - data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN).write(pointcloud2->is_bigendian()); - data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP).write(pointcloud2->point_step()); - data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP).write(pointcloud2->row_step()); - data_group_ptr->getAttribute(seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE).write(pointcloud2->is_dense()); + writeLabelsGeneral(HDF5_GROUP_POINTCLOUD, id, *pointcloud2.labels_general()); } - writePointFieldAttributes(*data_group_ptr, *pointcloud2->fields()); - writeHeaderAttributes(*data_group_ptr, *pointcloud2->header()); - - if (flatbuffers::IsFieldPresent(pointcloud2, seerep::fb::PointCloud2::VT_LABELS_GENERAL)) - writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, - *pointcloud2->labels_general()); - if (flatbuffers::IsFieldPresent(pointcloud2, seerep::fb::PointCloud2::VT_LABELS_BB)) - writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, uuid, - *pointcloud2->labels_bb()); - - CloudInfo info = getCloudInfo(*pointcloud2); + CloudInfo info = getCloudInfo(*pointcloud2.fields()); if (info.has_points) - writePoints(uuid, data_group_ptr, *pointcloud2); + writePoints(id, dataGroupPtr, pointcloud2); if (info.has_rgb) - writeColorsRGB(uuid, *pointcloud2); + writeColorsRGB(id, pointcloud2); if (info.has_rgba) - writeColorsRGBA(uuid, *pointcloud2); + writeColorsRGBA(id, pointcloud2); // TODO normals - if (!info.other_fields.empty()) - writeOtherFields(uuid, *pointcloud2, info.other_fields); + + // TODO other fields + // if (!info.other_fields.empty()) + // writeOtherFields(uuid, *pointcloud2, info.other_fields); m_file->flush(); - return data_group_ptr; } -void Hdf5FbPointCloud::writePoints(const std::string& uuid, const std::shared_ptr& data_group_ptr, +void Hdf5FbPointCloud::writePoints(const std::string& id, const std::shared_ptr& dataGroupPtr, const seerep::fb::PointCloud2& cloud) { - std::string points_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; - HighFive::DataSpace data_space({ cloud.height(), cloud.width(), 3 }); + std::string hdf5PointsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/points"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "writing dataset to: " << hdf5PointsPath; - std::shared_ptr points_dataset_ptr; - if (!m_file->exist(points_id)) - points_dataset_ptr = std::make_shared(m_file->createDataSet(points_id, data_space)); - else - points_dataset_ptr = std::make_shared(m_file->getDataSet(points_id)); + HighFive::DataSpace dataSpace({ cloud.height(), cloud.width(), 3 }); + std::shared_ptr pointsDatasetPtr; - std::vector>> point_data; - point_data.resize(cloud.height()); + try + { + checkExists(hdf5PointsPath); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) + << "hdf5 dataset: " << hdf5PointsPath << " already exists!"; + pointsDatasetPtr = std::make_shared(m_file->getDataSet(hdf5PointsPath)); + } + catch (std::invalid_argument const& e) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) + << "hdf5 dataset: " << hdf5PointsPath << " does not exist! Creating a new dataset"; + pointsDatasetPtr = std::make_shared(m_file->createDataSet(hdf5PointsPath, dataSpace)); + } - seerep_hdf5_fb::PointCloud2ConstIterator x_iter(cloud, "x"); - seerep_hdf5_fb::PointCloud2ConstIterator y_iter(cloud, "y"); - seerep_hdf5_fb::PointCloud2ConstIterator z_iter(cloud, "z"); + std::vector>> pointData; + pointData.resize(cloud.height()); std::array min, max; min[0] = min[1] = min[2] = std::numeric_limits::max(); max[0] = max[1] = max[2] = std::numeric_limits::min(); - for (unsigned int i = 0; i < cloud.height(); i++) + seerep_hdf5_fb::PointCloud2ConstIterator xIter(cloud, "x"); + seerep_hdf5_fb::PointCloud2ConstIterator yIter(cloud, "y"); + seerep_hdf5_fb::PointCloud2ConstIterator zIter(cloud, "z"); + + for (size_t row = 0; row < cloud.height(); row++) { - point_data[i].reserve(cloud.width()); - for (unsigned int j = 0; j < cloud.width(); j++) + pointData[row].reserve(cloud.width()); + for (size_t col = 0; col < cloud.width(); col++) { - const float& x = *x_iter; - const float& y = *y_iter; - const float& z = *z_iter; + const float& x = *xIter; + const float& y = *yIter; + const float& z = *zIter; - // compute bounding box + // compute bounding box for indicies at the same time if (x < min[0]) min[0] = x; if (x > max[0]) @@ -130,85 +131,108 @@ void Hdf5FbPointCloud::writePoints(const std::string& uuid, const std::shared_pt if (z > max[2]) max[2] = z; - point_data[i].push_back(std::vector{ x, y, z }); + pointData[row].push_back(std::vector{ x, y, z }); - ++x_iter, ++y_iter, ++z_iter; + ++xIter, ++yIter, ++zIter; } } - // write bounding box as attribute to dataset - const std::vector boundingbox{ min[0], min[1], min[2], max[0], max[1], max[2] }; + // min (x, y, z), max (x,y,z) + const std::vector boundingBox{ min[0], min[1], min[2], max[0], max[1], max[2] }; - writeAttributeToHdf5(*data_group_ptr, seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, boundingbox); + // write bounding box as data group attribute + writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, boundingBox); - // write data to dataset - points_dataset_ptr->write(point_data); + // write points to dataset + pointsDatasetPtr->write(pointData); } void Hdf5FbPointCloud::writeColorsRGB(const std::string& uuid, const seerep::fb::PointCloud2& cloud) { - const std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; - HighFive::DataSpace data_space({ cloud.height(), cloud.width(), 3 }); + const std::string hdf5ColorsPath = HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "writing dataset to: " << hdf5ColorsPath; - std::shared_ptr colors_dataset_ptr; - if (!m_file->exist(colors_id)) - colors_dataset_ptr = std::make_shared(m_file->createDataSet(colors_id, data_space)); - else - colors_dataset_ptr = std::make_shared(m_file->getDataSet(colors_id)); + HighFive::DataSpace dataSpace({ cloud.height(), cloud.width(), 3 }); + std::shared_ptr colorsDatasetPtr; + + try + { + checkExists(hdf5ColorsPath); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) + << "hdf5 dataset: " << hdf5ColorsPath << " already exists!"; + colorsDatasetPtr = std::make_shared(m_file->getDataSet(hdf5ColorsPath)); + } + catch (std::invalid_argument const& e) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) + << "hdf5 dataset: " << hdf5ColorsPath << " does not exist! Creating a new dataset"; + colorsDatasetPtr = std::make_shared(m_file->createDataSet(hdf5ColorsPath, dataSpace)); + } - std::vector>> colors_data; - colors_data.resize(cloud.height()); + std::vector>> colorsData; + colorsData.resize(cloud.height()); - seerep_hdf5_fb::PointCloud2ConstIterator r_iter(cloud, "r"); - seerep_hdf5_fb::PointCloud2ConstIterator g_iter(cloud, "g"); - seerep_hdf5_fb::PointCloud2ConstIterator b_iter(cloud, "b"); + seerep_hdf5_fb::PointCloud2ConstIterator rIter(cloud, "r"); + seerep_hdf5_fb::PointCloud2ConstIterator gIter(cloud, "g"); + seerep_hdf5_fb::PointCloud2ConstIterator bIter(cloud, "b"); - for (unsigned int i = 0; i < cloud.height(); i++) + for (size_t row = 0; row < cloud.height(); row++) { - colors_data[i].reserve(cloud.width()); - for (unsigned int j = 0; j < cloud.width(); j++) + colorsData[row].reserve(cloud.width()); + for (size_t col = 0; col < cloud.width(); col++) { - colors_data[i].push_back(std::vector{ *r_iter, *g_iter, *b_iter }); - ++r_iter, ++g_iter, ++b_iter; + colorsData[row].push_back(std::vector{ *rIter, *gIter, *bIter }); + ++rIter, ++gIter, ++bIter; } } - colors_dataset_ptr->write(colors_data); + colorsDatasetPtr->write(colorsData); } void Hdf5FbPointCloud::writeColorsRGBA(const std::string& uuid, const seerep::fb::PointCloud2& cloud) { - const std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; - HighFive::DataSpace data_space({ cloud.height(), cloud.width(), 4 }); + const std::string hdf5ColorsPath = HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "writing dataset to: " << hdf5ColorsPath; - std::shared_ptr colors_dataset_ptr; - if (!m_file->exist(colors_id)) - colors_dataset_ptr = std::make_shared(m_file->createDataSet(colors_id, data_space)); - else - colors_dataset_ptr = std::make_shared(m_file->getDataSet(colors_id)); + HighFive::DataSpace dataSpace({ cloud.height(), cloud.width(), 4 }); - std::vector>> colors_data; - colors_data.resize(cloud.height()); + std::shared_ptr colorsDatasetPtr; + try + { + checkExists(hdf5ColorsPath); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) + << "hdf5 dataset: " << hdf5ColorsPath << " already exists!"; + colorsDatasetPtr = std::make_shared(m_file->getDataSet(hdf5ColorsPath)); + } + catch (std::invalid_argument const& e) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) + << "hdf5 dataset: " << hdf5ColorsPath << " does not exist! Creating a new dataset"; + colorsDatasetPtr = std::make_shared(m_file->createDataSet(hdf5ColorsPath, dataSpace)); + } - seerep_hdf5_fb::PointCloud2ConstIterator r_iter(cloud, "r"); - seerep_hdf5_fb::PointCloud2ConstIterator g_iter(cloud, "g"); - seerep_hdf5_fb::PointCloud2ConstIterator b_iter(cloud, "b"); - seerep_hdf5_fb::PointCloud2ConstIterator a_iter(cloud, "a"); + std::vector>> colorsData; + colorsData.resize(cloud.height()); - for (unsigned int i = 0; i < cloud.height(); i++) + seerep_hdf5_fb::PointCloud2ConstIterator rIter(cloud, "r"); + seerep_hdf5_fb::PointCloud2ConstIterator gIter(cloud, "g"); + seerep_hdf5_fb::PointCloud2ConstIterator bIter(cloud, "b"); + seerep_hdf5_fb::PointCloud2ConstIterator aIter(cloud, "a"); + + for (size_t row = 0; row < cloud.height(); row++) { - colors_data[i].reserve(cloud.width()); - for (unsigned int j = 0; j < cloud.width(); j++) + colorsData[row].reserve(cloud.width()); + for (size_t col = 0; col < cloud.width(); col++) { - colors_data[i].push_back(std::vector{ *r_iter, *g_iter, *b_iter, *a_iter }); - ++r_iter; - ++g_iter; - ++b_iter; - ++a_iter; + colorsData[row].push_back(std::vector{ *rIter, *gIter, *bIter, *aIter }); + ++rIter; + ++gIter; + ++bIter; + ++aIter; } } - colors_dataset_ptr->write(colors_data); + colorsDatasetPtr->write(colorsData); } void Hdf5FbPointCloud::writeOtherFields(const std::string& uuid, const seerep::fb::PointCloud2& cloud, @@ -251,72 +275,17 @@ void Hdf5FbPointCloud::writeOtherFields(const std::string& uuid, const seerep::f } } -void Hdf5FbPointCloud::writePointFieldAttributes( - HighFive::Group& cloud_group, - const flatbuffers::Vector>& vectorPointField) -{ - std::vector names; - std::vector offsets, counts; - std::vector datatypes; - for (size_t i = 0; i < vectorPointField.size(); i++) - { - names.push_back(vectorPointField.Get(i)->name()->str()); - offsets.push_back(vectorPointField.Get(i)->offset()); - datatypes.push_back(static_cast(vectorPointField.Get(i)->datatype())); - counts.push_back(vectorPointField.Get(i)->count()); - } - writeAttributeToHdf5>(cloud_group, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME, names); - - writeAttributeToHdf5>(cloud_group, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET, offsets); - - writeAttributeToHdf5>(cloud_group, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_DATATYPE, - datatypes); - - writeAttributeToHdf5>(cloud_group, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_COUNT, counts); -} - -Hdf5FbPointCloud::CloudInfo Hdf5FbPointCloud::getCloudInfo(const seerep::fb::PointCloud2& cloud) -{ - bool hasFieldx = false; - bool hasFieldy = false; - bool hasFieldz = false; - - CloudInfo info; - for (size_t i = 0; i < cloud.fields()->size(); i++) - { - const std::string fieldName = cloud.fields()->Get(i)->name()->str(); - if (fieldName == "x") - hasFieldx = true; - else if (fieldName == "y") - hasFieldy = true; - else if (fieldName == "z") - hasFieldz = true; - else if (fieldName == "rgb") - info.has_rgb = true; - else if (fieldName == "rgba") - info.has_rgba = true; - else if (fieldName.find("normal") == 0) - info.has_normals = true; - else - { - info.other_fields[fieldName] = (struct PointFieldInfo){ .datatype = cloud.fields()->Get(i)->datatype(), - .count = cloud.fields()->Get(i)->count() }; - } - } - if (hasFieldx && hasFieldy && hasFieldz) - info.has_points = true; - return info; -} - -Hdf5FbPointCloud::CloudInfo Hdf5FbPointCloud::getCloudInfo(const std::vector& names) +Hdf5FbPointCloud::CloudInfo +Hdf5FbPointCloud::getCloudInfo(const flatbuffers::Vector>& pointFields) { bool hasFieldx = false; bool hasFieldy = false; bool hasFieldz = false; CloudInfo info; - for (auto fieldName : names) + for (size_t i = 0; i < pointFields.size(); i++) { + const std::string& fieldName = pointFields.Get(i)->name()->str(); if (fieldName == "x") hasFieldx = true; else if (fieldName == "y") @@ -401,7 +370,7 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) return std::nullopt; } - if (!sameVectorSize(names, offsets, counts, datatypes)) + if (!equalVectorLength(names, offsets, counts, datatypes)) { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << "number of read point field attributes does not match"; @@ -421,8 +390,10 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) } auto pointFieldsVector = builder.CreateVector(pointFields); - // combine read points fields - CloudInfo info = getCloudInfo(names); + // Note: Not pretty, but with this way both read and write use the same method + // for retrieving information about the point cloud + CloudInfo info = getCloudInfo(*reinterpret_cast>*>( + builder.GetCurrentBufferPointer())); std::vector>> points; std::vector>> pointsRGB; diff --git a/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp index 6b862032c..e1cf36309 100644 --- a/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp +++ b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp @@ -43,7 +43,7 @@ boost::uuids::uuid CoreFbPointCloud::addData(const seerep::fb::PointCloud2& pc) seerep_core_msgs::DatasetIndexable dataForIndices = CoreFbConversion::fromFb(pc); auto hdf5io = CoreFbGeneral::getHdf5(dataForIndices.header.uuidProject, m_seerepCore, m_hdf5IoMap); - hdf5io->writePointCloud2(boost::lexical_cast(dataForIndices.header.uuidData), &pc); + hdf5io->writePointCloud2(boost::lexical_cast(dataForIndices.header.uuidData), pc); m_seerepCore->addDataset(dataForIndices); From 78e135167fd96408091efc90f3a53a2987c4d53f Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Fri, 23 Sep 2022 13:56:35 +0000 Subject: [PATCH 22/54] add logging into hdf5 fb read of point cloud --- .../include/seerep-hdf5-fb/hdf5-fb-general.h | 7 +- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 9 +- .../seerep-hdf5-fb/impl/hdf5-fb-general.hpp | 8 +- .../seerep-hdf5-fb/src/hdf5-fb-image.cpp | 2 +- .../seerep-hdf5-fb/src/hdf5-fb-point.cpp | 2 +- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 119 ++++++++++-------- 6 files changed, 86 insertions(+), 61 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h index 6a90c5a18..8f2a02442 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h @@ -46,10 +46,11 @@ class Hdf5FbGeneral : public seerep_hdf5_core::Hdf5CoreGeneral void writeHeaderAttributes(HighFive::AnnotateTraits& object, const seerep::fb::Header& header); template - flatbuffers::Offset readHeaderAttributes(HighFive::AnnotateTraits& object, std::string uuidMsg, - flatbuffers::grpc::MessageBuilder& builder); + flatbuffers::Offset readHeaderAttributes(flatbuffers::grpc::MessageBuilder& builder, + HighFive::AnnotateTraits& object, + std::string uuidMsg); - //################ + //################s // BoundingBoxes //################ void writeBoundingBoxLabeled( diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index d55ecd4d5..3605f8988 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -53,10 +53,15 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral }; template - void read(const std::string cloud_uuid, const std::string& field_name, std::vector& data, size_t size) + void read(const std::string& id, const std::string& fieldName, std::vector& data, size_t size) { - const std::string id = seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX + "/" + cloud_uuid + "/" + field_name; + const std::string hdf5DatasetFieldPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/" + fieldName; + + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "reading " << fieldName << " from: " << hdf5DatasetFieldPath; + HighFive::DataSet dataset = m_file->getDataSet(id); + data.reserve(size); dataset.read(data); } diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-general.hpp b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-general.hpp index 62aa54210..e64587589 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-general.hpp +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-general.hpp @@ -59,10 +59,12 @@ void Hdf5FbGeneral::writeHeaderAttributes(HighFive::AnnotateTraits& object, c } template -flatbuffers::Offset Hdf5FbGeneral::readHeaderAttributes(HighFive::AnnotateTraits& object, - std::string uuidMsg, - flatbuffers::grpc::MessageBuilder& builder) +flatbuffers::Offset Hdf5FbGeneral::readHeaderAttributes(flatbuffers::grpc::MessageBuilder& builder, + HighFive::AnnotateTraits& object, + std::string uuidMsg) { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "loading flatbuffers header attributes"; + int64_t seconds; int32_t nanos; uint32_t seq; diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp index 9476ef5c4..0ff758bc4 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp @@ -164,7 +164,7 @@ std::optional> Hdf5FbImage::readIm { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "NOT loading the data. Just Meta-Data."; } - auto headerOffset = readHeaderAttributes(*data_set_ptr, id, builder); + auto headerOffset = readHeaderAttributes(builder, *data_set_ptr, id); std::vector boundingBoxesLabels; std::vector> boundingBoxes; diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp index 385c48ce1..00889bd1d 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp @@ -99,7 +99,7 @@ std::optional> Hdf5FbPoint: pointBuilder.add_z(read_data.at(2)); auto pointOffset = pointBuilder.Finish(); - auto headerOffset = readHeaderAttributes(*data_set_ptr, id, builder); + auto headerOffset = readHeaderAttributes(builder, *data_set_ptr, id); std::vector labelsGeneral; std::vector labelsGeneralInstances; diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 9d419a41a..9a8027389 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -66,6 +66,7 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: // TODO normals // TODO other fields + // if (!info.other_fields.empty()) // writeOtherFields(uuid, *pointcloud2, info.other_fields); @@ -314,42 +315,50 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) { const std::scoped_lock lock(*m_write_mtx); - const std::string hdf5DatasetPath = HDF5_GROUP_POINTCLOUD + "/" + id; + const std::string hdf5GroupPath = HDF5_GROUP_POINTCLOUD + "/" + id; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "loading flatbuffers point cloud from: " << hdf5GroupPath; + + std::shared_ptr dataGroupPtr; - // check if the point cloud exists - if (!m_file->exist(hdf5DatasetPath)) + try { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << hdf5DatasetPath << "does not exist"; + checkExists(hdf5GroupPath); + dataGroupPtr = std::make_shared(m_file->getGroup(hdf5GroupPath)); + } + catch (std::invalid_argument const& e) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) + << "hdf5 group " << hdf5GroupPath << " does not exist!"; return std::nullopt; } - std::shared_ptr data_group_ptr = - std::make_shared(m_file->getGroup(hdf5DatasetPath)); - flatbuffers::grpc::MessageBuilder builder; - // read general attributes - uint32_t height, width, point_step, row_step; - bool is_bigendian, is_dense; + // read general attributes from data group + uint32_t height, width, pointStep, rowStep; + bool isBigendian, isDense; try { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "loading general attributes"; - height = readAttributeFromHdf5(id, *data_group_ptr, HEIGHT); - width = readAttributeFromHdf5(id, *data_group_ptr, WIDTH); - point_step = readAttributeFromHdf5(id, *data_group_ptr, POINT_STEP); - row_step = readAttributeFromHdf5(id, *data_group_ptr, ROW_STEP); - is_bigendian = readAttributeFromHdf5(id, *data_group_ptr, IS_BIGENDIAN); - is_dense = readAttributeFromHdf5(id, *data_group_ptr, IS_DENSE); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "loading general attributes of : " << hdf5GroupPath; + height = readAttributeFromHdf5(id, *dataGroupPtr, HEIGHT); + width = readAttributeFromHdf5(id, *dataGroupPtr, WIDTH); + pointStep = readAttributeFromHdf5(id, *dataGroupPtr, POINT_STEP); + rowStep = readAttributeFromHdf5(id, *dataGroupPtr, ROW_STEP); + isBigendian = readAttributeFromHdf5(id, *dataGroupPtr, IS_BIGENDIAN); + isDense = readAttributeFromHdf5(id, *dataGroupPtr, IS_DENSE); } catch (const std::invalid_argument& e) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << "error: " << e.what(); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) + << "error loading general attributes of: " << hdf5GroupPath; return std::nullopt; } - // read header attributes - auto headerAttributes = readHeaderAttributes(*data_group_ptr, id, builder); + // reads and creates a flatbuffers header with offset + auto headerAttributesOffset = readHeaderAttributes(builder, *dataGroupPtr, id); // read point field attributes std::vector names; @@ -358,22 +367,24 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) try { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "loading point field attributes"; - names = readAttributeFromHdf5>(id, *data_group_ptr, FIELD_NAME); - offsets = readAttributeFromHdf5>(id, *data_group_ptr, FIELD_OFFSET); - counts = readAttributeFromHdf5>(id, *data_group_ptr, FIELD_COUNT); - datatypes = readAttributeFromHdf5>(id, *data_group_ptr, FIELD_DATATYPE); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "loading point field attributes of: " << hdf5GroupPath; + names = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_NAME); + offsets = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_OFFSET); + counts = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_COUNT); + datatypes = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_DATATYPE); } catch (const std::invalid_argument& e) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << "error: " << e.what(); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) + << "error loading point field attributes of: " << hdf5GroupPath; return std::nullopt; } if (!equalVectorLength(names, offsets, counts, datatypes)) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) - << "number of read point field attributes does not match"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) + << "unequal number of point field attributes: " << hdf5GroupPath; return std::nullopt; } @@ -390,11 +401,12 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) } auto pointFieldsVector = builder.CreateVector(pointFields); - // Note: Not pretty, but with this way both read and write use the same method - // for retrieving information about the point cloud CloudInfo info = getCloudInfo(*reinterpret_cast>*>( builder.GetCurrentBufferPointer())); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "reading point cloud data of: " << hdf5GroupPath; + std::vector>> points; std::vector>> pointsRGB; std::vector>> pointsRGBA; @@ -409,22 +421,24 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) if (info.has_rgba) readColorsRGBA(id, pointsRGBA); - if (!info.other_fields.empty()) - readOtherFields(id, info.other_fields, otherFields); + // TODO add other fields + + // if (!info.other_fields.empty()) + // readOtherFields(id, info.other_fields, otherFields); // TODO add normals std::vector data; if (!points.empty()) { - // converting the point cloud data into a 1D array - data.reserve(height * width); + data.reserve(height * width * pointStep); for (size_t row = 0; row < height; row++) { for (size_t col = 0; col < width; col++) { data.insert(data.end(), std::make_move_iterator(points.at(row).at(col).begin()), std::make_move_iterator(points.at(row).at(col).end())); + if (info.has_rgb) { data.insert(data.end(), std::make_move_iterator(pointsRGB.at(row).at(col).begin()), @@ -435,11 +449,11 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) data.insert(data.end(), std::make_move_iterator(pointsRGBA.at(row).at(col).begin()), std::make_move_iterator(pointsRGBA.at(row).at(col).end())); } - // TODO add support for other fields like laser scanner info + // TODO add support for other fields } } } - int vectorSize = sizeof(std::vector) + (sizeof(uint8_t) * data.size()); + auto dataVector = builder.CreateVector(data); // read labeled bounding box @@ -500,15 +514,15 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) // build the point cloud message seerep::fb::PointCloud2Builder pointCloudBuilder(builder); - pointCloudBuilder.add_header(headerAttributes); + pointCloudBuilder.add_header(headerAttributesOffset); pointCloudBuilder.add_height(height); pointCloudBuilder.add_width(width); pointCloudBuilder.add_fields(pointFieldsVector); - pointCloudBuilder.add_is_bigendian(is_bigendian); - pointCloudBuilder.add_point_step(point_step); - pointCloudBuilder.add_row_step(row_step); + pointCloudBuilder.add_is_bigendian(isBigendian); + pointCloudBuilder.add_point_step(pointStep); + pointCloudBuilder.add_row_step(rowStep); pointCloudBuilder.add_data(dataVector); - pointCloudBuilder.add_is_dense(is_dense); + pointCloudBuilder.add_is_dense(isDense); pointCloudBuilder.add_labels_general(labelsGeneralVector); pointCloudBuilder.add_labels_bb(boundingBoxLabeledVector); auto pointCloudOffset = pointCloudBuilder.Finish(); @@ -520,33 +534,36 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) void Hdf5FbPointCloud::readPoints(const std::string& uuid, std::vector>>& pointData) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "reading points from hdf5 file"; + const std::string hdf5DatasetPointsPath = HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; - std::string hdf5DatasetPointsPath = - seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "reading points from: " << hdf5DatasetPointsPath; HighFive::DataSet pointsDataset = m_file->getDataSet(hdf5DatasetPointsPath); + pointsDataset.read(pointData); } void Hdf5FbPointCloud::readColorsRGB(const std::string& uuid, std::vector>>& colorData) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "reading rgb from hdf5 file "; + const std::string hdf5DatasetColorsPath = HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "reading rgb from: " << hdf5DatasetColorsPath; - std::string colorsId = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; - HighFive::DataSet colorsDataset = m_file->getDataSet(colorsId); + HighFive::DataSet colorsDataset = m_file->getDataSet(hdf5DatasetColorsPath); colorsDataset.read(colorData); } void Hdf5FbPointCloud::readColorsRGBA(const std::string& uuid, std::vector>>& colorData) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "reading rgba from hdf5 file "; + const std::string hdf5DatasetColorsPath = HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; - std::string colors_id = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; - HighFive::DataSet colors_dataset = m_file->getDataSet(colors_id); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "reading rgba from: " << hdf5DatasetColorsPath; - colors_dataset.read(colorData); + HighFive::DataSet colorsDataset = m_file->getDataSet(hdf5DatasetColorsPath); + + colorsDataset.read(colorData); } void Hdf5FbPointCloud::readOtherFields(const std::string& uuid, const std::map& fields, From 5e6d647e5c339b3e8a664fdfbe016d358a5abcd4 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Fri, 23 Sep 2022 14:46:37 +0000 Subject: [PATCH 23/54] update logging in flatbuffers point cloud service --- .../src/fb-point-cloud-service.cpp | 46 ++++++++++++------- 1 file changed, 29 insertions(+), 17 deletions(-) diff --git a/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp b/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp index e757b1891..cb8f8f52a 100644 --- a/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp +++ b/seerep-srv/seerep-server/src/fb-point-cloud-service.cpp @@ -12,31 +12,37 @@ FbPointCloudService::GetPointCloud2(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, grpc::ServerWriter>* writer) { + // ignore without causing a warning (void)context; + auto requestRoot = request->GetRoot(); + std::stringstream debuginfo; - debuginfo << "sending point clouds with this query parameters:"; + + debuginfo << "sending point clouds with this query parameters: "; if (requestRoot->boundingbox() != NULL) { - debuginfo << "bounding box min(" << requestRoot->boundingbox()->point_min()->x() << "/" - << requestRoot->boundingbox()->point_min()->y() << "/" << requestRoot->boundingbox()->point_min()->z() - << "), max(" << requestRoot->boundingbox()->point_max()->x() << "/" - << requestRoot->boundingbox()->point_max()->y() << "/" << requestRoot->boundingbox()->point_max()->z() - << ")"; + debuginfo << "\n bounding box min(x: " << requestRoot->boundingbox()->point_min()->x() + << ", y: " << requestRoot->boundingbox()->point_min()->y() + << ", z: " << requestRoot->boundingbox()->point_min()->z() + << "), max(x: " << requestRoot->boundingbox()->point_max()->x() + << ", y: " << requestRoot->boundingbox()->point_max()->y() + << ", z: " << requestRoot->boundingbox()->point_max()->z() << ")"; } if (requestRoot->timeinterval() != NULL) { - debuginfo << "time interval (" << requestRoot->timeinterval()->time_min()->seconds() << "/" - << requestRoot->timeinterval()->time_max()->seconds() << ")"; + debuginfo << "\n time interval (seconds since epoch: " << requestRoot->timeinterval()->time_min()->seconds() + << ", nanoseconds since epoch:" << requestRoot->timeinterval()->time_max()->seconds() << ")"; } if (requestRoot->label() != NULL) { - debuginfo << "labels general"; + debuginfo << "\n labels general:"; for (auto label : *requestRoot->label()) { - debuginfo << "'" << label->str() << "' "; + debuginfo << " '" << label->str() << "' "; } } + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << debuginfo.rdbuf(); try @@ -45,7 +51,7 @@ FbPointCloudService::GetPointCloud2(grpc::ServerContext* context, } catch (std::runtime_error const& e) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) << e.what(); return grpc::Status(grpc::StatusCode::INVALID_ARGUMENT, e.what()); } return grpc::Status::OK; @@ -55,16 +61,21 @@ grpc::Status FbPointCloudService::TransferPointCloud2( grpc::ServerContext* context, grpc::ServerReader>* reader, flatbuffers::grpc::Message* response) { - (void)context; // ignore - std::string answer = "everything stored!"; + // ignore without causing warning + (void)context; + + std::string answer = "Saved the flatbuffers point cloud message!"; flatbuffers::grpc::Message pointCloudMsg; + while (reader->Read(&pointCloudMsg)) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "received point cloud ..."; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "received flatbuffers point cloud"; + auto pointCloud = pointCloudMsg.GetRoot(); - std::string uuidProject = pointCloud->header()->uuid_project()->str(); + const std::string& uuidProject = pointCloud->header()->uuid_project()->str(); + if (!uuidProject.empty()) { try @@ -73,7 +84,7 @@ grpc::Status FbPointCloudService::TransferPointCloud2( } catch (std::runtime_error const& e) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << e.what(); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) << e.what(); seerep_server_util::createResponseFb(std::string(e.what()), seerep::fb::TRANSMISSION_STATE_FAILURE, response); @@ -82,7 +93,8 @@ grpc::Status FbPointCloudService::TransferPointCloud2( } else { - answer = "a msg had no project uuid!"; + answer = "No project uuid in the header of the received flatbuffers point cloud message!"; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) << answer; } } seerep_server_util::createResponseFb(answer, seerep::fb::TRANSMISSION_STATE_SUCCESS, response); From fa1116094c887dfa769024037509681e02d4222e Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Fri, 23 Sep 2022 15:53:52 +0000 Subject: [PATCH 24/54] update logging in core fb point cloud --- .../include/seerep-core-fb/core-fb-pointcloud.h | 4 ---- .../seerep-core-fb/src/core-fb-pointcloud.cpp | 14 +++++--------- 2 files changed, 5 insertions(+), 13 deletions(-) diff --git a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h index 4c89388a0..4048f7b21 100644 --- a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h +++ b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h @@ -1,9 +1,6 @@ #ifndef SEEREP_CORE_FB_POINTCLOUD_H_ #define SEEREP_CORE_FB_POINTCLOUD_H_ -#include -#include - #include "core-fb-conversion.h" #include "core-fb-general.h" @@ -34,7 +31,6 @@ class CoreFbPointCloud { public: CoreFbPointCloud(std::shared_ptr seerepCore); - ~CoreFbPointCloud(); void getData(const seerep::fb::Query* query, grpc::ServerWriter>* const writer); diff --git a/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp index e1cf36309..8668c4cc0 100644 --- a/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp +++ b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp @@ -7,14 +7,9 @@ CoreFbPointCloud::CoreFbPointCloud(std::shared_ptr seerepCore CoreFbGeneral::getAllFileAccessorFromCore(m_seerepCore, m_hdf5IoMap); } -CoreFbPointCloud::~CoreFbPointCloud() -{ -} - void CoreFbPointCloud::getData(const seerep::fb::Query* query, grpc::ServerWriter>* const writer) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) << "loading point cloud from points/" << std::endl; seerep_core_msgs::Query queryCore = seerep_core_fb::CoreFbConversion::fromFb(query, seerep_core_msgs::Datatype::PointCloud); @@ -22,16 +17,17 @@ void CoreFbPointCloud::getData(const seerep::fb::Query* query, for (auto project : resultCore.queryResultProjects) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) - << "sending point cloud from project" << boost::lexical_cast(project.projectUuid); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "sending flatbuffer point cloud from project: " << boost::lexical_cast(project.projectUuid); + for (auto uuidPc : project.dataOrInstanceUuids) { auto pc = CoreFbGeneral::getHdf5(project.projectUuid, m_seerepCore, m_hdf5IoMap) ->readPointCloud2(boost::lexical_cast(uuidPc)); if (pc) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::info) - << "sending point cloud " << boost::lexical_cast(uuidPc); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "sending flatbuffers point cloud with UUID: " << boost::lexical_cast(uuidPc); writer->Write(pc.value()); } } From 35bbc6272e8eef8c5e29cac81bf830507b66ec1e Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 4 Oct 2022 12:41:42 +0000 Subject: [PATCH 25/54] read and write of points into hdf5 for flatbuffer point clouds --- .../pointcloud/gRPC_fb_queryPointCloud.py | 9 +- .../gRPC/pointcloud/gRPC_fb_sendPointCloud.py | 11 +- .../hdf5-fb-point-cloud2-iterator.h | 88 +++------ .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 7 +- .../impl/hdf5-fb-point-cloud2-iterator.hpp | 55 +++--- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 178 +++++++++++++----- 6 files changed, 201 insertions(+), 147 deletions(-) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py index 832769165..f9d190e17 100644 --- a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py @@ -1,7 +1,9 @@ import os +import struct import sys import flatbuffers +import numpy as np from fb import point_cloud_service_grpc_fb as pointCloudService from fb.PointCloud2 import PointCloud2 @@ -77,5 +79,8 @@ print(f"Label {i}: {response.LabelsGeneral(i).Label().decode('utf-8')}") print(f"Instance {i}: {response.LabelsGeneral(i).InstanceUuid().decode('utf-8')}") - print("---Points---") - # TODO check if the read point cloud matches + print("---Data--") + rawData = response.DataAsNumpy() + data = [struct.unpack('f', rawData[i : i + 4]) for i in range(0, rawData.shape[0], 4)] + reshapedData = np.array(data).reshape(960, 1280, 4) + print(f"Data: {reshapedData}") diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py index bdb5535b3..af8e2dc01 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py @@ -63,8 +63,9 @@ def createPointCloud(builder, header, height=960, width=1280): labelsBbVector = addToBoundingBoxLabeledVector(builder, labelsBb) # create ordered point cloud with dim (height, width, 6) - points = np.random.randn(height, width, 6).astype(np.float32) - print(f"Data shape: {points.shape}") + # TODO rgb field should be int32 + points = np.random.randn(height, width, 4).astype(np.float32) + print(f"Data: {points}") pointsVector = builder.CreateByteVector(points.tobytes()) @@ -73,9 +74,9 @@ def createPointCloud(builder, header, height=960, width=1280): PointCloud2.AddHeader(builder, header) PointCloud2.AddHeight(builder, points.shape[0]) PointCloud2.AddWidth(builder, points.shape[1]) - PointCloud2.AddIsBigendian(builder, False) - PointCloud2.AddPointStep(builder, 24) - PointCloud2.AddRowStep(builder, points.shape[1] * 24) + PointCloud2.AddIsBigendian(builder, True) + PointCloud2.AddPointStep(builder, 16) + PointCloud2.AddRowStep(builder, points.shape[1] * 16) PointCloud2.AddFields(builder, pointFieldsVector) PointCloud2.AddData(builder, pointsVector) PointCloud2.AddLabelsGeneral(builder, labelsGeneralVector) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h index f476b3b24..02b68888d 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h @@ -175,23 +175,14 @@ namespace impl * T is the type of the value on which the child class will be templated * TT is the type of the value to be retrieved (same as T except for constness) * U is the type of the raw data in PointCloud2 (only uchar and const uchar are supported) - * C is the type of the pointcloud to intialize from (const or not) * V is the derived class (yop, curiously recurring template pattern) */ -template class V> +template class V> class PointCloud2IteratorBase { public: - /** - */ PointCloud2IteratorBase(); - /** - * @param cloud_msg The PointCloud2 to iterate upon - * @param field_name The field to iterate upon - */ - PointCloud2IteratorBase(C& cloud_msg, const std::string& field_name); - /** Assignment operator * @param iter the iterator to copy data from * @return a reference to *this @@ -236,14 +227,16 @@ class PointCloud2IteratorBase */ V end() const; + // TODO find common place for calculation of the offset + /** Common code to set the field of the PointCloud2 * @param cloud_msg the PointCloud2 to modify * @param field_name the name of the field to iterate upon * @return the offset at which the field is found */ - int set_field(const seerep::fb::PointCloud2& cloud_msg, const std::string& field_name); + uint32_t set_field(const seerep::fb::PointCloud2& cloud_msg, const std::string& field_name); - /** The "point_step" of the original cloud */ + /** The "point_step" of the point cloud */ int point_step_; /** The raw data in uchar* where the iterator is */ U* data_char_; @@ -257,74 +250,53 @@ class PointCloud2IteratorBase } // namespace impl /** - * @brief Class that can iterate over a PointCloud2 - * - * T type of the element being iterated upon - * E.g, you create your PointClou2 message as follows: - * @verbatim - * setPointCloud2FieldsByString(cloud_msg, 2, "xyz", "rgb"); - * @endverbatim - * - * For iterating over XYZ, you do : - * @verbatim - * seerep::PointCloud2Iterator iter_x(cloud_msg, "x"); - * @endverbatim - * and then access X through iter_x[0] or *iter_x - * You could create an iterator for Y and Z too but as they are consecutive, - * you can just use iter_x[1] and iter_x[2] - * - * For iterating over RGB, you do: - * @verbatim - * seerep::PointCloud2Iterator iter_rgb(cloud_msg, "rgb"); - * @endverbatim - * and then access R,G,B through iter_rgb[0], iter_rgb[1], iter_rgb[2] + * @brief Helper iterators for reading and writing the data field of a flatbuffers point cloud message */ template -class PointCloud2Iterator - : public impl::PointCloud2IteratorBase +class PointCloud2WriteIterator : public impl::PointCloud2IteratorBase { public: /** - * @brief Construct a new PointCloud2Const iterator based on a cloud message. + * @brief Iterator to use for iterating over an pre-allocated data field and writing the values * - * @param cloud_msg the cloud message to use - * @param field_name the field to iterate over + * @param data a pointer to the start of the data field + * @param offset the offset of the field that should be written + * @param size the length of the data field */ - PointCloud2Iterator(seerep::fb::PointCloud2& cloud_msg, const std::string& field_name) - : impl::PointCloud2IteratorBase::PointCloud2IteratorBase(cloud_msg, field_name) + PointCloud2WriteIterator(uint8_t* data, uint32_t offset, uint32_t size, int32_t pointStep) + : impl::PointCloud2IteratorBase::PointCloud2IteratorBase() { - int offset = this->set_field(cloud_msg, field_name); - this->data_char_ = reinterpret_cast(cloud_msg.mutable_data()->data() + offset); + uint32_t offset_ = offset; + this->point_step_ = pointStep; + this->data_char_ = reinterpret_cast(data + offset); this->data_ = reinterpret_cast(this->data_char_); - this->data_end_ = reinterpret_cast(cloud_msg.mutable_data()->data() + cloud_msg.data()->size() + offset); + this->data_end_ = reinterpret_cast(data + size + offset); } }; /** - * @brief Same as a PointCloud2Iterator but for const data + * @brief Reading iterator for the data field of a received flatbuffers point cloud message */ template -class PointCloud2ConstIterator - : public impl::PointCloud2IteratorBase +class PointCloud2ReadIterator + : public impl::PointCloud2IteratorBase { public: /** - * @brief Construct a new PointCloud2Const iterator based on a cloud message. + * @brief Iterator over the data field of a received flatbuffers point cloud message * - * @param cloud_msg the cloud message to use - * @param field_name the field to iterate over + * @param cloudMsg the flatbuffers point cloud message to use + * @param fieldName the field to iterate over */ - PointCloud2ConstIterator(const seerep::fb::PointCloud2& cloud_msg, const std::string& field_name) - : impl::PointCloud2IteratorBase::PointCloud2IteratorBase(cloud_msg, - field_name) + PointCloud2ReadIterator(const seerep::fb::PointCloud2& cloudMsg, const std::string& fieldName) + : impl::PointCloud2IteratorBase::PointCloud2IteratorBase() { - int offset = this->set_field(cloud_msg, field_name); - this->data_char_ = reinterpret_cast(cloud_msg.data()->data() + offset); + int offset = this->set_field(cloudMsg, fieldName); + this->data_char_ = reinterpret_cast(cloudMsg.data()->data() + offset); this->data_ = reinterpret_cast(this->data_char_); - this->data_end_ = reinterpret_cast(cloud_msg.data()->data() + cloud_msg.data()->size() + offset); + this->data_end_ = reinterpret_cast(cloudMsg.data()->data() + cloudMsg.data()->size() + offset); } }; } // namespace seerep_hdf5_fb diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index 3605f8988..a651497b8 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -79,7 +79,7 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral else dataset_ptr = std::make_shared(m_file->getDataSet(id)); - PointCloud2ConstIterator iter(cloud, field_name); + PointCloud2ReadIterator iter(cloud, field_name); std::vector data; data.reserve(size); @@ -134,7 +134,7 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral void writeOtherFields(const std::string& uuid, const seerep::fb::PointCloud2& cloud, const std::map& fields); - void readPoints(const std::string& uuid, std::vector>>& pointData); + void readPoints(const std::string& uuid, uint8_t* data); void readColorsRGB(const std::string& uuid, std::vector>>& colorData); @@ -143,6 +143,9 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral void readOtherFields(const std::string& uuid, const std::map& fields, std::vector otherFieldsData); + uint32_t getOffset(const std::vector& names, const std::vector& offsets, + const std::string& fieldName, bool isBigendian, uint32_t pointStep); + // pointcloud attribute keys inline static const std::string HEIGHT = "height"; inline static const std::string WIDTH = "width"; diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp index 674382b1c..56d405bf9 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp @@ -92,30 +92,17 @@ namespace seerep_hdf5_fb { namespace impl { -/** - */ -template class V> -PointCloud2IteratorBase::PointCloud2IteratorBase() : data_char_(0), data_(0), data_end_(0) -{ -} - -/** - * @param cloud_msg The PointCloud2 to iterate upon - * @param field_name The field to iterate upon - */ -template class V> -PointCloud2IteratorBase::PointCloud2IteratorBase(C& cloud_msg, const std::string& field_name) +template class V> +PointCloud2IteratorBase::PointCloud2IteratorBase() { - (void)cloud_msg; // ignore that variable without causing warnings - (void)field_name; // ignore that variable without causing warnings } /** Assignment operator * @param iter the iterator to copy data from * @return a reference to *this */ -template class V> -V& PointCloud2IteratorBase::operator=(const V& iter) +template class V> +V& PointCloud2IteratorBase::operator=(const V& iter) { if (this != &iter) { @@ -134,8 +121,8 @@ V& PointCloud2IteratorBase::operator=(const V& iter) * @param i * @return a reference to the i^th value from the current position */ -template class V> -TT& PointCloud2IteratorBase::operator[](size_t i) const +template class V> +TT& PointCloud2IteratorBase::operator[](size_t i) const { return *(data_ + i); } @@ -143,8 +130,8 @@ TT& PointCloud2IteratorBase::operator[](size_t i) const /** Dereference the iterator. Equivalent to accessing it through [0] * @return the value to which the iterator is pointing */ -template class V> -TT& PointCloud2IteratorBase::operator*() const +template class V> +TT& PointCloud2IteratorBase::operator*() const { return *data_; } @@ -152,8 +139,8 @@ TT& PointCloud2IteratorBase::operator*() const /** Increase the iterator to the next element * @return a reference to the updated iterator */ -template class V> -V& PointCloud2IteratorBase::operator++() +template class V> +V& PointCloud2IteratorBase::operator++() { data_char_ += point_step_; data_ = reinterpret_cast(data_char_); @@ -164,8 +151,8 @@ V& PointCloud2IteratorBase::operator++() * @param i the amount to increase the iterator by * @return an iterator with an increased position */ -template class V> -V PointCloud2IteratorBase::operator+(int i) +template class V> +V PointCloud2IteratorBase::operator+(int i) { V res = *static_cast*>(this); @@ -178,8 +165,8 @@ V PointCloud2IteratorBase::operator+(int i) /** Increase the iterator by a certain amount * @return a reference to the updated iterator */ -template class V> -V& PointCloud2IteratorBase::operator+=(int i) +template class V> +V& PointCloud2IteratorBase::operator+=(int i) { data_char_ += i * point_step_; data_ = reinterpret_cast(data_char_); @@ -189,8 +176,8 @@ V& PointCloud2IteratorBase::operator+=(int i) /** Compare to another iterator * @return whether the current iterator points to a different address than the other one */ -template class V> -bool PointCloud2IteratorBase::operator!=(const V& iter) const +template class V> +bool PointCloud2IteratorBase::operator!=(const V& iter) const { return iter.data_ != data_; } @@ -198,8 +185,8 @@ bool PointCloud2IteratorBase::operator!=(const V& iter) const /** Return the end iterator * @return the end iterator (useful when performing normal iterator processing with ++) */ -template class V> -V PointCloud2IteratorBase::end() const +template class V> +V PointCloud2IteratorBase::end() const { V res = *static_cast*>(this); res.data_ = data_end_; @@ -211,9 +198,9 @@ V PointCloud2IteratorBase::end() const * @param field_name the name of the field to iterate upon * @return the offset at which the field is found */ -template class V> -int PointCloud2IteratorBase::set_field(const seerep::fb::PointCloud2& cloud_msg, - const std::string& field_name) +template class V> +uint32_t PointCloud2IteratorBase::set_field(const seerep::fb::PointCloud2& cloud_msg, + const std::string& field_name) { is_bigendian_ = cloud_msg.is_bigendian(); point_step_ = cloud_msg.point_step(); diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 9a8027389..b528063a0 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -103,9 +103,9 @@ void Hdf5FbPointCloud::writePoints(const std::string& id, const std::shared_ptr< min[0] = min[1] = min[2] = std::numeric_limits::max(); max[0] = max[1] = max[2] = std::numeric_limits::min(); - seerep_hdf5_fb::PointCloud2ConstIterator xIter(cloud, "x"); - seerep_hdf5_fb::PointCloud2ConstIterator yIter(cloud, "y"); - seerep_hdf5_fb::PointCloud2ConstIterator zIter(cloud, "z"); + seerep_hdf5_fb::PointCloud2ReadIterator xIter(cloud, "x"); + seerep_hdf5_fb::PointCloud2ReadIterator yIter(cloud, "y"); + seerep_hdf5_fb::PointCloud2ReadIterator zIter(cloud, "z"); for (size_t row = 0; row < cloud.height(); row++) { @@ -173,9 +173,9 @@ void Hdf5FbPointCloud::writeColorsRGB(const std::string& uuid, const seerep::fb: std::vector>> colorsData; colorsData.resize(cloud.height()); - seerep_hdf5_fb::PointCloud2ConstIterator rIter(cloud, "r"); - seerep_hdf5_fb::PointCloud2ConstIterator gIter(cloud, "g"); - seerep_hdf5_fb::PointCloud2ConstIterator bIter(cloud, "b"); + seerep_hdf5_fb::PointCloud2ReadIterator rIter(cloud, "r"); + seerep_hdf5_fb::PointCloud2ReadIterator gIter(cloud, "g"); + seerep_hdf5_fb::PointCloud2ReadIterator bIter(cloud, "b"); for (size_t row = 0; row < cloud.height(); row++) { @@ -215,10 +215,10 @@ void Hdf5FbPointCloud::writeColorsRGBA(const std::string& uuid, const seerep::fb std::vector>> colorsData; colorsData.resize(cloud.height()); - seerep_hdf5_fb::PointCloud2ConstIterator rIter(cloud, "r"); - seerep_hdf5_fb::PointCloud2ConstIterator gIter(cloud, "g"); - seerep_hdf5_fb::PointCloud2ConstIterator bIter(cloud, "b"); - seerep_hdf5_fb::PointCloud2ConstIterator aIter(cloud, "a"); + seerep_hdf5_fb::PointCloud2ReadIterator rIter(cloud, "r"); + seerep_hdf5_fb::PointCloud2ReadIterator gIter(cloud, "g"); + seerep_hdf5_fb::PointCloud2ReadIterator bIter(cloud, "b"); + seerep_hdf5_fb::PointCloud2ReadIterator aIter(cloud, "a"); for (size_t row = 0; row < cloud.height(); row++) { @@ -309,6 +309,79 @@ Hdf5FbPointCloud::getCloudInfo(const flatbuffers::Vector& names, const std::vector& offsets, + const std::string& fieldName, bool isBigendian, uint32_t poinStep) +{ + for (size_t i = 0; i < names.size(); i++) + { + if (names[i] == fieldName) + { + return offsets[i]; + } + } + + // Handle the special case of rgb(a), since the rgb(a) channel is encoded in a single 32 Bit integer + + // Needs C++17 + std::set fieldNames = { "r", "g", "b", "a" }; + + if (fieldNames.find(fieldName) != fieldNames.end()) + { + for (size_t i = 0; i < names.size(); i++) + { + if (names[i] == "rgb" || names[i] == "rgba") + { + if (fieldName == "r") + { + if (isBigendian) + { + return offsets[i] + 1; + } + else + { + return offsets[i] + 2; + } + } + if (fieldName == "g") + { + if (isBigendian) + { + return offsets[i] + 2; + } + else + { + return offsets[i] + 1; + } + } + if (fieldName == "b") + { + if (isBigendian) + { + return offsets[i] + 3; + } + else + { + return offsets[i] + 0; + } + } + if (fieldName == "a") + { + if (isBigendian) + { + return offsets[i] + 0; + } + else + { + return offsets[i] + 3; + } + } + } + } + } + throw std::runtime_error("Field " + fieldName + " does not exist!"); +} + // TODO read partial point cloud, e.g. only xyz without color, etc. std::optional> Hdf5FbPointCloud::readPointCloud2(const std::string& id) @@ -407,54 +480,60 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "reading point cloud data of: " << hdf5GroupPath; - std::vector>> points; - std::vector>> pointsRGB; - std::vector>> pointsRGBA; - std::vector otherFields; + // pre allocate vector, to write values in it from the iterators + // should be way faster then copying it in vectors; + + // pointer to the data field of the pre-allocated array + uint8_t* data; + // allocate height * width * point setp bytes + auto vector = builder.CreateUninitializedVector(height * width * pointStep, sizeof(uint8_t), &data); if (info.has_points) - readPoints(id, points); + { + const std::string hdf5DatasetPointsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/points"; - if (info.has_rgb) - readColorsRGB(id, pointsRGB); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "reading points from: " << hdf5DatasetPointsPath; - if (info.has_rgba) - readColorsRGBA(id, pointsRGBA); + HighFive::DataSet pointsDataset = m_file->getDataSet(hdf5DatasetPointsPath); - // TODO add other fields + std::vector>> pointData; + pointsDataset.read(pointData); - // if (!info.other_fields.empty()) - // readOtherFields(id, info.other_fields, otherFields); + int xOffset = getOffset(names, offsets, "x", isBigendian, pointStep); + int yOffset = getOffset(names, offsets, "y", isBigendian, pointStep); + int zOffset = getOffset(names, offsets, "z", isBigendian, pointStep); - // TODO add normals + seerep_hdf5_fb::PointCloud2WriteIterator xIter(data, xOffset, height * width, pointStep); + seerep_hdf5_fb::PointCloud2WriteIterator yIter(data, yOffset, height * width, pointStep); + seerep_hdf5_fb::PointCloud2WriteIterator zIter(data, zOffset, height * width, pointStep); - std::vector data; - if (!points.empty()) - { - data.reserve(height * width * pointStep); - for (size_t row = 0; row < height; row++) + for (auto col : pointData) { - for (size_t col = 0; col < width; col++) + for (auto row : col) { - data.insert(data.end(), std::make_move_iterator(points.at(row).at(col).begin()), - std::make_move_iterator(points.at(row).at(col).end())); - - if (info.has_rgb) - { - data.insert(data.end(), std::make_move_iterator(pointsRGB.at(row).at(col).begin()), - std::make_move_iterator(pointsRGB.at(row).at(col).end())); - } - if (info.has_rgba) - { - data.insert(data.end(), std::make_move_iterator(pointsRGBA.at(row).at(col).begin()), - std::make_move_iterator(pointsRGBA.at(row).at(col).end())); - } - // TODO add support for other fields + *xIter = row[0]; + *yIter = row[1]; + *zIter = row[2]; + ++xIter, ++yIter, ++zIter; } } } - auto dataVector = builder.CreateVector(data); + // if (info.has_rgb + // readColorsRGB(id, pointsRGB); + + // if (info.has_rgba) + // readColorsRGBA(id, pointsRGBA); + + // // TODO add other fields + + // // if (!info.other_fields.empty()) + // // readOtherFields(id, info.other_fields, otherFields); + + // // TODO add normals + + // auto dataVector = builder.CreateVector(data); // read labeled bounding box std::vector boundingBoxesLabels; @@ -521,7 +600,7 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) pointCloudBuilder.add_is_bigendian(isBigendian); pointCloudBuilder.add_point_step(pointStep); pointCloudBuilder.add_row_step(rowStep); - pointCloudBuilder.add_data(dataVector); + pointCloudBuilder.add_data(vector); pointCloudBuilder.add_is_dense(isDense); pointCloudBuilder.add_labels_general(labelsGeneralVector); pointCloudBuilder.add_labels_bb(boundingBoxLabeledVector); @@ -532,7 +611,8 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) return grpcPointCloud; } -void Hdf5FbPointCloud::readPoints(const std::string& uuid, std::vector>>& pointData) +// TODO check if the copy into the tmp vector can be removed somehow +void Hdf5FbPointCloud::readPoints(const std::string& uuid, uint8_t* data) { const std::string hdf5DatasetPointsPath = HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; @@ -541,7 +621,13 @@ void Hdf5FbPointCloud::readPoints(const std::string& uuid, std::vectorgetDataSet(hdf5DatasetPointsPath); + std::vector>> pointData; pointsDataset.read(pointData); + + for (auto col : pointData) + { + for (auto row : col) {} + } } void Hdf5FbPointCloud::readColorsRGB(const std::string& uuid, std::vector>>& colorData) From fd7126d5709a8854d718fae018ab33b4b2de6833 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 4 Oct 2022 13:54:07 +0000 Subject: [PATCH 26/54] read and write of rgb(a) to hdf5 from flatbuffer point clouds --- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 90 +++++++++++++------ 1 file changed, 65 insertions(+), 25 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index b528063a0..a09bc51a1 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -520,11 +520,72 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) } } - // if (info.has_rgb - // readColorsRGB(id, pointsRGB); + if (info.has_rgb) + { + const std::string hdf5DatasetColorsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; + + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "reading rgb from: " << hdf5DatasetColorsPath; + + HighFive::DataSet colorsDataset = m_file->getDataSet(hdf5DatasetColorsPath); + + std::vector>> colorData; + colorsDataset.read(colorData); + + int rOffset = getOffset(names, offsets, "r", isBigendian, pointStep); + int gOffset = getOffset(names, offsets, "g", isBigendian, pointStep); + int bOffset = getOffset(names, offsets, "b", isBigendian, pointStep); + + seerep_hdf5_fb::PointCloud2WriteIterator rIter(data, rOffset, height * width, pointStep); + seerep_hdf5_fb::PointCloud2WriteIterator gIter(data, gOffset, height * width, pointStep); + seerep_hdf5_fb::PointCloud2WriteIterator bIter(data, bOffset, height * width, pointStep); + + for (auto col : colorData) + { + for (auto row : col) + { + *rIter = row[0]; + *gIter = row[1]; + *bIter = row[2]; + ++rIter, ++gIter, ++bIter; + } + } + } + + if (info.has_rgba) + { + const std::string hdf5DatasetColorsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; + + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "reading rgba from: " << hdf5DatasetColorsPath; + + HighFive::DataSet colorsDataset = m_file->getDataSet(hdf5DatasetColorsPath); + + std::vector>> colorData; + colorsDataset.read(colorData); - // if (info.has_rgba) - // readColorsRGBA(id, pointsRGBA); + int rOffset = getOffset(names, offsets, "r", isBigendian, pointStep); + int gOffset = getOffset(names, offsets, "g", isBigendian, pointStep); + int bOffset = getOffset(names, offsets, "b", isBigendian, pointStep); + int aOffset = getOffset(names, offsets, "a", isBigendian, pointStep); + + seerep_hdf5_fb::PointCloud2WriteIterator rIter(data, rOffset, height * width, pointStep); + seerep_hdf5_fb::PointCloud2WriteIterator gIter(data, gOffset, height * width, pointStep); + seerep_hdf5_fb::PointCloud2WriteIterator bIter(data, bOffset, height * width, pointStep); + seerep_hdf5_fb::PointCloud2WriteIterator aIter(data, aOffset, height * width, pointStep); + + for (auto col : colorData) + { + for (auto row : col) + { + *rIter = row[0]; + *gIter = row[1]; + *bIter = row[2]; + *aIter = row[3]; + ++rIter, ++gIter, ++bIter, ++aIter; + } + } + } // // TODO add other fields @@ -614,31 +675,10 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) // TODO check if the copy into the tmp vector can be removed somehow void Hdf5FbPointCloud::readPoints(const std::string& uuid, uint8_t* data) { - const std::string hdf5DatasetPointsPath = HDF5_GROUP_POINTCLOUD + "/" + uuid + "/points"; - - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) - << "reading points from: " << hdf5DatasetPointsPath; - - HighFive::DataSet pointsDataset = m_file->getDataSet(hdf5DatasetPointsPath); - - std::vector>> pointData; - pointsDataset.read(pointData); - - for (auto col : pointData) - { - for (auto row : col) {} - } } void Hdf5FbPointCloud::readColorsRGB(const std::string& uuid, std::vector>>& colorData) { - const std::string hdf5DatasetColorsPath = HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; - - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "reading rgb from: " << hdf5DatasetColorsPath; - - HighFive::DataSet colorsDataset = m_file->getDataSet(hdf5DatasetColorsPath); - - colorsDataset.read(colorData); } void Hdf5FbPointCloud::readColorsRGBA(const std::string& uuid, std::vector>>& colorData) From d1a40df15576bcd4ba6dd4f73c914005b356df97 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Thu, 6 Oct 2022 07:46:01 +0000 Subject: [PATCH 27/54] refactor flatbuffers hdf5 file access --- .../gRPC/pointcloud/gRPC_fb_sendPointCloud.py | 3 +- .../hdf5-fb-point-cloud2-iterator.h | 25 +- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 72 +- .../impl/hdf5-fb-point-cloud2-iterator.hpp | 85 -- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 880 +++++++++--------- 5 files changed, 512 insertions(+), 553 deletions(-) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py index af8e2dc01..a079172b6 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py @@ -62,8 +62,7 @@ def createPointCloud(builder, header, height=960, width=1280): labelsBb = createBoundingBoxesLabeled(builder, labelWithInstances, boundingBoxes) labelsBbVector = addToBoundingBoxLabeledVector(builder, labelsBb) - # create ordered point cloud with dim (height, width, 6) - # TODO rgb field should be int32 + # Note: rgb field is float, for simplification points = np.random.randn(height, width, 4).astype(np.float32) print(f"Data: {points}") diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h index 02b68888d..839eb18f1 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h @@ -234,10 +234,7 @@ class PointCloud2IteratorBase * @param field_name the name of the field to iterate upon * @return the offset at which the field is found */ - uint32_t set_field(const seerep::fb::PointCloud2& cloud_msg, const std::string& field_name); - /** The "point_step" of the point cloud */ - int point_step_; /** The raw data in uchar* where the iterator is */ U* data_char_; /** The cast data where the iterator is */ @@ -246,6 +243,10 @@ class PointCloud2IteratorBase TT* data_end_; /** Whether the fields are stored as bigendian */ bool is_bigendian_; + /** The "point_step" of the point cloud */ + uint32_t point_step_; + /** Offset of the field to iterate on */ + uint32_t field_offset_; }; } // namespace impl @@ -263,15 +264,15 @@ class PointCloud2WriteIterator : public impl::PointCloud2IteratorBase::PointCloud2IteratorBase() { - uint32_t offset_ = offset; + this->field_offset_ = offset; this->point_step_ = pointStep; - this->data_char_ = reinterpret_cast(data + offset); + this->data_char_ = reinterpret_cast(data + this->field_offset_); this->data_ = reinterpret_cast(this->data_char_); - this->data_end_ = reinterpret_cast(data + size + offset); + this->data_end_ = reinterpret_cast(data + offset + (height * width - 1) * pointStep); } }; @@ -289,14 +290,16 @@ class PointCloud2ReadIterator * @param cloudMsg the flatbuffers point cloud message to use * @param fieldName the field to iterate over */ - PointCloud2ReadIterator(const seerep::fb::PointCloud2& cloudMsg, const std::string& fieldName) + PointCloud2ReadIterator(uint8_t* data, uint32_t offset, uint32_t height, uint32_t width, u_int32_t pointStep) : impl::PointCloud2IteratorBase::PointCloud2IteratorBase() { - int offset = this->set_field(cloudMsg, fieldName); - this->data_char_ = reinterpret_cast(cloudMsg.data()->data() + offset); + this->field_offset_ = offset; + this->point_step_ = pointStep; + this->data_char_ = reinterpret_cast(data + this->field_offset_); this->data_ = reinterpret_cast(this->data_char_); - this->data_end_ = reinterpret_cast(cloudMsg.data()->data() + cloudMsg.data()->size() + offset); + // last element is at: offset + (n -1) * pointStep + this->data_end_ = reinterpret_cast(data + offset + (height * width - 1) * pointStep); } }; } // namespace seerep_hdf5_fb diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index a651497b8..f2c1694dd 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -14,7 +14,6 @@ #include // std -#include #include #include @@ -29,27 +28,13 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral std::optional> readPointCloud2(const std::string& uuid); - std::vector loadBoundingBox(const std::string& uuid); - private: - /* - Note: This struct is used to store the necessary information of a point field. - The generated seerep::fb::point struct can't be used since it doesn't have a copy - constructor resp. assignment operator and thus can't be used in maps (see CloudInfo struct). - */ - struct PointFieldInfo - { - int32_t datatype; - uint32_t count; - }; - struct CloudInfo { bool has_points = false; bool has_rgb = false; bool has_rgba = false; bool has_normals = false; - std::map other_fields; }; template @@ -92,12 +77,6 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral dataset_ptr->write(data); } - template - bool equalVectorLength(T const& first, U const&... rest) - { - return ((first.size() == rest.size()) && ...); - } - template void writePointFieldAttributes(HighFive::AnnotateTraits& object, const flatbuffers::Vector>& pointFields) @@ -122,29 +101,54 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral writeAttributeToHdf5>(object, FIELD_COUNT, counts); } - CloudInfo getCloudInfo(const flatbuffers::Vector>& pointFields); + /** write */ + void writePoints(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, + uint32_t height, uint32_t width, const std::shared_ptr& groupPtr); + + void writeColorsRGB(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, + uint32_t height, uint32_t width); + + void writeColorsRGBA(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, + uint32_t height, uint32_t width); + + /** read */ + + void readGeneralAttributes(const std::string& id, std::shared_ptr dataGroupPtr, uint32_t& height, + uint32_t& width, uint32_t& pointStep, uint32_t& rowStep, bool& isBigendian, bool& isDense); + + void readPointFields(const std::string& id, std::shared_ptr dataGroupPtr, + std::vector& names, std::vector& offsets, std::vector& counts, + std::vector& datatypes); + + void readPoints(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, + uint32_t height, uint32_t width); - void writePoints(const std::string& uuid, const std::shared_ptr& data_group_ptr, - const seerep::fb::PointCloud2& cloud); + void readColorsRGB(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, + uint32_t height, uint32_t width); - void writeColorsRGB(const std::string& uuid, const seerep::fb::PointCloud2& cloud); + void readColorsRGBA(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, + uint32_t height, uint32_t width); - void writeColorsRGBA(const std::string& uuid, const seerep::fb::PointCloud2& cloud); + /** helpers for building the point cloud message*/ + flatbuffers::Offset>> + readLabelsGeneralOffset(flatbuffers::grpc::MessageBuilder& builder, const std::string& id); - void writeOtherFields(const std::string& uuid, const seerep::fb::PointCloud2& cloud, - const std::map& fields); + flatbuffers::Offset>> + readLabelsBoundingBoxOffset(flatbuffers::grpc::MessageBuilder& builder, const std::string& id); - void readPoints(const std::string& uuid, uint8_t* data); + flatbuffers::Offset>> + readPointFieldsOffset(flatbuffers::grpc::MessageBuilder& builder, std::vector& names, + std::vector& offsets, std::vector& counts, std::vector& datatypes); - void readColorsRGB(const std::string& uuid, std::vector>>& colorData); + /** helpers for managing point cloud information*/ + CloudInfo getCloudInfo(const seerep::fb::PointCloud2& cloud); - void readColorsRGBA(const std::string& uuid, std::vector>>& colorData); + CloudInfo getCloudInfo(const std::vector& fields); - void readOtherFields(const std::string& uuid, const std::map& fields, - std::vector otherFieldsData); + uint32_t getOffset(const seerep::fb::PointCloud2& cloud, const std::string& fieldName); uint32_t getOffset(const std::vector& names, const std::vector& offsets, - const std::string& fieldName, bool isBigendian, uint32_t pointStep); + const std::string& fieldName, bool isBigendian); // pointcloud attribute keys inline static const std::string HEIGHT = "height"; diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp index 56d405bf9..a862f8b6b 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp @@ -193,91 +193,6 @@ V PointCloud2IteratorBase::end() const return res; } -/** Common code to set the field of the PointCloud2 - * @param cloud_msg the PointCloud2 to modify - * @param field_name the name of the field to iterate upon - * @return the offset at which the field is found - */ -template class V> -uint32_t PointCloud2IteratorBase::set_field(const seerep::fb::PointCloud2& cloud_msg, - const std::string& field_name) -{ - is_bigendian_ = cloud_msg.is_bigendian(); - point_step_ = cloud_msg.point_step(); - // make sure the channel is valid - - for (size_t i = 0; i < cloud_msg.fields()->size(); i++) - { - const seerep::fb::PointField& field = *cloud_msg.fields()->Get(i); - if (field.name()->str() == field_name) - { - return field.offset(); - } - } - - // // Handle the special case of r,g,b,a (we assume they are understood as the - // // channels of an rgb or rgba field) - - std::set field_names = { "r", "g", "b", "a" }; - - if (field_names.find(field_name) != field_names.end()) - { - for (size_t i = 0; i < cloud_msg.fields()->size(); i++) - { - const seerep::fb::PointField& field = *cloud_msg.fields()->Get(i); - - if (field.name()->str() == "rgb" || field.name()->str() == "rgba") - { - if (field_name == "r") - { - if (is_bigendian_) - { - return field.offset() + 1; - } - else - { - return field.offset() + 2; - } - } - if (field_name == "g") - { - if (is_bigendian_) - { - return field.offset() + 2; - } - else - { - return field.offset() + 1; - } - } - if (field_name == "b") - { - if (is_bigendian_) - { - return field.offset() + 3; - } - else - { - return field.offset() + 0; - } - } - if (field_name == "a") - { - if (is_bigendian_) - { - return field.offset() + 0; - } - else - { - return field.offset() + 3; - } - } - } - } - } - throw std::runtime_error("Field " + field_name + " does not exist!"); -} - } // namespace impl } // namespace seerep_hdf5_fb diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index a09bc51a1..2fd9b01c5 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -9,7 +9,9 @@ Hdf5FbPointCloud::Hdf5FbPointCloud(std::shared_ptr& file, std::s { } -void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb::PointCloud2& pointcloud2) +// TODO refactor again, still not happy with the API + +void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb::PointCloud2& cloud) { const std::scoped_lock lock(*m_write_mtx); @@ -33,53 +35,83 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: dataGroupPtr = std::make_shared(m_file->createGroup(hdf5GroupPath)); } - writeAttributeToHdf5(*dataGroupPtr, HEIGHT, pointcloud2.height()); - writeAttributeToHdf5(*dataGroupPtr, WIDTH, pointcloud2.width()); - writeAttributeToHdf5(*dataGroupPtr, IS_BIGENDIAN, pointcloud2.is_bigendian()); - writeAttributeToHdf5(*dataGroupPtr, POINT_STEP, pointcloud2.point_step()); - writeAttributeToHdf5(*dataGroupPtr, ROW_STEP, pointcloud2.row_step()); - writeAttributeToHdf5(*dataGroupPtr, IS_DENSE, pointcloud2.is_dense()); + writeAttributeToHdf5(*dataGroupPtr, HEIGHT, cloud.height()); + writeAttributeToHdf5(*dataGroupPtr, WIDTH, cloud.width()); + writeAttributeToHdf5(*dataGroupPtr, IS_BIGENDIAN, cloud.is_bigendian()); + writeAttributeToHdf5(*dataGroupPtr, POINT_STEP, cloud.point_step()); + writeAttributeToHdf5(*dataGroupPtr, ROW_STEP, cloud.row_step()); + writeAttributeToHdf5(*dataGroupPtr, IS_DENSE, cloud.is_dense()); - writeHeaderAttributes(*dataGroupPtr, *pointcloud2.header()); + writeHeaderAttributes(*dataGroupPtr, *cloud.header()); - writePointFieldAttributes(*dataGroupPtr, *pointcloud2.fields()); + writePointFieldAttributes(*dataGroupPtr, *cloud.fields()); - if (pointcloud2.labels_bb() != nullptr) + if (cloud.labels_bb() != nullptr) { - writeBoundingBoxLabeled(HDF5_GROUP_POINTCLOUD, id, *pointcloud2.labels_bb()); + writeBoundingBoxLabeled(HDF5_GROUP_POINTCLOUD, id, *cloud.labels_bb()); } - if (pointcloud2.labels_general() != nullptr) + if (cloud.labels_general() != nullptr) { - writeLabelsGeneral(HDF5_GROUP_POINTCLOUD, id, *pointcloud2.labels_general()); + writeLabelsGeneral(HDF5_GROUP_POINTCLOUD, id, *cloud.labels_general()); } - CloudInfo info = getCloudInfo(*pointcloud2.fields()); + CloudInfo info = getCloudInfo(cloud); if (info.has_points) - writePoints(id, dataGroupPtr, pointcloud2); + { + const std::vector fields = { "x", "y", "z" }; + std::vector xyzOffsets; + for (auto field : fields) + { + xyzOffsets.push_back(getOffset(cloud, field)); + } + + writePoints(id, xyzOffsets, const_cast(cloud.data()->data()), cloud.point_step(), cloud.height(), + cloud.width(), dataGroupPtr); + } + if (info.has_rgb) - writeColorsRGB(id, pointcloud2); + { + const std::vector fields = { "r", "g", "b" }; + std::vector rgbOffsets; + for (auto field : fields) + { + rgbOffsets.push_back(getOffset(cloud, field)); + } + + writeColorsRGBA(id, rgbOffsets, const_cast(cloud.data()->data()), cloud.point_step(), cloud.height(), + cloud.width()); + } + if (info.has_rgba) - writeColorsRGBA(id, pointcloud2); + { + const std::vector fields = { "r", "g", "b", "a" }; + std::vector rgbaOffsets; + for (auto field : fields) + { + rgbaOffsets.push_back(getOffset(cloud, field)); + } + + writeColorsRGBA(id, rgbaOffsets, const_cast(cloud.data()->data()), cloud.point_step(), cloud.height(), + cloud.width()); + } // TODO normals // TODO other fields - // if (!info.other_fields.empty()) - // writeOtherFields(uuid, *pointcloud2, info.other_fields); - m_file->flush(); } -void Hdf5FbPointCloud::writePoints(const std::string& id, const std::shared_ptr& dataGroupPtr, - const seerep::fb::PointCloud2& cloud) +void Hdf5FbPointCloud::writePoints(const std::string& id, const std::vector& offsets, uint8_t* data, + uint32_t pointStep, uint32_t height, uint32_t width, + const std::shared_ptr& groupPtr) { std::string hdf5PointsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/points"; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "writing dataset to: " << hdf5PointsPath; - HighFive::DataSpace dataSpace({ cloud.height(), cloud.width(), 3 }); + HighFive::DataSpace dataSpace({ height, width, 3 }); std::shared_ptr pointsDatasetPtr; try @@ -97,26 +129,26 @@ void Hdf5FbPointCloud::writePoints(const std::string& id, const std::shared_ptr< } std::vector>> pointData; - pointData.resize(cloud.height()); + pointData.resize(height); std::array min, max; min[0] = min[1] = min[2] = std::numeric_limits::max(); max[0] = max[1] = max[2] = std::numeric_limits::min(); - seerep_hdf5_fb::PointCloud2ReadIterator xIter(cloud, "x"); - seerep_hdf5_fb::PointCloud2ReadIterator yIter(cloud, "y"); - seerep_hdf5_fb::PointCloud2ReadIterator zIter(cloud, "z"); + seerep_hdf5_fb::PointCloud2ReadIterator xIter(data, offsets[0], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ReadIterator yIter(data, offsets[1], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ReadIterator zIter(data, offsets[2], height, width, pointStep); - for (size_t row = 0; row < cloud.height(); row++) + for (size_t row = 0; row < height; row++) { - pointData[row].reserve(cloud.width()); - for (size_t col = 0; col < cloud.width(); col++) + pointData[row].reserve(width); + for (size_t col = 0; col < width; col++) { const float& x = *xIter; const float& y = *yIter; const float& z = *zIter; - // compute bounding box for indicies at the same time + // compute bounding box for indicies if (x < min[0]) min[0] = x; if (x > max[0]) @@ -142,18 +174,18 @@ void Hdf5FbPointCloud::writePoints(const std::string& id, const std::shared_ptr< const std::vector boundingBox{ min[0], min[1], min[2], max[0], max[1], max[2] }; // write bounding box as data group attribute - writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, boundingBox); + writeAttributeToHdf5(*groupPtr, seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX, boundingBox); - // write points to dataset pointsDatasetPtr->write(pointData); } -void Hdf5FbPointCloud::writeColorsRGB(const std::string& uuid, const seerep::fb::PointCloud2& cloud) +void Hdf5FbPointCloud::writeColorsRGB(const std::string& id, const std::vector& offsets, uint8_t* data, + uint32_t pointStep, uint32_t height, uint32_t width) { - const std::string hdf5ColorsPath = HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + const std::string hdf5ColorsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "writing dataset to: " << hdf5ColorsPath; - HighFive::DataSpace dataSpace({ cloud.height(), cloud.width(), 3 }); + HighFive::DataSpace dataSpace({ height, width, 3 }); std::shared_ptr colorsDatasetPtr; try @@ -171,16 +203,16 @@ void Hdf5FbPointCloud::writeColorsRGB(const std::string& uuid, const seerep::fb: } std::vector>> colorsData; - colorsData.resize(cloud.height()); + colorsData.resize(height); - seerep_hdf5_fb::PointCloud2ReadIterator rIter(cloud, "r"); - seerep_hdf5_fb::PointCloud2ReadIterator gIter(cloud, "g"); - seerep_hdf5_fb::PointCloud2ReadIterator bIter(cloud, "b"); + seerep_hdf5_fb::PointCloud2ReadIterator rIter(data, offsets[0], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ReadIterator gIter(data, offsets[1], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ReadIterator bIter(data, offsets[2], height, width, pointStep); - for (size_t row = 0; row < cloud.height(); row++) + for (size_t row = 0; row < height; row++) { - colorsData[row].reserve(cloud.width()); - for (size_t col = 0; col < cloud.width(); col++) + colorsData[row].reserve(width); + for (size_t col = 0; col < width; col++) { colorsData[row].push_back(std::vector{ *rIter, *gIter, *bIter }); ++rIter, ++gIter, ++bIter; @@ -190,12 +222,13 @@ void Hdf5FbPointCloud::writeColorsRGB(const std::string& uuid, const seerep::fb: colorsDatasetPtr->write(colorsData); } -void Hdf5FbPointCloud::writeColorsRGBA(const std::string& uuid, const seerep::fb::PointCloud2& cloud) +void Hdf5FbPointCloud::writeColorsRGBA(const std::string& id, const std::vector& offsets, uint8_t* data, + uint32_t pointStep, uint32_t height, uint32_t width) { - const std::string hdf5ColorsPath = HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + const std::string hdf5ColorsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "writing dataset to: " << hdf5ColorsPath; - HighFive::DataSpace dataSpace({ cloud.height(), cloud.width(), 4 }); + HighFive::DataSpace dataSpace({ height, width, 4 }); std::shared_ptr colorsDatasetPtr; try @@ -213,17 +246,17 @@ void Hdf5FbPointCloud::writeColorsRGBA(const std::string& uuid, const seerep::fb } std::vector>> colorsData; - colorsData.resize(cloud.height()); + colorsData.resize(height); - seerep_hdf5_fb::PointCloud2ReadIterator rIter(cloud, "r"); - seerep_hdf5_fb::PointCloud2ReadIterator gIter(cloud, "g"); - seerep_hdf5_fb::PointCloud2ReadIterator bIter(cloud, "b"); - seerep_hdf5_fb::PointCloud2ReadIterator aIter(cloud, "a"); + seerep_hdf5_fb::PointCloud2ReadIterator rIter(data, offsets[0], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ReadIterator gIter(data, offsets[1], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ReadIterator bIter(data, offsets[2], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ReadIterator aIter(data, offsets[3], height, width, pointStep); - for (size_t row = 0; row < cloud.height(); row++) + for (size_t row = 0; row < height; row++) { - colorsData[row].reserve(cloud.width()); - for (size_t col = 0; col < cloud.width(); col++) + colorsData[row].reserve(width); + for (size_t col = 0; col < width; col++) { colorsData[row].push_back(std::vector{ *rIter, *gIter, *bIter, *aIter }); ++rIter; @@ -236,153 +269,7 @@ void Hdf5FbPointCloud::writeColorsRGBA(const std::string& uuid, const seerep::fb colorsDatasetPtr->write(colorsData); } -void Hdf5FbPointCloud::writeOtherFields(const std::string& uuid, const seerep::fb::PointCloud2& cloud, - const std::map& fields) -{ - for (auto map_entry : fields) - { - const std::string name = map_entry.first; - const PointFieldInfo info = map_entry.second; - switch (info.datatype) - { - case seerep::fb::Point_Field_Datatype_INT8: - write(uuid, name, cloud, info.count); - break; - case seerep::fb::Point_Field_Datatype_UINT8: - write(uuid, name, cloud, info.count); - break; - case seerep::fb::Point_Field_Datatype_INT16: - write(uuid, name, cloud, info.count); - break; - case seerep::fb::Point_Field_Datatype_UINT16: - write(uuid, name, cloud, info.count); - break; - case seerep::fb::Point_Field_Datatype_INT32: - write(uuid, name, cloud, info.count); - break; - case seerep::fb::Point_Field_Datatype_UINT32: - write(uuid, name, cloud, info.count); - break; - case seerep::fb::Point_Field_Datatype_FLOAT32: - write(uuid, name, cloud, info.count); - break; - case seerep::fb::Point_Field_Datatype_FLOAT64: - write(uuid, name, cloud, info.count); - break; - default: - std::cout << "datatype of pointcloud unknown" << std::endl; - break; - } - } -} - -Hdf5FbPointCloud::CloudInfo -Hdf5FbPointCloud::getCloudInfo(const flatbuffers::Vector>& pointFields) -{ - bool hasFieldx = false; - bool hasFieldy = false; - bool hasFieldz = false; - - CloudInfo info; - for (size_t i = 0; i < pointFields.size(); i++) - { - const std::string& fieldName = pointFields.Get(i)->name()->str(); - if (fieldName == "x") - hasFieldx = true; - else if (fieldName == "y") - hasFieldy = true; - else if (fieldName == "z") - hasFieldz = true; - else if (fieldName == "rgb") - info.has_rgb = true; - else if (fieldName == "rgba") - info.has_rgba = true; - else if (fieldName.find("normal") == 0) - info.has_normals = true; - else - { - info.other_fields[fieldName] = (struct PointFieldInfo){}; - } - } - if (hasFieldx && hasFieldy && hasFieldz) - info.has_points = true; - return info; -} - -// Assumption: names and offsets vector have the same order of the fields -uint32_t Hdf5FbPointCloud::getOffset(const std::vector& names, const std::vector& offsets, - const std::string& fieldName, bool isBigendian, uint32_t poinStep) -{ - for (size_t i = 0; i < names.size(); i++) - { - if (names[i] == fieldName) - { - return offsets[i]; - } - } - - // Handle the special case of rgb(a), since the rgb(a) channel is encoded in a single 32 Bit integer - - // Needs C++17 - std::set fieldNames = { "r", "g", "b", "a" }; - - if (fieldNames.find(fieldName) != fieldNames.end()) - { - for (size_t i = 0; i < names.size(); i++) - { - if (names[i] == "rgb" || names[i] == "rgba") - { - if (fieldName == "r") - { - if (isBigendian) - { - return offsets[i] + 1; - } - else - { - return offsets[i] + 2; - } - } - if (fieldName == "g") - { - if (isBigendian) - { - return offsets[i] + 2; - } - else - { - return offsets[i] + 1; - } - } - if (fieldName == "b") - { - if (isBigendian) - { - return offsets[i] + 3; - } - else - { - return offsets[i] + 0; - } - } - if (fieldName == "a") - { - if (isBigendian) - { - return offsets[i] + 0; - } - else - { - return offsets[i] + 3; - } - } - } - } - } - throw std::runtime_error("Field " + fieldName + " does not exist!"); -} - -// TODO read partial point cloud, e.g. only xyz without color, etc. +// TODO partial point cloud read std::optional> Hdf5FbPointCloud::readPointCloud2(const std::string& id) { @@ -416,12 +303,7 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "loading general attributes of : " << hdf5GroupPath; - height = readAttributeFromHdf5(id, *dataGroupPtr, HEIGHT); - width = readAttributeFromHdf5(id, *dataGroupPtr, WIDTH); - pointStep = readAttributeFromHdf5(id, *dataGroupPtr, POINT_STEP); - rowStep = readAttributeFromHdf5(id, *dataGroupPtr, ROW_STEP); - isBigendian = readAttributeFromHdf5(id, *dataGroupPtr, IS_BIGENDIAN); - isDense = readAttributeFromHdf5(id, *dataGroupPtr, IS_DENSE); + readGeneralAttributes(id, dataGroupPtr, height, width, pointStep, rowStep, isBigendian, isDense); } catch (const std::invalid_argument& e) { @@ -430,7 +312,7 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) return std::nullopt; } - // reads and creates a flatbuffers header with offset + // reads and creates a flatbuffers header auto headerAttributesOffset = readHeaderAttributes(builder, *dataGroupPtr, id); // read point field attributes @@ -442,10 +324,7 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "loading point field attributes of: " << hdf5GroupPath; - names = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_NAME); - offsets = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_OFFSET); - counts = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_COUNT); - datatypes = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_DATATYPE); + readPointFields(id, dataGroupPtr, names, offsets, counts, datatypes); } catch (const std::invalid_argument& e) { @@ -454,149 +333,218 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) return std::nullopt; } - if (!equalVectorLength(names, offsets, counts, datatypes)) - { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) - << "unequal number of point field attributes: " << hdf5GroupPath; - return std::nullopt; - } - - std::vector> pointFields; - for (size_t i = 0; i < names.size(); i++) - { - auto nameStr = builder.CreateString(names.at(i)); - seerep::fb::PointFieldBuilder pointField(builder); - pointField.add_name(nameStr); - pointField.add_offset(offsets.at(i)); - pointField.add_datatype(static_cast(datatypes.at(i))); - pointField.add_count(counts.at(i)); - pointFields.push_back(pointField.Finish()); - } - auto pointFieldsVector = builder.CreateVector(pointFields); + auto pointFieldsVectorOffset = readPointFieldsOffset(builder, names, offsets, counts, datatypes); - CloudInfo info = getCloudInfo(*reinterpret_cast>*>( - builder.GetCurrentBufferPointer())); + // get info about the point cloud + CloudInfo info = getCloudInfo(names); BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "reading point cloud data of: " << hdf5GroupPath; - // pre allocate vector, to write values in it from the iterators - // should be way faster then copying it in vectors; - - // pointer to the data field of the pre-allocated array + // pointer to the pre-allocated array uint8_t* data; - // allocate height * width * point setp bytes + // allocate height * width * pointSetp bytes auto vector = builder.CreateUninitializedVector(height * width * pointStep, sizeof(uint8_t), &data); if (info.has_points) { - const std::string hdf5DatasetPointsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/points"; - - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) - << "reading points from: " << hdf5DatasetPointsPath; - - HighFive::DataSet pointsDataset = m_file->getDataSet(hdf5DatasetPointsPath); + const std::vector fields = { "x", "y", "z" }; + std::vector xyzOffsets; + for (auto field : fields) + { + xyzOffsets.push_back(getOffset(names, offsets, field, isBigendian)); + } - std::vector>> pointData; - pointsDataset.read(pointData); + readPoints(id, xyzOffsets, data, pointStep, height, width); + } - int xOffset = getOffset(names, offsets, "x", isBigendian, pointStep); - int yOffset = getOffset(names, offsets, "y", isBigendian, pointStep); - int zOffset = getOffset(names, offsets, "z", isBigendian, pointStep); + if (info.has_rgb) + { + const std::vector fields = { "r", "g", "b" }; + std::vector rgbOffsets; + for (auto field : fields) + { + rgbOffsets.push_back(getOffset(names, offsets, field, isBigendian)); + } - seerep_hdf5_fb::PointCloud2WriteIterator xIter(data, xOffset, height * width, pointStep); - seerep_hdf5_fb::PointCloud2WriteIterator yIter(data, yOffset, height * width, pointStep); - seerep_hdf5_fb::PointCloud2WriteIterator zIter(data, zOffset, height * width, pointStep); + readColorsRGB(id, rgbOffsets, data, pointStep, height, width); + } - for (auto col : pointData) + if (info.has_rgba) + { + const std::vector fields = { "r", "g", "b", "a" }; + std::vector rgbaOffsets; + for (auto field : fields) { - for (auto row : col) - { - *xIter = row[0]; - *yIter = row[1]; - *zIter = row[2]; - ++xIter, ++yIter, ++zIter; - } + rgbaOffsets.push_back(getOffset(names, offsets, field, isBigendian)); } + + readPoints(id, rgbaOffsets, data, pointStep, height, width); } - if (info.has_rgb) - { - const std::string hdf5DatasetColorsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; + // TODO add normals - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) - << "reading rgb from: " << hdf5DatasetColorsPath; + // TODO add other fields + + auto generalLabelsOffset = readLabelsGeneralOffset(builder, id); + + auto boundingBoxLabeledOffset = readLabelsBoundingBoxOffset(builder, id); + + // build the point cloud message + seerep::fb::PointCloud2Builder pointCloudBuilder(builder); + pointCloudBuilder.add_header(headerAttributesOffset); + pointCloudBuilder.add_height(height); + pointCloudBuilder.add_width(width); + pointCloudBuilder.add_fields(pointFieldsVectorOffset); + pointCloudBuilder.add_is_bigendian(isBigendian); + pointCloudBuilder.add_point_step(pointStep); + pointCloudBuilder.add_row_step(rowStep); + pointCloudBuilder.add_data(vector); + pointCloudBuilder.add_is_dense(isDense); + pointCloudBuilder.add_labels_general(generalLabelsOffset); + pointCloudBuilder.add_labels_bb(boundingBoxLabeledOffset); + auto pointCloudOffset = pointCloudBuilder.Finish(); + builder.Finish(pointCloudOffset); + + auto grpcPointCloud = builder.ReleaseMessage(); + return grpcPointCloud; +} + +void Hdf5FbPointCloud::readGeneralAttributes(const std::string& id, std::shared_ptr dataGroupPtr, + uint32_t& height, uint32_t& width, uint32_t& pointStep, uint32_t& rowStep, + bool& isBigendian, bool& isDense) +{ + height = readAttributeFromHdf5(id, *dataGroupPtr, HEIGHT); + width = readAttributeFromHdf5(id, *dataGroupPtr, WIDTH); + pointStep = readAttributeFromHdf5(id, *dataGroupPtr, POINT_STEP); + rowStep = readAttributeFromHdf5(id, *dataGroupPtr, ROW_STEP); + isBigendian = readAttributeFromHdf5(id, *dataGroupPtr, IS_BIGENDIAN); + isDense = readAttributeFromHdf5(id, *dataGroupPtr, IS_DENSE); +} + +void Hdf5FbPointCloud::readPointFields(const std::string& id, std::shared_ptr dataGroupPtr, + std::vector& names, std::vector& offsets, + std::vector& counts, std::vector& datatypes) +{ + names = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_NAME); + offsets = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_OFFSET); + counts = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_COUNT); + datatypes = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_DATATYPE); +} - HighFive::DataSet colorsDataset = m_file->getDataSet(hdf5DatasetColorsPath); +void Hdf5FbPointCloud::readPoints(const std::string& id, const std::vector& offsets, uint8_t* data, + uint32_t pointStep, uint32_t height, uint32_t width) +{ + const std::string hdf5DatasetPointsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/points"; + + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "reading points from: " << hdf5DatasetPointsPath; - std::vector>> colorData; - colorsDataset.read(colorData); + HighFive::DataSet pointsDataset = m_file->getDataSet(hdf5DatasetPointsPath); - int rOffset = getOffset(names, offsets, "r", isBigendian, pointStep); - int gOffset = getOffset(names, offsets, "g", isBigendian, pointStep); - int bOffset = getOffset(names, offsets, "b", isBigendian, pointStep); + std::vector>> pointData; + pointsDataset.read(pointData); - seerep_hdf5_fb::PointCloud2WriteIterator rIter(data, rOffset, height * width, pointStep); - seerep_hdf5_fb::PointCloud2WriteIterator gIter(data, gOffset, height * width, pointStep); - seerep_hdf5_fb::PointCloud2WriteIterator bIter(data, bOffset, height * width, pointStep); + seerep_hdf5_fb::PointCloud2WriteIterator xIter(data, offsets[0], pointStep, height, width); + seerep_hdf5_fb::PointCloud2WriteIterator yIter(data, offsets[1], pointStep, height, width); + seerep_hdf5_fb::PointCloud2WriteIterator zIter(data, offsets[2], pointStep, height, width); - for (auto col : colorData) + for (auto col : pointData) + { + for (auto row : col) { - for (auto row : col) - { - *rIter = row[0]; - *gIter = row[1]; - *bIter = row[2]; - ++rIter, ++gIter, ++bIter; - } + *xIter = row[0]; + *yIter = row[1]; + *zIter = row[2]; + ++xIter, ++yIter, ++zIter; } } +} - if (info.has_rgba) +void Hdf5FbPointCloud::readColorsRGB(const std::string& id, const std::vector& offsets, uint8_t* data, + uint32_t pointStep, uint32_t height, uint32_t width) +{ + const std::string hdf5DatasetPointsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; + + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "reading rgb from: " << hdf5DatasetPointsPath; + + HighFive::DataSet colorsDataset = m_file->getDataSet(hdf5DatasetPointsPath); + + std::vector>> colorData; + colorsDataset.read(colorData); + + seerep_hdf5_fb::PointCloud2WriteIterator rIter(data, offsets[0], pointStep, height, width); + seerep_hdf5_fb::PointCloud2WriteIterator gIter(data, offsets[1], pointStep, height, width); + seerep_hdf5_fb::PointCloud2WriteIterator bIter(data, offsets[2], pointStep, height, width); + + for (auto col : colorData) { - const std::string hdf5DatasetColorsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; + for (auto row : col) + { + *rIter = row[0]; + *gIter = row[1]; + *bIter = row[2]; + ++rIter, ++gIter, ++bIter; + } + } +} - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) - << "reading rgba from: " << hdf5DatasetColorsPath; +void Hdf5FbPointCloud::readColorsRGBA(const std::string& id, const std::vector& offsets, uint8_t* data, + uint32_t pointStep, uint32_t height, uint32_t width) +{ + const std::string hdf5DatasetPointsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; - HighFive::DataSet colorsDataset = m_file->getDataSet(hdf5DatasetColorsPath); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "reading rgba from: " << hdf5DatasetPointsPath; - std::vector>> colorData; - colorsDataset.read(colorData); + HighFive::DataSet colorsDataset = m_file->getDataSet(hdf5DatasetPointsPath); - int rOffset = getOffset(names, offsets, "r", isBigendian, pointStep); - int gOffset = getOffset(names, offsets, "g", isBigendian, pointStep); - int bOffset = getOffset(names, offsets, "b", isBigendian, pointStep); - int aOffset = getOffset(names, offsets, "a", isBigendian, pointStep); + std::vector>> colorData; + colorsDataset.read(colorData); - seerep_hdf5_fb::PointCloud2WriteIterator rIter(data, rOffset, height * width, pointStep); - seerep_hdf5_fb::PointCloud2WriteIterator gIter(data, gOffset, height * width, pointStep); - seerep_hdf5_fb::PointCloud2WriteIterator bIter(data, bOffset, height * width, pointStep); - seerep_hdf5_fb::PointCloud2WriteIterator aIter(data, aOffset, height * width, pointStep); + seerep_hdf5_fb::PointCloud2WriteIterator rIter(data, offsets[0], pointStep, height, width); + seerep_hdf5_fb::PointCloud2WriteIterator gIter(data, offsets[1], pointStep, height, width); + seerep_hdf5_fb::PointCloud2WriteIterator bIter(data, offsets[2], pointStep, height, width); + seerep_hdf5_fb::PointCloud2WriteIterator aIter(data, offsets[3], pointStep, height, width); - for (auto col : colorData) + for (auto col : colorData) + { + for (auto row : col) { - for (auto row : col) - { - *rIter = row[0]; - *gIter = row[1]; - *bIter = row[2]; - *aIter = row[3]; - ++rIter, ++gIter, ++bIter, ++aIter; - } + *rIter = row[0]; + *gIter = row[1]; + *bIter = row[2]; + *aIter = row[3]; + ++rIter, ++gIter, ++bIter, ++aIter; } } +} - // // TODO add other fields +flatbuffers::Offset>> +Hdf5FbPointCloud::readLabelsGeneralOffset(flatbuffers::grpc::MessageBuilder& builder, const std::string& id) +{ + std::vector labelsGeneral; + std::vector labelsGeneralInstances; + readLabelsGeneral(HDF5_GROUP_POINTCLOUD, id, labelsGeneral, labelsGeneralInstances); - // // if (!info.other_fields.empty()) - // // readOtherFields(id, info.other_fields, otherFields); + std::vector> labelGeneral; + labelGeneral.reserve(labelsGeneral.size()); + for (size_t i = 0; i < labelsGeneral.size(); i++) + { + auto labelOffset = builder.CreateString(labelsGeneral.at(i)); + auto instanceOffset = builder.CreateString(labelsGeneralInstances.at(i)); - // // TODO add normals + seerep::fb::LabelWithInstanceBuilder labelBuilder(builder); + labelBuilder.add_label(labelOffset); + labelBuilder.add_instanceUuid(instanceOffset); + labelGeneral.push_back(labelBuilder.Finish()); + } - // auto dataVector = builder.CreateVector(data); + return builder.CreateVector>(labelGeneral); +} - // read labeled bounding box +flatbuffers::Offset>> +Hdf5FbPointCloud::readLabelsBoundingBoxOffset(flatbuffers::grpc::MessageBuilder& builder, const std::string& id) +{ std::vector boundingBoxesLabels; std::vector> boundingBoxes; std::vector boundingBoxesInstances; @@ -629,140 +577,230 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) boundingBoxLabeled.push_back(bblabeledBuilder.Finish()); } - auto boundingBoxLabeledVector = builder.CreateVector(boundingBoxLabeled); - - // read labels general - std::vector labelsGeneral; - std::vector labelsGeneralInstances; - readLabelsGeneral(HDF5_GROUP_POINTCLOUD, id, labelsGeneral, labelsGeneralInstances); + return builder.CreateVector(boundingBoxLabeled); +} - // TODO add this to a higher level, it's the same for images and point clouds - std::vector> labelGeneral; - labelGeneral.reserve(labelsGeneral.size()); - for (size_t i = 0; i < labelsGeneral.size(); i++) +flatbuffers::Offset>> +Hdf5FbPointCloud::readPointFieldsOffset(flatbuffers::grpc::MessageBuilder& builder, std::vector& names, + std::vector& offsets, std::vector& counts, + std::vector& datatypes) +{ + std::vector> pointFields; + for (size_t i = 0; i < names.size(); i++) { - auto labelOffset = builder.CreateString(labelsGeneral.at(i)); - auto instanceOffset = builder.CreateString(labelsGeneralInstances.at(i)); - - seerep::fb::LabelWithInstanceBuilder labelBuilder(builder); - labelBuilder.add_label(labelOffset); - labelBuilder.add_instanceUuid(instanceOffset); - labelGeneral.push_back(labelBuilder.Finish()); + auto nameStr = builder.CreateString(names.at(i)); + seerep::fb::PointFieldBuilder pointField(builder); + pointField.add_name(nameStr); + pointField.add_offset(offsets.at(i)); + pointField.add_datatype(static_cast(datatypes.at(i))); + pointField.add_count(counts.at(i)); + pointFields.push_back(pointField.Finish()); } + return builder.CreateVector(pointFields); +} - auto labelsGeneralVector = builder.CreateVector>(labelGeneral); +Hdf5FbPointCloud::CloudInfo Hdf5FbPointCloud::getCloudInfo(const seerep::fb::PointCloud2& cloud) +{ + bool hasFieldx = false; + bool hasFieldy = false; + bool hasFieldz = false; - // build the point cloud message - seerep::fb::PointCloud2Builder pointCloudBuilder(builder); - pointCloudBuilder.add_header(headerAttributesOffset); - pointCloudBuilder.add_height(height); - pointCloudBuilder.add_width(width); - pointCloudBuilder.add_fields(pointFieldsVector); - pointCloudBuilder.add_is_bigendian(isBigendian); - pointCloudBuilder.add_point_step(pointStep); - pointCloudBuilder.add_row_step(rowStep); - pointCloudBuilder.add_data(vector); - pointCloudBuilder.add_is_dense(isDense); - pointCloudBuilder.add_labels_general(labelsGeneralVector); - pointCloudBuilder.add_labels_bb(boundingBoxLabeledVector); - auto pointCloudOffset = pointCloudBuilder.Finish(); - builder.Finish(pointCloudOffset); + CloudInfo info; - auto grpcPointCloud = builder.ReleaseMessage(); - return grpcPointCloud; + for (size_t i = 0; i < cloud.fields()->size(); i++) + { + const std::string& fieldName = cloud.fields()->Get(i)->name()->str(); + if (fieldName == "x") + hasFieldx = true; + else if (fieldName == "y") + hasFieldy = true; + else if (fieldName == "z") + hasFieldz = true; + else if (fieldName == "rgb") + info.has_rgb = true; + else if (fieldName == "rgba") + info.has_rgba = true; + else if (fieldName.find("normal") == 0) + info.has_normals = true; + } + if (hasFieldx && hasFieldy && hasFieldz) + info.has_points = true; + return info; } -// TODO check if the copy into the tmp vector can be removed somehow -void Hdf5FbPointCloud::readPoints(const std::string& uuid, uint8_t* data) +Hdf5FbPointCloud::CloudInfo Hdf5FbPointCloud::getCloudInfo(const std::vector& fields) { -} + bool hasFieldx = false; + bool hasFieldy = false; + bool hasFieldz = false; -void Hdf5FbPointCloud::readColorsRGB(const std::string& uuid, std::vector>>& colorData) -{ + CloudInfo info; + + for (auto fieldName : fields) + { + if (fieldName == "x") + hasFieldx = true; + else if (fieldName == "y") + hasFieldy = true; + else if (fieldName == "z") + hasFieldz = true; + else if (fieldName == "rgb") + info.has_rgb = true; + else if (fieldName == "rgba") + info.has_rgba = true; + else if (fieldName.find("normal") == 0) + info.has_normals = true; + } + if (hasFieldx && hasFieldy && hasFieldz) + info.has_points = true; + return info; } -void Hdf5FbPointCloud::readColorsRGBA(const std::string& uuid, std::vector>>& colorData) +uint32_t Hdf5FbPointCloud::getOffset(const std::vector& names, const std::vector& offsets, + const std::string& fieldName, bool isBigendian) { - const std::string hdf5DatasetColorsPath = HDF5_GROUP_POINTCLOUD + "/" + uuid + "/colors"; + for (size_t i = 0; i < names.size(); i++) + { + if (names[i] == fieldName) + { + return offsets[i]; + } + } - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "reading rgba from: " << hdf5DatasetColorsPath; + // Handle the special case of rgb(a), since the rgb(a) channel is encoded in a single 32 Bit integer - HighFive::DataSet colorsDataset = m_file->getDataSet(hdf5DatasetColorsPath); + std::set fieldNames = { "r", "g", "b", "a" }; - colorsDataset.read(colorData); + if (fieldNames.find(fieldName) != fieldNames.end()) + { + for (size_t i = 0; i < names.size(); i++) + { + if (names[i] == "rgb" || names[i] == "rgba") + { + if (fieldName == "r") + { + if (isBigendian) + { + return offsets[i] + 1; + } + else + { + return offsets[i] + 2; + } + } + if (fieldName == "g") + { + if (isBigendian) + { + return offsets[i] + 2; + } + else + { + return offsets[i] + 1; + } + } + if (fieldName == "b") + { + if (isBigendian) + { + return offsets[i] + 3; + } + else + { + return offsets[i] + 0; + } + } + if (fieldName == "a") + { + if (isBigendian) + { + return offsets[i] + 0; + } + else + { + return offsets[i] + 3; + } + } + } + } + } + throw std::runtime_error("Field " + fieldName + " does not exist!"); } -void Hdf5FbPointCloud::readOtherFields(const std::string& uuid, const std::map& fields, - std::vector otherFieldsData) +uint32_t Hdf5FbPointCloud::getOffset(const seerep::fb::PointCloud2& cloud, const std::string& fieldName) { - for (auto field_map_entry : fields) + bool isBigendian = cloud.is_bigendian(); + + for (size_t i = 0; i < cloud.fields()->size(); i++) { - const std::string& name = field_map_entry.first; - const PointFieldInfo pointFieldInfo = field_map_entry.second; - switch (pointFieldInfo.datatype) + const seerep::fb::PointField& field = *cloud.fields()->Get(i); + if (field.name()->str() == fieldName) { - case seerep::fb::Point_Field_Datatype_INT8: - { - std::vector data; - read(uuid, name, data, pointFieldInfo.count); - otherFieldsData.push_back(std::move(data)); - } - break; - case seerep::fb::Point_Field_Datatype_UINT8: - { - std::vector data; - read(uuid, name, data, pointFieldInfo.count); - otherFieldsData.push_back(std::move(data)); - } - break; - case seerep::fb::Point_Field_Datatype_INT16: - { - std::vector data; - read(uuid, name, data, pointFieldInfo.count); - otherFieldsData.push_back(std::move(data)); - } - break; - case seerep::fb::Point_Field_Datatype_UINT16: - { - std::vector data; - read(uuid, name, data, pointFieldInfo.count); - otherFieldsData.push_back(std::move(data)); - } - break; - case seerep::fb::Point_Field_Datatype_INT32: - { - std::vector data; - read(uuid, name, data, pointFieldInfo.count); - otherFieldsData.push_back(std::move(data)); - } - break; - case seerep::fb::Point_Field_Datatype_UINT32: - { - std::vector data; - read(uuid, name, data, pointFieldInfo.count); - otherFieldsData.push_back(std::move(data)); - } - break; - case seerep::fb::Point_Field_Datatype_FLOAT32: - { - std::vector data; - read(uuid, name, data, pointFieldInfo.count); - otherFieldsData.push_back(std::move(data)); - } - break; - case seerep::fb::Point_Field_Datatype_FLOAT64: + return field.offset(); + } + } + + // // Handle the special case of r,g,b,a (we assume they are understood as the + // // channels of an rgb or rgba field) + + std::set field_names = { "r", "g", "b", "a" }; + + if (field_names.find(fieldName) != field_names.end()) + { + for (size_t i = 0; i < cloud.fields()->size(); i++) + { + const seerep::fb::PointField& field = *cloud.fields()->Get(i); + + if (field.name()->str() == "rgb" || field.name()->str() == "rgba") { - std::vector data; - read(uuid, name, data, pointFieldInfo.count); - otherFieldsData.push_back(std::move(data)); + if (fieldName == "r") + { + if (isBigendian) + { + return field.offset() + 1; + } + else + { + return field.offset() + 2; + } + } + if (fieldName == "g") + { + if (isBigendian) + { + return field.offset() + 2; + } + else + { + return field.offset() + 1; + } + } + if (fieldName == "b") + { + if (isBigendian) + { + return field.offset() + 3; + } + else + { + return field.offset() + 0; + } + } + if (fieldName == "a") + { + if (isBigendian) + { + return field.offset() + 0; + } + else + { + return field.offset() + 3; + } + } } - break; - default: - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) - << "datatype of field in point cloud unknown"; - break; } } + throw std::runtime_error("Field " + fieldName + " does not exist!"); } } // namespace seerep_hdf5_fb From fc9938ca11720ece1d24e89c156757dbb7f4ad12 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Thu, 6 Oct 2022 08:09:25 +0000 Subject: [PATCH 28/54] add read without data to flatbuffers point clouds --- .../pointcloud/gRPC_fb_queryPointCloud.py | 9 ++- examples/python/gRPC/util_fb.py | 3 +- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 3 +- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 79 ++++++++++--------- .../seerep-core-fb/src/core-fb-pointcloud.cpp | 2 +- 5 files changed, 53 insertions(+), 43 deletions(-) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py index f9d190e17..7e4662beb 100644 --- a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py @@ -80,7 +80,8 @@ print(f"Instance {i}: {response.LabelsGeneral(i).InstanceUuid().decode('utf-8')}") print("---Data--") - rawData = response.DataAsNumpy() - data = [struct.unpack('f', rawData[i : i + 4]) for i in range(0, rawData.shape[0], 4)] - reshapedData = np.array(data).reshape(960, 1280, 4) - print(f"Data: {reshapedData}") + if not response.DataIsNone(): + rawData = response.DataAsNumpy() + data = [struct.unpack('f', rawData[i : i + 4]) for i in range(0, rawData.shape[0], 4)] + reshapedData = np.array(data).reshape(960, 1280, 4) + print(f"Data: {reshapedData}") diff --git a/examples/python/gRPC/util_fb.py b/examples/python/gRPC/util_fb.py index dcab71cb7..16cd985bc 100644 --- a/examples/python/gRPC/util_fb.py +++ b/examples/python/gRPC/util_fb.py @@ -204,7 +204,7 @@ def addToPointFieldVector(builder, pointFieldList): return builder.EndVector() -def createQuery(builder, projectUuids, timeInterval, generalLabels): +def createQuery(builder, projectUuids, timeInterval, generalLabels, withoutData=False): # add project uuids Query.StartProjectuuidVector(builder, len(projectUuids)) for projectUuid in reversed(projectUuids): @@ -220,6 +220,7 @@ def createQuery(builder, projectUuids, timeInterval, generalLabels): Query.AddProjectuuid(builder, projectUuidsOffset) Query.AddTimeinterval(builder, timeInterval) Query.AddLabel(builder, labelsOffset) + Query.AddWithoutdata(builder, withoutData) return Query.End(builder) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index f2c1694dd..b409469c5 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -26,7 +26,8 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral void writePointCloud2(const std::string& uuid, const seerep::fb::PointCloud2& pointcloud2); - std::optional> readPointCloud2(const std::string& uuid); + std::optional> readPointCloud2(const std::string& uuid, + const bool withoutData = false); private: struct CloudInfo diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 2fd9b01c5..ce2082759 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -271,7 +271,7 @@ void Hdf5FbPointCloud::writeColorsRGBA(const std::string& id, const std::vector< // TODO partial point cloud read std::optional> -Hdf5FbPointCloud::readPointCloud2(const std::string& id) +Hdf5FbPointCloud::readPointCloud2(const std::string& id, const bool withoutData) { const std::scoped_lock lock(*m_write_mtx); @@ -335,51 +335,55 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) auto pointFieldsVectorOffset = readPointFieldsOffset(builder, names, offsets, counts, datatypes); - // get info about the point cloud - CloudInfo info = getCloudInfo(names); + flatbuffers::uoffset_t dataOffset; + if (!withoutData) + { + // get info about the point cloud + CloudInfo info = getCloudInfo(names); - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) - << "reading point cloud data of: " << hdf5GroupPath; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) + << "reading point cloud data of: " << hdf5GroupPath; - // pointer to the pre-allocated array - uint8_t* data; - // allocate height * width * pointSetp bytes - auto vector = builder.CreateUninitializedVector(height * width * pointStep, sizeof(uint8_t), &data); + // pointer to the pre-allocated array + uint8_t* data; + // allocate height * width * pointSetp bytes + dataOffset = builder.CreateUninitializedVector(height * width * pointStep, sizeof(uint8_t), &data); - if (info.has_points) - { - const std::vector fields = { "x", "y", "z" }; - std::vector xyzOffsets; - for (auto field : fields) + if (info.has_points) { - xyzOffsets.push_back(getOffset(names, offsets, field, isBigendian)); - } + const std::vector fields = { "x", "y", "z" }; + std::vector xyzOffsets; + for (auto field : fields) + { + xyzOffsets.push_back(getOffset(names, offsets, field, isBigendian)); + } - readPoints(id, xyzOffsets, data, pointStep, height, width); - } + readPoints(id, xyzOffsets, data, pointStep, height, width); + } - if (info.has_rgb) - { - const std::vector fields = { "r", "g", "b" }; - std::vector rgbOffsets; - for (auto field : fields) + if (info.has_rgb) { - rgbOffsets.push_back(getOffset(names, offsets, field, isBigendian)); - } + const std::vector fields = { "r", "g", "b" }; + std::vector rgbOffsets; + for (auto field : fields) + { + rgbOffsets.push_back(getOffset(names, offsets, field, isBigendian)); + } - readColorsRGB(id, rgbOffsets, data, pointStep, height, width); - } + readColorsRGB(id, rgbOffsets, data, pointStep, height, width); + } - if (info.has_rgba) - { - const std::vector fields = { "r", "g", "b", "a" }; - std::vector rgbaOffsets; - for (auto field : fields) + if (info.has_rgba) { - rgbaOffsets.push_back(getOffset(names, offsets, field, isBigendian)); - } + const std::vector fields = { "r", "g", "b", "a" }; + std::vector rgbaOffsets; + for (auto field : fields) + { + rgbaOffsets.push_back(getOffset(names, offsets, field, isBigendian)); + } - readPoints(id, rgbaOffsets, data, pointStep, height, width); + readPoints(id, rgbaOffsets, data, pointStep, height, width); + } } // TODO add normals @@ -399,7 +403,10 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id) pointCloudBuilder.add_is_bigendian(isBigendian); pointCloudBuilder.add_point_step(pointStep); pointCloudBuilder.add_row_step(rowStep); - pointCloudBuilder.add_data(vector); + if (!withoutData) + { + pointCloudBuilder.add_data(dataOffset); + } pointCloudBuilder.add_is_dense(isDense); pointCloudBuilder.add_labels_general(generalLabelsOffset); pointCloudBuilder.add_labels_bb(boundingBoxLabeledOffset); diff --git a/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp index 8668c4cc0..1765b8b3f 100644 --- a/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp +++ b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp @@ -23,7 +23,7 @@ void CoreFbPointCloud::getData(const seerep::fb::Query* query, for (auto uuidPc : project.dataOrInstanceUuids) { auto pc = CoreFbGeneral::getHdf5(project.projectUuid, m_seerepCore, m_hdf5IoMap) - ->readPointCloud2(boost::lexical_cast(uuidPc)); + ->readPointCloud2(boost::lexical_cast(uuidPc), queryCore.withoutData); if (pc) { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) From 34dba98b6260c208956cc826ab2d253039f82a20 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Fri, 7 Oct 2022 11:55:27 +0000 Subject: [PATCH 29/54] use hdf5 definitions from seerep-hdf5-core #183 --- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 29 ++------- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 63 +++++++++++-------- 2 files changed, 42 insertions(+), 50 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index b409469c5..fa67283df 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -41,7 +41,8 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral template void read(const std::string& id, const std::string& fieldName, std::vector& data, size_t size) { - const std::string hdf5DatasetFieldPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/" + fieldName; + const std::string hdf5DatasetFieldPath = + seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id + "/" + fieldName; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "reading " << fieldName << " from: " << hdf5DatasetFieldPath; @@ -96,10 +97,10 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral counts.push_back(pointField->count()); } - writeAttributeToHdf5>(object, FIELD_NAME, names); - writeAttributeToHdf5>(object, FIELD_OFFSET, offsets); - writeAttributeToHdf5>(object, FIELD_DATATYPE, datatypes); - writeAttributeToHdf5>(object, FIELD_COUNT, counts); + writeAttributeToHdf5>(object, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME, names); + writeAttributeToHdf5>(object, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET, offsets); + writeAttributeToHdf5>(object, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_DATATYPE, datatypes); + writeAttributeToHdf5>(object, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_COUNT, counts); } /** write */ @@ -150,24 +151,6 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral uint32_t getOffset(const std::vector& names, const std::vector& offsets, const std::string& fieldName, bool isBigendian); - - // pointcloud attribute keys - inline static const std::string HEIGHT = "height"; - inline static const std::string WIDTH = "width"; - inline static const std::string ENCODING = "encoding"; - inline static const std::string IS_BIGENDIAN = "is_bigendian"; - inline static const std::string ROW_STEP = "row_step"; - inline static const std::string POINT_STEP = "point_step"; - inline static const std::string IS_DENSE = "is_dense"; - - // point field attribute keys - inline static const std::string FIELD_NAME = "field_name"; - inline static const std::string FIELD_OFFSET = "field_offset"; - inline static const std::string FIELD_DATATYPE = "field_datatype"; - inline static const std::string FIELD_COUNT = "field_count"; - -public: - inline static const std::string HDF5_GROUP_POINTCLOUD = "pointclouds"; }; } // namespace seerep_hdf5_fb diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index ce2082759..f1302a619 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -35,12 +35,12 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: dataGroupPtr = std::make_shared(m_file->createGroup(hdf5GroupPath)); } - writeAttributeToHdf5(*dataGroupPtr, HEIGHT, cloud.height()); - writeAttributeToHdf5(*dataGroupPtr, WIDTH, cloud.width()); - writeAttributeToHdf5(*dataGroupPtr, IS_BIGENDIAN, cloud.is_bigendian()); - writeAttributeToHdf5(*dataGroupPtr, POINT_STEP, cloud.point_step()); - writeAttributeToHdf5(*dataGroupPtr, ROW_STEP, cloud.row_step()); - writeAttributeToHdf5(*dataGroupPtr, IS_DENSE, cloud.is_dense()); + writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT, cloud.height()); + writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::WIDTH, cloud.width()); + writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN, cloud.is_bigendian()); + writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP, cloud.point_step()); + writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP, cloud.row_step()); + writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE, cloud.is_dense()); writeHeaderAttributes(*dataGroupPtr, *cloud.header()); @@ -48,12 +48,12 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: if (cloud.labels_bb() != nullptr) { - writeBoundingBoxLabeled(HDF5_GROUP_POINTCLOUD, id, *cloud.labels_bb()); + writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, *cloud.labels_bb()); } if (cloud.labels_general() != nullptr) { - writeLabelsGeneral(HDF5_GROUP_POINTCLOUD, id, *cloud.labels_general()); + writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, *cloud.labels_general()); } CloudInfo info = getCloudInfo(cloud); @@ -108,7 +108,7 @@ void Hdf5FbPointCloud::writePoints(const std::string& id, const std::vector& groupPtr) { - std::string hdf5PointsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/points"; + std::string hdf5PointsPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id + "/points"; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "writing dataset to: " << hdf5PointsPath; HighFive::DataSpace dataSpace({ height, width, 3 }); @@ -182,7 +182,7 @@ void Hdf5FbPointCloud::writePoints(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width) { - const std::string hdf5ColorsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; + const std::string hdf5ColorsPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "writing dataset to: " << hdf5ColorsPath; HighFive::DataSpace dataSpace({ height, width, 3 }); @@ -225,7 +225,7 @@ void Hdf5FbPointCloud::writeColorsRGB(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width) { - const std::string hdf5ColorsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; + const std::string hdf5ColorsPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "writing dataset to: " << hdf5ColorsPath; HighFive::DataSpace dataSpace({ height, width, 4 }); @@ -275,7 +275,7 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id, const bool withoutData) { const std::scoped_lock lock(*m_write_mtx); - const std::string hdf5GroupPath = HDF5_GROUP_POINTCLOUD + "/" + id; + const std::string hdf5GroupPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "loading flatbuffers point cloud from: " << hdf5GroupPath; @@ -421,28 +421,33 @@ void Hdf5FbPointCloud::readGeneralAttributes(const std::string& id, std::shared_ uint32_t& height, uint32_t& width, uint32_t& pointStep, uint32_t& rowStep, bool& isBigendian, bool& isDense) { - height = readAttributeFromHdf5(id, *dataGroupPtr, HEIGHT); - width = readAttributeFromHdf5(id, *dataGroupPtr, WIDTH); - pointStep = readAttributeFromHdf5(id, *dataGroupPtr, POINT_STEP); - rowStep = readAttributeFromHdf5(id, *dataGroupPtr, ROW_STEP); - isBigendian = readAttributeFromHdf5(id, *dataGroupPtr, IS_BIGENDIAN); - isDense = readAttributeFromHdf5(id, *dataGroupPtr, IS_DENSE); + height = readAttributeFromHdf5(id, *dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT); + width = readAttributeFromHdf5(id, *dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::WIDTH); + pointStep = readAttributeFromHdf5(id, *dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP); + rowStep = readAttributeFromHdf5(id, *dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP); + isBigendian = readAttributeFromHdf5(id, *dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN); + isDense = readAttributeFromHdf5(id, *dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE); } void Hdf5FbPointCloud::readPointFields(const std::string& id, std::shared_ptr dataGroupPtr, std::vector& names, std::vector& offsets, std::vector& counts, std::vector& datatypes) { - names = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_NAME); - offsets = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_OFFSET); - counts = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_COUNT); - datatypes = readAttributeFromHdf5>(id, *dataGroupPtr, FIELD_DATATYPE); + names = readAttributeFromHdf5>(id, *dataGroupPtr, + seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME); + offsets = readAttributeFromHdf5>(id, *dataGroupPtr, + seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET); + counts = readAttributeFromHdf5>(id, *dataGroupPtr, + seerep_hdf5_core::Hdf5CorePointCloud::FIELD_COUNT); + datatypes = readAttributeFromHdf5>(id, *dataGroupPtr, + seerep_hdf5_core::Hdf5CorePointCloud::FIELD_DATATYPE); } void Hdf5FbPointCloud::readPoints(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width) { - const std::string hdf5DatasetPointsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/points"; + const std::string hdf5DatasetPointsPath = + seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id + "/points"; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "reading points from: " << hdf5DatasetPointsPath; @@ -471,7 +476,8 @@ void Hdf5FbPointCloud::readPoints(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width) { - const std::string hdf5DatasetPointsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; + const std::string hdf5DatasetPointsPath = + seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "reading rgb from: " << hdf5DatasetPointsPath; @@ -499,7 +505,8 @@ void Hdf5FbPointCloud::readColorsRGB(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width) { - const std::string hdf5DatasetPointsPath = HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; + const std::string hdf5DatasetPointsPath = + seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "reading rgba from: " << hdf5DatasetPointsPath; @@ -531,7 +538,8 @@ Hdf5FbPointCloud::readLabelsGeneralOffset(flatbuffers::grpc::MessageBuilder& bui { std::vector labelsGeneral; std::vector labelsGeneralInstances; - readLabelsGeneral(HDF5_GROUP_POINTCLOUD, id, labelsGeneral, labelsGeneralInstances); + readLabelsGeneral(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, labelsGeneral, + labelsGeneralInstances); std::vector> labelGeneral; labelGeneral.reserve(labelsGeneral.size()); @@ -555,7 +563,8 @@ Hdf5FbPointCloud::readLabelsBoundingBoxOffset(flatbuffers::grpc::MessageBuilder& std::vector boundingBoxesLabels; std::vector> boundingBoxes; std::vector boundingBoxesInstances; - readBoundingBoxLabeled(HDF5_GROUP_POINTCLOUD, id, boundingBoxesLabels, boundingBoxes, boundingBoxesInstances); + readBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, boundingBoxesLabels, + boundingBoxes, boundingBoxesInstances); std::vector> boundingBoxLabeled; for (size_t i = 0; i < boundingBoxes.size(); i++) From 25d0a37384a251ddb8690d561139f00eaba9b16c Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 10 Oct 2022 08:01:25 +0000 Subject: [PATCH 30/54] remove point cloud modifier for flatbuffers point cloud Flatbuffers messages are not intended to be changed after they are created. Also, I currently don't see a use case for the modifier --- .../hdf5-fb-point-cloud2-iterator.h | 98 +------------------ 1 file changed, 1 insertion(+), 97 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h index 839eb18f1..8e24f3a06 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h @@ -41,35 +41,7 @@ #include /** - * @brief Tools for manipulating sensor_msgs - * - * This file provides two sets of utilities to modify and parse PointCloud2 - * The first set allows you to conveniently set the fields by hand: - * - * @verbatim - * #include - * // Create a PointCloud2 - * seerep::PointCloud2 cloud_msg; - * // Fill some internals of the PoinCloud2 like the header/width/height ... - * cloud_msgs.height = 1; cloud_msgs.width = 4; - * // Set the point fields to xyzrgb and resize the vector with the following command - * // 4 is for the number of added fields. Each come in triplet: the name of the PointField, - * // the number of occurrences of the type in the PointField, the type of the PointField - * seerep::PointCloud2Modifier modifier(cloud_msg); - * modifier.setPointCloud2Fields(4, "x", 1, seerep::PointField::FLOAT32, - * "y", 1, seerep::PointField::FLOAT32, - * "z", 1, seerep::PointField::FLOAT32, - * "rgb", 1, seerep::PointField::FLOAT32); - * // For convenience and the xyz, rgb, rgba fields, you can also use the following overloaded function. - * // You have to be aware that the following function does add extra padding for backward compatibility though - * // so it is definitely the solution of choice for PointXYZ and PointXYZRGB - * // 2 is for the number of fields to add - * modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); - * // You can then reserve / resize as usual - * modifier.resize(100); - * @endverbatim - * - * The second set allows you to traverse your PointCloud using an iterator: + * @brief Iterator to parse a flatbuffers PointCloud2 * * @verbatim * // Define some raw data we'll put in the PointCloud2 @@ -101,74 +73,6 @@ namespace seerep_hdf5_fb { -/** - * @brief Enables modifying a seerep::PointCloud2 like a container - */ -// class PointCloud2Modifier -// { -// public: -// /** -// * @brief Default constructor -// * @param cloud_msg The seerep::PointCloud2 to modify -// */ -// explicit PointCloud2Modifier(seerep::PointCloud2& cloud_msg); - -// /** -// * @return the number of T's in the original seerep::PointCloud2 -// */ -// size_t size() const; - -// /** -// * @param size The number of T's to reserve in the original seerep::PointCloud2 for -// */ -// void reserve(size_t size); - -// /** -// * @param size The number of T's to change the size of the original seerep::PointCloud2 by -// */ -// void resize(size_t size); - -// /** -// * @brief remove all T's from the original seerep::PointCloud2 -// */ -// void clear(); - -// /** -// * @brief Function setting some fields in a PointCloud and adjusting the -// * internals of the PointCloud2 -// * @param n_fields the number of fields to add. The fields are given as triplets: name of the -// * field as char*, number of elements in the field, the datatype of the field, the datatype -// * of the elements in the field -// * -// * E.g, you create your PointCloud2 message with XYZ/RGB as follows: -// * @verbatim -// * setPointCloud2Fields(cloud_msg, 4, "x", 1, seerep::PointField::FLOAT32, -// * "y", 1, seerep::PointField::FLOAT32, -// * "z", 1, seerep::PointField::FLOAT32, -// * "rgb", 1, seerep::PointField::FLOAT32); -// * @endverbatim -// * WARNING: THIS DOES NOT TAKE INTO ACCOUNT ANY PADDING -// * For simple usual cases, the overloaded setPointCloud2FieldsByString is what you want. -// */ -// void setPointCloud2Fields(int n_fields, ...); - -// /** -// * @brief Function setting some fields in a PointCloud and adjusting the -// * internals of the PointCloud2 -// * @param n_fields the number of fields to add. The fields are given as -// * strings: "xyz" (3 floats), "rgb" (3 uchar stacked in a float), -// * "rgba" (4 uchar stacked in a float) -// * @return void -// * -// * WARNING: THIS FUNCTION ADDS ANY NECESSARY PADDING TRANSPARENTLY -// */ -// void setPointCloud2FieldsByString(int n_fields, ...); - -// protected: -// /** A reference to the original seerep::PointCloud2 that we read */ -// seerep::PointCloud2& cloud_msg_; -// }; - namespace impl { /** Private base class for PointCloud2Iterator and PointCloud2ConstIterator From 817500f426c10a94dedefc0fdd931393c4c785dc Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 10 Oct 2022 08:40:18 +0000 Subject: [PATCH 31/54] rename fb point cloud iterator --- .../hdf5-fb-point-cloud2-iterator.h | 23 ++--- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 12 +-- .../impl/hdf5-fb-point-cloud2-iterator.hpp | 84 ++++--------------- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 55 ++++++------ 4 files changed, 62 insertions(+), 112 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h index 8e24f3a06..03e2b907a 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h @@ -36,7 +36,6 @@ #include #include -#include #include #include @@ -81,7 +80,7 @@ namespace impl * U is the type of the raw data in PointCloud2 (only uchar and const uchar are supported) * V is the derived class (yop, curiously recurring template pattern) */ -template class V> +template class V> class PointCloud2IteratorBase { public: @@ -158,7 +157,8 @@ class PointCloud2IteratorBase * @brief Helper iterators for reading and writing the data field of a flatbuffers point cloud message */ template -class PointCloud2WriteIterator : public impl::PointCloud2IteratorBase +class PointCloud2Iterator + : public impl::PointCloud2IteratorBase { public: /** @@ -168,9 +168,9 @@ class PointCloud2WriteIterator : public impl::PointCloud2IteratorBase::PointCloud2IteratorBase() + PointCloud2Iterator(uint8_t* data, uint32_t offset, uint32_t pointStep, uint32_t height, uint32_t width) + : impl::PointCloud2IteratorBase::PointCloud2IteratorBase() { this->field_offset_ = offset; this->point_step_ = pointStep; @@ -184,8 +184,9 @@ class PointCloud2WriteIterator : public impl::PointCloud2IteratorBase -class PointCloud2ReadIterator - : public impl::PointCloud2IteratorBase +class PointCloud2ConstIterator + : public impl::PointCloud2IteratorBase { public: /** @@ -194,9 +195,9 @@ class PointCloud2ReadIterator * @param cloudMsg the flatbuffers point cloud message to use * @param fieldName the field to iterate over */ - PointCloud2ReadIterator(uint8_t* data, uint32_t offset, uint32_t height, uint32_t width, u_int32_t pointStep) - : impl::PointCloud2IteratorBase::PointCloud2IteratorBase() + PointCloud2ConstIterator(const uint8_t* data, uint32_t offset, uint32_t height, uint32_t width, uint32_t pointStep) + : impl::PointCloud2IteratorBase::PointCloud2IteratorBase() { this->field_offset_ = offset; this->point_step_ = pointStep; diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index fa67283df..4c0ab1b93 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -66,7 +66,7 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral else dataset_ptr = std::make_shared(m_file->getDataSet(id)); - PointCloud2ReadIterator iter(cloud, field_name); + PointCloud2ConstIterator iter(cloud, field_name); std::vector data; data.reserve(size); @@ -104,14 +104,14 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral } /** write */ - void writePoints(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, + void writePoints(const std::string& id, const std::vector& offsets, const uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width, const std::shared_ptr& groupPtr); - void writeColorsRGB(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, - uint32_t height, uint32_t width); + void writeColorsRGB(const std::string& id, const std::vector& offsets, const uint8_t* data, + uint32_t pointStep, uint32_t height, uint32_t width); - void writeColorsRGBA(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, - uint32_t height, uint32_t width); + void writeColorsRGBA(const std::string& id, const std::vector& offsets, const uint8_t* data, + uint32_t pointStep, uint32_t height, uint32_t width); /** read */ diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp index a862f8b6b..6e30ac47f 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp @@ -40,60 +40,12 @@ #include #include -// namespace -// { -// /** Return the size of a datatype (which is an enum of seerep::PointField::) in bytes -// * @param datatype one of the enums of seerep::PointField:: -// */ -// inline int sizeOfPointField(int datatype) -// { -// switch (datatype) -// { -// case seerep::fb::Point_Field_Datatype_INT8: -// case seerep::fb::Point_Field_Datatype_UINT8: -// return 1; -// case seerep::fb::Point_Field_Datatype_INT16: -// case seerep::fb::Point_Field_Datatype_UINT16: -// return 2; -// case seerep::fb::Point_Field_Datatype_INT32: -// case seerep::fb::Point_Field_Datatype_UINT32: -// case seerep::fb::Point_Field_Datatype_FLOAT32: -// return 4; -// case seerep::fb::Point_Field_Datatype_FLOAT64: -// return 8; - -// default: -// std::stringstream err; -// err << "PointField of type " << datatype << " does not exist" << std::endl; -// throw std::runtime_error(err.str()); -// return -1; -// } -// } - -// /** Private function that adds a PointField to the "fields" member of a PointCloud2 -// * @param cloud_msg the PointCloud2 to add a field to -// * @param name the name of the field -// * @param count the number of elements in the PointField -// * @param datatype the datatype of the elements -// * @param offset the offset of that element -// * @return the offset of the next PointField that will be added to the PointCloud2 -// */ -// inline int addPointField(seerep::fb::PointCloud2& cloud_msg, const std::string& name, int count, int datatype, -// int offset) -// { -// // ToDO add point_field to point cloud if it's needed - -// // Update the offset -// return offset + count * sizeOfPointField(datatype); -// } -// } // namespace - namespace seerep_hdf5_fb { namespace impl { -template class V> -PointCloud2IteratorBase::PointCloud2IteratorBase() +template class V> +PointCloud2IteratorBase::PointCloud2IteratorBase() { } @@ -101,8 +53,8 @@ PointCloud2IteratorBase::PointCloud2IteratorBase() * @param iter the iterator to copy data from * @return a reference to *this */ -template class V> -V& PointCloud2IteratorBase::operator=(const V& iter) +template class V> +V& PointCloud2IteratorBase::operator=(const V& iter) { if (this != &iter) { @@ -121,8 +73,8 @@ V& PointCloud2IteratorBase::operator=(const V& iter) * @param i * @return a reference to the i^th value from the current position */ -template class V> -TT& PointCloud2IteratorBase::operator[](size_t i) const +template class V> +TT& PointCloud2IteratorBase::operator[](size_t i) const { return *(data_ + i); } @@ -130,8 +82,8 @@ TT& PointCloud2IteratorBase::operator[](size_t i) const /** Dereference the iterator. Equivalent to accessing it through [0] * @return the value to which the iterator is pointing */ -template class V> -TT& PointCloud2IteratorBase::operator*() const +template class V> +TT& PointCloud2IteratorBase::operator*() const { return *data_; } @@ -139,8 +91,8 @@ TT& PointCloud2IteratorBase::operator*() const /** Increase the iterator to the next element * @return a reference to the updated iterator */ -template class V> -V& PointCloud2IteratorBase::operator++() +template class V> +V& PointCloud2IteratorBase::operator++() { data_char_ += point_step_; data_ = reinterpret_cast(data_char_); @@ -151,8 +103,8 @@ V& PointCloud2IteratorBase::operator++() * @param i the amount to increase the iterator by * @return an iterator with an increased position */ -template class V> -V PointCloud2IteratorBase::operator+(int i) +template class V> +V PointCloud2IteratorBase::operator+(int i) { V res = *static_cast*>(this); @@ -165,8 +117,8 @@ V PointCloud2IteratorBase::operator+(int i) /** Increase the iterator by a certain amount * @return a reference to the updated iterator */ -template class V> -V& PointCloud2IteratorBase::operator+=(int i) +template class V> +V& PointCloud2IteratorBase::operator+=(int i) { data_char_ += i * point_step_; data_ = reinterpret_cast(data_char_); @@ -176,8 +128,8 @@ V& PointCloud2IteratorBase::operator+=(int i) /** Compare to another iterator * @return whether the current iterator points to a different address than the other one */ -template class V> -bool PointCloud2IteratorBase::operator!=(const V& iter) const +template class V> +bool PointCloud2IteratorBase::operator!=(const V& iter) const { return iter.data_ != data_; } @@ -185,8 +137,8 @@ bool PointCloud2IteratorBase::operator!=(const V& iter) const /** Return the end iterator * @return the end iterator (useful when performing normal iterator processing with ++) */ -template class V> -V PointCloud2IteratorBase::end() const +template class V> +V PointCloud2IteratorBase::end() const { V res = *static_cast*>(this); res.data_ = data_end_; diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index f1302a619..b9feacb40 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -67,8 +67,7 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: xyzOffsets.push_back(getOffset(cloud, field)); } - writePoints(id, xyzOffsets, const_cast(cloud.data()->data()), cloud.point_step(), cloud.height(), - cloud.width(), dataGroupPtr); + writePoints(id, xyzOffsets, cloud.data()->data(), cloud.point_step(), cloud.height(), cloud.width(), dataGroupPtr); } if (info.has_rgb) @@ -80,8 +79,7 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: rgbOffsets.push_back(getOffset(cloud, field)); } - writeColorsRGBA(id, rgbOffsets, const_cast(cloud.data()->data()), cloud.point_step(), cloud.height(), - cloud.width()); + writeColorsRGBA(id, rgbOffsets, cloud.data()->data(), cloud.point_step(), cloud.height(), cloud.width()); } if (info.has_rgba) @@ -93,8 +91,7 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: rgbaOffsets.push_back(getOffset(cloud, field)); } - writeColorsRGBA(id, rgbaOffsets, const_cast(cloud.data()->data()), cloud.point_step(), cloud.height(), - cloud.width()); + writeColorsRGBA(id, rgbaOffsets, cloud.data()->data(), cloud.point_step(), cloud.height(), cloud.width()); } // TODO normals @@ -104,7 +101,7 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: m_file->flush(); } -void Hdf5FbPointCloud::writePoints(const std::string& id, const std::vector& offsets, uint8_t* data, +void Hdf5FbPointCloud::writePoints(const std::string& id, const std::vector& offsets, const uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width, const std::shared_ptr& groupPtr) { @@ -135,9 +132,9 @@ void Hdf5FbPointCloud::writePoints(const std::string& id, const std::vector::max(); max[0] = max[1] = max[2] = std::numeric_limits::min(); - seerep_hdf5_fb::PointCloud2ReadIterator xIter(data, offsets[0], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ReadIterator yIter(data, offsets[1], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ReadIterator zIter(data, offsets[2], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ConstIterator xIter(data, offsets[0], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ConstIterator yIter(data, offsets[1], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ConstIterator zIter(data, offsets[2], height, width, pointStep); for (size_t row = 0; row < height; row++) { @@ -179,7 +176,7 @@ void Hdf5FbPointCloud::writePoints(const std::string& id, const std::vectorwrite(pointData); } -void Hdf5FbPointCloud::writeColorsRGB(const std::string& id, const std::vector& offsets, uint8_t* data, +void Hdf5FbPointCloud::writeColorsRGB(const std::string& id, const std::vector& offsets, const uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width) { const std::string hdf5ColorsPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; @@ -205,9 +202,9 @@ void Hdf5FbPointCloud::writeColorsRGB(const std::string& id, const std::vector>> colorsData; colorsData.resize(height); - seerep_hdf5_fb::PointCloud2ReadIterator rIter(data, offsets[0], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ReadIterator gIter(data, offsets[1], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ReadIterator bIter(data, offsets[2], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ConstIterator rIter(data, offsets[0], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ConstIterator gIter(data, offsets[1], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ConstIterator bIter(data, offsets[2], height, width, pointStep); for (size_t row = 0; row < height; row++) { @@ -222,7 +219,7 @@ void Hdf5FbPointCloud::writeColorsRGB(const std::string& id, const std::vectorwrite(colorsData); } -void Hdf5FbPointCloud::writeColorsRGBA(const std::string& id, const std::vector& offsets, uint8_t* data, +void Hdf5FbPointCloud::writeColorsRGBA(const std::string& id, const std::vector& offsets, const uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width) { const std::string hdf5ColorsPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id + "/colors"; @@ -248,10 +245,10 @@ void Hdf5FbPointCloud::writeColorsRGBA(const std::string& id, const std::vector< std::vector>> colorsData; colorsData.resize(height); - seerep_hdf5_fb::PointCloud2ReadIterator rIter(data, offsets[0], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ReadIterator gIter(data, offsets[1], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ReadIterator bIter(data, offsets[2], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ReadIterator aIter(data, offsets[3], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ConstIterator rIter(data, offsets[0], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ConstIterator gIter(data, offsets[1], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ConstIterator bIter(data, offsets[2], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ConstIterator aIter(data, offsets[3], height, width, pointStep); for (size_t row = 0; row < height; row++) { @@ -457,9 +454,9 @@ void Hdf5FbPointCloud::readPoints(const std::string& id, const std::vector>> pointData; pointsDataset.read(pointData); - seerep_hdf5_fb::PointCloud2WriteIterator xIter(data, offsets[0], pointStep, height, width); - seerep_hdf5_fb::PointCloud2WriteIterator yIter(data, offsets[1], pointStep, height, width); - seerep_hdf5_fb::PointCloud2WriteIterator zIter(data, offsets[2], pointStep, height, width); + seerep_hdf5_fb::PointCloud2Iterator xIter(data, offsets[0], pointStep, height, width); + seerep_hdf5_fb::PointCloud2Iterator yIter(data, offsets[1], pointStep, height, width); + seerep_hdf5_fb::PointCloud2Iterator zIter(data, offsets[2], pointStep, height, width); for (auto col : pointData) { @@ -486,9 +483,9 @@ void Hdf5FbPointCloud::readColorsRGB(const std::string& id, const std::vector>> colorData; colorsDataset.read(colorData); - seerep_hdf5_fb::PointCloud2WriteIterator rIter(data, offsets[0], pointStep, height, width); - seerep_hdf5_fb::PointCloud2WriteIterator gIter(data, offsets[1], pointStep, height, width); - seerep_hdf5_fb::PointCloud2WriteIterator bIter(data, offsets[2], pointStep, height, width); + seerep_hdf5_fb::PointCloud2Iterator rIter(data, offsets[0], pointStep, height, width); + seerep_hdf5_fb::PointCloud2Iterator gIter(data, offsets[1], pointStep, height, width); + seerep_hdf5_fb::PointCloud2Iterator bIter(data, offsets[2], pointStep, height, width); for (auto col : colorData) { @@ -515,10 +512,10 @@ void Hdf5FbPointCloud::readColorsRGBA(const std::string& id, const std::vector>> colorData; colorsDataset.read(colorData); - seerep_hdf5_fb::PointCloud2WriteIterator rIter(data, offsets[0], pointStep, height, width); - seerep_hdf5_fb::PointCloud2WriteIterator gIter(data, offsets[1], pointStep, height, width); - seerep_hdf5_fb::PointCloud2WriteIterator bIter(data, offsets[2], pointStep, height, width); - seerep_hdf5_fb::PointCloud2WriteIterator aIter(data, offsets[3], pointStep, height, width); + seerep_hdf5_fb::PointCloud2Iterator rIter(data, offsets[0], pointStep, height, width); + seerep_hdf5_fb::PointCloud2Iterator gIter(data, offsets[1], pointStep, height, width); + seerep_hdf5_fb::PointCloud2Iterator bIter(data, offsets[2], pointStep, height, width); + seerep_hdf5_fb::PointCloud2Iterator aIter(data, offsets[3], pointStep, height, width); for (auto col : colorData) { From 75d4832081bcf1c8b045faff66c6f3d73ee87743 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 10 Oct 2022 09:07:47 +0000 Subject: [PATCH 32/54] add docs in seerep-hdf5-fb for point clouds --- .../hdf5-fb-point-cloud2-iterator.h | 66 ++----- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 179 +++++++++++++++++- .../impl/hdf5-fb-point-cloud2-iterator.hpp | 1 + 3 files changed, 188 insertions(+), 58 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h index 03e2b907a..38c2672c4 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h @@ -39,37 +39,6 @@ #include #include -/** - * @brief Iterator to parse a flatbuffers PointCloud2 - * - * @verbatim - * // Define some raw data we'll put in the PointCloud2 - * float point_data[] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0}; - * uint8_t color_data[] = {40, 80, 120, 160, 200, 240, 20, 40, 60, 80, 100, 120}; - * // Define the iterators. When doing so, you define the Field you would like to iterate upon and - * // the type of you would like returned: it is not necessary the type of the PointField as sometimes - * // you pack data in another type (e.g. 3 uchar + 1 uchar for RGB are packed in a float) - * sensor_msgs::PointCloud2Iterator iter_x(cloud_msg, "x"); - * sensor_msgs::PointCloud2Iterator iter_y(cloud_msg, "y"); - * sensor_msgs::PointCloud2Iterator iter_z(cloud_msg, "z"); - * // Even though the r,g,b,a fields do not exist (it's usually rgb, rgba), you can create iterators for - * // those: they will handle data packing for you (in little endian RGB is packed as \*,R,G,B in a float - * // and RGBA as A,R,G,B) - * sensor_msgs::PointCloud2Iterator iter_r(cloud_msg, "r"); - * sensor_msgs::PointCloud2Iterator iter_g(cloud_msg, "g"); - * sensor_msgs::PointCloud2Iterator iter_b(cloud_msg, "b"); - * // Fill the PointCloud2 - * for(size_t i=0; i end() const; - // TODO find common place for calculation of the offset - - /** Common code to set the field of the PointCloud2 - * @param cloud_msg the PointCloud2 to modify - * @param field_name the name of the field to iterate upon - * @return the offset at which the field is found - */ - - /** The raw data in uchar* where the iterator is */ + /** The raw data in uchar* where the iterator is */ U* data_char_; /** The cast data where the iterator is */ TT* data_; @@ -154,7 +115,7 @@ class PointCloud2IteratorBase } // namespace impl /** - * @brief Helper iterators for reading and writing the data field of a flatbuffers point cloud message + * @brief Class that can iterate over a flatbuffers PointCloud2 (used for writing into the data field) */ template class PointCloud2Iterator @@ -162,11 +123,13 @@ class PointCloud2Iterator { public: /** - * @brief Iterator to use for iterating over an pre-allocated data field and writing the values + * @brief Construct a new PointCloud2Iterator * - * @param data a pointer to the start of the data field - * @param offset the offset of the field that should be written - * @param size the length of the data field + * @param data pointer to a uint8_t array to iterate over + * @param offset the offset of the field to iterate on + * @param pointStep the pointStep of the point cloud + * @param height the height of the point cloud (1 for unorganized point clouds) + * @param width the width of the point cloud */ PointCloud2Iterator(uint8_t* data, uint32_t offset, uint32_t pointStep, uint32_t height, uint32_t width) : impl::PointCloud2IteratorBasepoint_step_ = pointStep; this->data_char_ = reinterpret_cast(data + this->field_offset_); this->data_ = reinterpret_cast(this->data_char_); + // last element is at: offset + (n -1) * pointStep this->data_end_ = reinterpret_cast(data + offset + (height * width - 1) * pointStep); } }; /** - * @brief Reading iterator for the data field of a received flatbuffers point cloud message + * @brief Class that can iterate over a flatbuffers PointCloud2 (used for reading the data field) */ template class PointCloud2ConstIterator @@ -189,11 +153,15 @@ class PointCloud2ConstIterator PointCloud2ConstIterator> { public: + // TODO same parameter order as above /** - * @brief Iterator over the data field of a received flatbuffers point cloud message + * @brief Construct a new PointCloud2ConstIterator * - * @param cloudMsg the flatbuffers point cloud message to use - * @param fieldName the field to iterate over + * @param data pointer to a const uint8_t array to iterate over + * @param offset the offset of the field to iterate on + * @param height the height of the point cloud (1 for unorganized point clouds) + * @param width the width of the point cloud + * @param pointStep the pointStep of the point cloud */ PointCloud2ConstIterator(const uint8_t* data, uint32_t offset, uint32_t height, uint32_t width, uint32_t pointStep) : impl::PointCloud2IteratorBase& file, std::shared_ptr& write_mtx); + /** + * @brief Method for writing a flatbuffers PointCloud2 message to hdf5 + * + * @param uuid the uuid of the point cloud + * @param pointcloud2 the received PointCloud2 message + */ void writePointCloud2(const std::string& uuid, const seerep::fb::PointCloud2& pointcloud2); + /** + * @brief Method for reading a flatbuffers PointCloud2 message from hdf5 + * + * @param uuid the uuid of the point cloud + * @param withoutData omit the data field + * @return std::optional> returns the + */ std::optional> readPointCloud2(const std::string& uuid, const bool withoutData = false); private: + /** + * @brief struct to store information about the PointCloud2 + * + */ struct CloudInfo { bool has_points = false; @@ -38,6 +61,7 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral bool has_normals = false; }; + // TODO remove template void read(const std::string& id, const std::string& fieldName, std::vector& data, size_t size) { @@ -53,6 +77,7 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral dataset.read(data); } + // TODO remove template void write(const std::string cloud_uuid, const std::string& field_name, const seerep::fb ::PointCloud2& cloud, size_t size) @@ -79,6 +104,12 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral dataset_ptr->write(data); } + /** + * @brief Writes information about point fields as attributes to hdf5 + * + * @param object HighFive object to write to (representing a data set or data group) + * @param pointFields flatbuffers vector of the point fields to write + */ template void writePointFieldAttributes(HighFive::AnnotateTraits& object, const flatbuffers::Vector>& pointFields) @@ -103,52 +134,182 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral writeAttributeToHdf5>(object, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_COUNT, counts); } - /** write */ + /** + * @brief Write x,y,z of a point cloud to a /points dataset + * + * @param id the uuid of the point cloud + * @param offsets the offsets of the x,y,z fields + * @param data pointer to a const uint8_t array to read the elements from + * @param pointStep the point step of the point cloud + * @param height the height of the point cloud (1 of unorganized point clouds) + * @param width the width of the point cloud + * @param groupPtr shared pointer to the data group of the point cloud. + * (Used to add a bounding box of x,y,z as an attribute of the point cloud) + */ void writePoints(const std::string& id, const std::vector& offsets, const uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width, const std::shared_ptr& groupPtr); + /** + * @brief Write r,g,b of a point cloud to a /colors dataset + * + * @param id the uuid of the point cloud + * @param offsets the offsets of the r,g,b fields + * @param data pointer to a const uint8_t array to read the elements from + * @param pointStep the point step of the point cloud + * @param height the height of the point cloud (1 of unorganized point clouds) + * @param width the width of the point cloud + */ void writeColorsRGB(const std::string& id, const std::vector& offsets, const uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width); + /** + * @brief Write r,g,b,a of a point cloud to a /colors dataset + * + * @param id the uuid of the point cloud + * @param offsets the offsets of the r,g,b,a fields + * @param data pointer to a const uint8_t array to read the elements from + * @param pointStep the point step of the point cloud + * @param height the height of the point cloud (1 of unorganized point clouds) + * @param width the width of the point cloud + */ void writeColorsRGBA(const std::string& id, const std::vector& offsets, const uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width); - /** read */ - + /** + * @brief Reads general attributes from hdf5 + * + * @param id the uuid of the point cloud + * @param dataGroupPtr shared pointer to the data group of the point cloud + * @param height reference to which the height is written + * @param width reference to which the width is written + * @param pointStep reference to which the pointStep is written + * @param rowStep reference to which the rowStep is written + * @param isBigendian reference to which isBigendian is written + * @param isDense reference to which isDense is written + */ void readGeneralAttributes(const std::string& id, std::shared_ptr dataGroupPtr, uint32_t& height, uint32_t& width, uint32_t& pointStep, uint32_t& rowStep, bool& isBigendian, bool& isDense); - + /** + * @brief Reads point field information from hdf5 + * + * @param id uuid of the point cloud + * @param dataGroupPtr shared pointer to the data group of the point cloud + * @param names reference to a vector to which the names are written + * @param offsets reference to a vector to which the offsets are written + * @param counts reference to a vector to which the count are written + * @param datatypes reference to a vector to which the datatypes are written + */ void readPointFields(const std::string& id, std::shared_ptr dataGroupPtr, std::vector& names, std::vector& offsets, std::vector& counts, std::vector& datatypes); - + /** + * @brief Read x,y,z from hdf5 + * + * @param id the uuid of the point cloud + * @param offsets the offsets of the x,y,z fields + * @param data pointer to a uint8_t array to store the data in + * @param pointStep the pointStep of the point cloud + * @param height the height of the point cloud (1 for unorganized point clouds) + * @param width the width the width of the point cloud + */ void readPoints(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width); - + /** + * @brief Read r,g,b from hdf5 + * + * @param id the uuid of the point cloud + * @param offsets the offsets of the r,g,b fields + * @param data pointer to a uint8_t array to store the data in + * @param pointStep the pointStep of the point cloud + * @param height the height of the point cloud (1 for unorganized point clouds) + * @param width the width the width of the point cloud + */ void readColorsRGB(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width); - + /** + * @brief Read r,g,b,a from hdf5 + * + * @param id the uuid of the point cloud + * @param offsets the offsets of the r,g,b,a fields + * @param data pointer to a uint8_t array to store the data in + * @param pointStep the pointStep of the point cloud + * @param height the height of the point cloud (1 for unorganized point clouds) + * @param width the width the width of the point cloud + */ void readColorsRGBA(const std::string& id, const std::vector& offsets, uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width); - /** helpers for building the point cloud message*/ + /** + * @brief Helper method to construct a flatbuffers vector of LabelWithInstance + * + * @param builder reference to the used flatbuffers builder + * @param id the uuid of the point cloud + * @return flatbuffers::Offset>> + * flatbuffers vector of LabelWithInstance + */ flatbuffers::Offset>> readLabelsGeneralOffset(flatbuffers::grpc::MessageBuilder& builder, const std::string& id); + /** + * @brief Helper method to construct a flatbuffers vector of BoundingBoxLabeled + * + * @param builder reference to the used flatbuffers builder + * @param id the uuid of the point cloud + * @return flatbuffers::Offset>> + * flatbuffers vector of BoundingBoxLabeled + */ flatbuffers::Offset>> readLabelsBoundingBoxOffset(flatbuffers::grpc::MessageBuilder& builder, const std::string& id); + /** + * @brief Helper method to construct a flatbuffers vector of pointFields + * + * @param builder builder reference to the used flatbuffers builder + * @param names vector of pointField names + * @param offsets vector of pointField offsets + * @param counts vector of pointField counts + * @param datatypes vector of pointField datatypes + * @return flatbuffers::Offset>> + * flatbuffers vector of pointFields + */ flatbuffers::Offset>> readPointFieldsOffset(flatbuffers::grpc::MessageBuilder& builder, std::vector& names, std::vector& offsets, std::vector& counts, std::vector& datatypes); - /** helpers for managing point cloud information*/ + /** + * @brief Get information about the fields of a point cloud from a flatbuffers PointCloud2 message + * + * @param cloud the point cloud to get the information about + * @return CloudInfo extracted information in form of a CloudInfo struct + */ CloudInfo getCloudInfo(const seerep::fb::PointCloud2& cloud); + /** + * @brief Get information about the fields of a point cloud from a vector of fields + * + * @param fields vector of field names of a point cloud + * @return CloudInfo extracted information in form of a CloudInfo struct + */ CloudInfo getCloudInfo(const std::vector& fields); + /** + * @brief Get the offset of a field from a flatbuffers PointCloud2 message + * + * @param cloud the point cloud to get the information from + * @param fieldName the name of the field to get the offset for + * @return uint32_t offset of the fieldName + */ uint32_t getOffset(const seerep::fb::PointCloud2& cloud, const std::string& fieldName); + /** + * @brief Get the offset of a field from hdf5 + * + * @param names the names of the pointFields + * @param offsets the offsets of the pointFields + * @param fieldName the fieldName to get the offset for + * @param isBigendian endianness of the point cloud + * @return uint32_t the offset of the fieldName + */ uint32_t getOffset(const std::vector& names, const std::vector& offsets, const std::string& fieldName, bool isBigendian); }; diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp index 6e30ac47f..648e55af8 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp @@ -44,6 +44,7 @@ namespace seerep_hdf5_fb { namespace impl { +// TODO remove default constructor template class V> PointCloud2IteratorBase::PointCloud2IteratorBase() { From ab5bd5bf8ce3dbad4dfede7e3882e8fa818fd823 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 10 Oct 2022 12:43:11 +0000 Subject: [PATCH 33/54] remove unused methods from hdfb-fb-pointcloud --- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 43 ------------------- 1 file changed, 43 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index 615278f96..cb703c10e 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -61,49 +61,6 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral bool has_normals = false; }; - // TODO remove - template - void read(const std::string& id, const std::string& fieldName, std::vector& data, size_t size) - { - const std::string hdf5DatasetFieldPath = - seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id + "/" + fieldName; - - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) - << "reading " << fieldName << " from: " << hdf5DatasetFieldPath; - - HighFive::DataSet dataset = m_file->getDataSet(id); - - data.reserve(size); - dataset.read(data); - } - - // TODO remove - template - void write(const std::string cloud_uuid, const std::string& field_name, const seerep::fb ::PointCloud2& cloud, - size_t size) - { - const std::string id = seerep_hdf5_core::Hdf5CorePointCloud::BOUNDINGBOX + "/" + cloud_uuid + "/" + field_name; - HighFive::DataSpace data_space(size); - - std::shared_ptr dataset_ptr; - if (!m_file->exist(id)) - dataset_ptr = std::make_shared(m_file->createDataSet(id, data_space)); - else - dataset_ptr = std::make_shared(m_file->getDataSet(id)); - - PointCloud2ConstIterator iter(cloud, field_name); - std::vector data; - data.reserve(size); - - for (size_t i = 0; i < size; i++) - { - data.push_back(*iter); - ++iter; - } - - dataset_ptr->write(data); - } - /** * @brief Writes information about point fields as attributes to hdf5 * From c46a765ff858df7df62702146f4cce4afbe7bf0f Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 10 Oct 2022 12:52:52 +0000 Subject: [PATCH 34/54] update flatbuffers pointcloud2iterator --- .../hdf5-fb-point-cloud2-iterator.h | 6 +++--- .../impl/hdf5-fb-point-cloud2-iterator.hpp | 6 ------ .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 20 +++++++++---------- 3 files changed, 13 insertions(+), 19 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h index 38c2672c4..446eb08e2 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-point-cloud2-iterator.h @@ -53,7 +53,7 @@ template c class PointCloud2IteratorBase { public: - PointCloud2IteratorBase(); + PointCloud2IteratorBase() = default; /** Assignment operator * @param iter the iterator to copy data from @@ -159,11 +159,11 @@ class PointCloud2ConstIterator * * @param data pointer to a const uint8_t array to iterate over * @param offset the offset of the field to iterate on + * @param pointStep the pointStep of the point cloud * @param height the height of the point cloud (1 for unorganized point clouds) * @param width the width of the point cloud - * @param pointStep the pointStep of the point cloud */ - PointCloud2ConstIterator(const uint8_t* data, uint32_t offset, uint32_t height, uint32_t width, uint32_t pointStep) + PointCloud2ConstIterator(const uint8_t* data, uint32_t offset, uint32_t pointStep, uint32_t height, uint32_t width) : impl::PointCloud2IteratorBase::PointCloud2IteratorBase() { diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp index 648e55af8..c7c631386 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-point-cloud2-iterator.hpp @@ -44,12 +44,6 @@ namespace seerep_hdf5_fb { namespace impl { -// TODO remove default constructor -template class V> -PointCloud2IteratorBase::PointCloud2IteratorBase() -{ -} - /** Assignment operator * @param iter the iterator to copy data from * @return a reference to *this diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index b9feacb40..378975aea 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -132,9 +132,9 @@ void Hdf5FbPointCloud::writePoints(const std::string& id, const std::vector::max(); max[0] = max[1] = max[2] = std::numeric_limits::min(); - seerep_hdf5_fb::PointCloud2ConstIterator xIter(data, offsets[0], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ConstIterator yIter(data, offsets[1], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ConstIterator zIter(data, offsets[2], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ConstIterator xIter(data, offsets[0], pointStep, height, width); + seerep_hdf5_fb::PointCloud2ConstIterator yIter(data, offsets[1], pointStep, height, width); + seerep_hdf5_fb::PointCloud2ConstIterator zIter(data, offsets[2], pointStep, height, width); for (size_t row = 0; row < height; row++) { @@ -202,9 +202,9 @@ void Hdf5FbPointCloud::writeColorsRGB(const std::string& id, const std::vector>> colorsData; colorsData.resize(height); - seerep_hdf5_fb::PointCloud2ConstIterator rIter(data, offsets[0], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ConstIterator gIter(data, offsets[1], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ConstIterator bIter(data, offsets[2], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ConstIterator rIter(data, offsets[0], pointStep, height, width); + seerep_hdf5_fb::PointCloud2ConstIterator gIter(data, offsets[1], pointStep, height, width); + seerep_hdf5_fb::PointCloud2ConstIterator bIter(data, offsets[2], pointStep, height, width); for (size_t row = 0; row < height; row++) { @@ -245,10 +245,10 @@ void Hdf5FbPointCloud::writeColorsRGBA(const std::string& id, const std::vector< std::vector>> colorsData; colorsData.resize(height); - seerep_hdf5_fb::PointCloud2ConstIterator rIter(data, offsets[0], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ConstIterator gIter(data, offsets[1], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ConstIterator bIter(data, offsets[2], height, width, pointStep); - seerep_hdf5_fb::PointCloud2ConstIterator aIter(data, offsets[3], height, width, pointStep); + seerep_hdf5_fb::PointCloud2ConstIterator rIter(data, offsets[0], pointStep, height, width); + seerep_hdf5_fb::PointCloud2ConstIterator gIter(data, offsets[1], pointStep, height, width); + seerep_hdf5_fb::PointCloud2ConstIterator bIter(data, offsets[2], pointStep, height, width); + seerep_hdf5_fb::PointCloud2ConstIterator aIter(data, offsets[3], pointStep, height, width); for (size_t row = 0; row < height; row++) { From 3ade86505dfd0cc2c9f0c1d392b7d9bb1d5108ac Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 10 Oct 2022 13:44:56 +0000 Subject: [PATCH 35/54] add docs to core-fb-pointcloud --- .../seerep-core-fb/core-fb-pointcloud.h | 32 ++++++++++++++++++- 1 file changed, 31 insertions(+), 1 deletion(-) diff --git a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h index 4048f7b21..d225c62ee 100644 --- a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h +++ b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h @@ -27,21 +27,51 @@ namespace seerep_core_fb { +/** + * @brief This class is the center piece between the gRPC interface, the core and the hdf5-io for point clouds + * + * The functions of this class are called by the corresponding gRPC services. The data storing and loading is done + * directly via the hdf5-io-fb. When adding new data the needed information for the indices are disclosed to the core. + * When data is queried the core is consulted to get the UUIDs of the data answering the query. The data is then + * loaded from this class via the hdf5-io. + */ class CoreFbPointCloud { public: + /** + * @brief Constructs the point cloud specific object based on the general core + * + * @param seerepCore a shared pointer to the general core + */ CoreFbPointCloud(std::shared_ptr seerepCore); + /** + * @brief Function to query point clouds + * + * @param query the flatbuffer query + * @param writer the writer object used to send the point clouds matching the query directly via gRPC + * + * Based on the query the indices are used to get the uuids of the point clouds matching the query. Then the point + * clouds are loaded by the hdf5-fb-io and send via gRPC directly using the writer + */ void getData(const seerep::fb::Query* query, grpc::ServerWriter>* const writer); + /** + * @brief Add a point cloud to the indices and write the data to hdf5 + * + * @param pc the point cloud message to index and save + * @return boost::uuids::uuid the uuid of the stored image + */ boost::uuids::uuid addData(const seerep::fb::PointCloud2& pc); private: - std::shared_ptr getHdf5(boost::uuids::uuid project); + /** @brief a shared pointer to the general core */ std::shared_ptr m_seerepCore; + /** @brief a map from the uuids of the project to the hdf5-io objects handling the io for the object*/ std::unordered_map, boost::hash> m_hdf5IoMap; + /** @brief the logger for the logging framework*/ boost::log::sources::severity_logger m_logger; }; From 87dd1ea836d4f656de2db4ccb1bf54c4215ccd1f Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 10 Oct 2022 14:12:22 +0000 Subject: [PATCH 36/54] remove temporary fix from core-fb-pointcloud --- .../seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h | 3 +++ .../seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h | 2 -- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index cb703c10e..4014e7fdd 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -13,6 +13,9 @@ // seerep-msgs #include +// seerep-com +#include + // std #include #include diff --git a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h index d225c62ee..bf85d7120 100644 --- a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h +++ b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-pointcloud.h @@ -15,8 +15,6 @@ // seerep-core #include -// add as temporary fix -#include // uuid #include From 0c2b42ed82e49e738ae54f70441ad68b4c554849 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 10 Oct 2022 14:22:00 +0000 Subject: [PATCH 37/54] remove quaternion attributes from hdf5-core-point-cloud --- .../include/seerep-hdf5-core/hdf5-core-point-cloud.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-point-cloud.h b/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-point-cloud.h index aae32ee6a..7fe14f4f3 100644 --- a/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-point-cloud.h +++ b/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-point-cloud.h @@ -50,12 +50,6 @@ class Hdf5CorePointCloud : public Hdf5CoreGeneral, public Hdf5CoreDatatypeInterf inline static const std::string FIELD_DATATYPE = "field_datatype"; inline static const std::string FIELD_COUNT = "field_count"; - // point and quaternion attribute keys - const std::string X = "x"; - const std::string Y = "y"; - const std::string Z = "z"; - const std::string W = "w"; - // make private again after fixing io calls of pointcloud.cpp and pointcloud-overview.cpp inline static const std::string BOUNDINGBOX = "boundingbox"; // datatype group names in hdf5 From c997cc8bd8eb135756cf6cbc1c850e2512cd16b6 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 11 Oct 2022 07:09:36 +0000 Subject: [PATCH 38/54] add docs to flatbuffers point cloud service --- .../seerep-server/fb-point-cloud-service.h | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h b/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h index 33dcc724f..05613e477 100644 --- a/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h +++ b/seerep-srv/seerep-server/include/seerep-server/fb-point-cloud-service.h @@ -14,20 +14,49 @@ namespace seerep_server { +/** + * @brief Class which implements the flatbuffers PointCloudService defined in seerep-com. + * Main task is to forward the calls to the core-fb specific write and read functions. + * + */ class FbPointCloudService final : public seerep::fb::PointCloudService::Service { public: + /** + * @brief Constructs the service based on the general core + * + * @param seerepCore a shared pointer to the general core + */ FbPointCloudService(std::shared_ptr seerepCore); + /** + * @brief Stream point clouds matching the query to the client + * + * @param context custom inital and trailing metadata (currently not used) + * @param request incoming gRPC query request + * @param writer writer object used to stream the point clouds + * @return grpc::Status status of the request. Did it work? + */ grpc::Status GetPointCloud2(grpc::ServerContext* context, const flatbuffers::grpc::Message* request, grpc::ServerWriter>* writer) override; + /** + * @brief Save point clouds from the incoming gRPC stream on the server + * + * @param context custom inital and trailing metadata (currently not used) + * @param reader incoming message stream of point clouds from the client + * @param response gRPC message to describe the transmission state of the point clouds + * @return grpc::Status status of the request. Did it work? + */ grpc::Status TransferPointCloud2(grpc::ServerContext* context, grpc::ServerReader>* reader, flatbuffers::grpc::Message* response) override; private: + /** @brief a shared pointer to the general core */ std::shared_ptr pointCloudFb; + /** @brief the logger for the logging framework */ boost::log::sources::severity_logger m_logger; }; } /* namespace seerep_server */ + #endif // SEEREP_SERVER_FB_POINT_CLOUD_SERVICE_H_ From 7c30d32c354450b9d3885de4c3d0c78298b93f00 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 11 Oct 2022 08:24:31 +0000 Subject: [PATCH 39/54] fix: funcions call in hdf5-fb-point and hdf5-fb-image after rebase --- seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp | 15 +++------------ seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp | 6 +----- 2 files changed, 4 insertions(+), 17 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp index 0ff758bc4..eb8d2db51 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp @@ -75,13 +75,8 @@ void Hdf5FbImage::writeImage(const std::string& id, const seerep::fb::Image& ima data_set_ptr->write(tmp); writeHeaderAttributes(*data_set_ptr, *image.header()); -<<<<<<< HEAD - writeBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, image.labels_bb()); - writeLabelsGeneral(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, image.labels_general()); -======= - writeBoundingBox2DLabeled(HDF5_GROUP_IMAGE, id, *image.labels_bb()); - writeLabelsGeneral(HDF5_GROUP_IMAGE, id, *image.labels_general()); ->>>>>>> refactor to only use reference where possible in hdf5 fb + writeBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, *image.labels_bb()); + writeLabelsGeneral(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, *image.labels_general()); m_file->flush(); } @@ -91,11 +86,7 @@ void Hdf5FbImage::writeImageBoundingBox2DLabeled(const std::string& id, { const std::scoped_lock lock(*m_write_mtx); -<<<<<<< HEAD - writeBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, bb2dLabeledStamped.labels_bb()); -======= - writeBoundingBox2DLabeled(HDF5_GROUP_IMAGE, id, *bb2dLabeledStamped.labels_bb()); ->>>>>>> refactor to only use reference where possible in hdf5 fb + writeBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, *bb2dLabeledStamped.labels_bb()); } std::optional> Hdf5FbImage::readImage(const std::string& id, diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp index 00889bd1d..eb87a5e83 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp @@ -43,11 +43,7 @@ void Hdf5FbPoint::writePoint(const std::string& id, const seerep::fb::PointStamp writeAttributeMap(data_set_ptr, *point->attribute()); -<<<<<<< HEAD - writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePoint::HDF5_GROUP_POINT, id, point->labels_general()); -======= - writeLabelsGeneral(HDF5_GROUP_POINT, id, *point->labels_general()); ->>>>>>> refactor to only use reference where possible in hdf5 fb + writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePoint::HDF5_GROUP_POINT, id, *point->labels_general()); m_file->flush(); } From 9b872b52918db8b482940a133045f71150f34327 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 11 Oct 2022 13:21:39 +0000 Subject: [PATCH 40/54] fix: general labels not in indicies for flatbuffer point clouds --- seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp b/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp index 01a402e64..5575fe966 100644 --- a/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp +++ b/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp @@ -116,12 +116,18 @@ seerep_core_msgs::DatasetIndexable CoreFbConversion::fromFb(const seerep::fb::Po dataForIndices.boundingbox.max_corner().set<2>(0); // semantic - int labelSizeBB = 0; + int labelSizeAll = 0; + if (cloud.labels_general()) + { + labelSizeAll += cloud.labels_general()->size(); + } if (cloud.labels_bb()) { - labelSizeBB = cloud.labels_bb()->size(); + labelSizeAll += cloud.labels_bb()->size(); } - dataForIndices.labelsWithInstances.reserve(labelSizeBB); + dataForIndices.labelsWithInstances.reserve(labelSizeAll); + + fromFbDataLabelsGeneral(cloud.labels_general(), dataForIndices.labelsWithInstances); fromFbDataLabelsGeneral(cloud.labels_bb(), dataForIndices.labelsWithInstances); return dataForIndices; From cc8f42d5b522e6f7d6693698184fe845a34ce1d0 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 11 Oct 2022 13:57:57 +0000 Subject: [PATCH 41/54] update header and query in fb_util to use optional parameters --- .../pointcloud/gRPC_fb_queryPointCloud.py | 7 +- .../gRPC/pointcloud/gRPC_fb_sendPointCloud.py | 2 +- examples/python/gRPC/util_fb.py | 82 +++++++++++++------ 3 files changed, 64 insertions(+), 27 deletions(-) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py index 7e4662beb..1f576602d 100644 --- a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py @@ -28,14 +28,15 @@ builder = flatbuffers.Builder(1024) -# cerate time interval - timeMin = util_fb.createTimeStamp(builder, 1610549273, 0) timeMax = util_fb.createTimeStamp(builder, 1938549273, 0) timeInterval = util_fb.createTimeInterval(builder, timeMin, timeMax) queryMsg = util_fb.createQuery( - builder, [builder.CreateString(projectUuid)], timeInterval, [builder.CreateString("BoundingBoxLabel0")] + builder, + projectUuids=[builder.CreateString(projectUuid)], + timeInterval=timeInterval, + labels=[builder.CreateString("BoundingBoxLabel0")], ) builder.Finish(queryMsg) buf = builder.Output() diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py index a079172b6..788eb96ed 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py @@ -91,7 +91,7 @@ def createPointClouds(projectUuid, numOf): builder = flatbuffers.Builder(1024) timeStamp = createTimeStamp(builder, theTime + i) - header = createHeader(builder, timeStamp, "camera", projectUuid) + header = createHeader(builder, timeStamp, "map", projectUuid) pointCloudMsg = createPointCloud(builder, header) builder.Finish(pointCloudMsg) diff --git a/examples/python/gRPC/util_fb.py b/examples/python/gRPC/util_fb.py index 16cd985bc..8a8cd9515 100644 --- a/examples/python/gRPC/util_fb.py +++ b/examples/python/gRPC/util_fb.py @@ -1,7 +1,5 @@ import sys -import time -import flatbuffers from fb import ( Boundingbox, BoundingBoxLabeled, @@ -73,7 +71,6 @@ def getOrCreateProject(builder, channel, name, create=True, mapFrameId="map"): return projectUuid -# Header def createTimeStamp(builder, seconds, nanoseconds=0): '''Create a time stamp in flatbuffers''' Timestamp.Start(builder) @@ -82,14 +79,19 @@ def createTimeStamp(builder, seconds, nanoseconds=0): return Timestamp.End(builder) -def createHeader(builder, timeStamp, frame, projectUuid): - '''Creates a message header in flatbuffers''' - frameStr = builder.CreateString(frame) - projectUuidStr = builder.CreateString(projectUuid) +def createHeader(builder, timeStamp=None, frame=None, projectUuid=None): + '''Creates a message header in flatbuffers, all parameters are optional''' + if frame: + frameStr = builder.CreateString(frame) + if projectUuid: + projectUuidStr = builder.CreateString(projectUuid) Header.Start(builder) - Header.AddFrameId(builder, frameStr) - Header.AddStamp(builder, timeStamp) - Header.AddUuidProject(builder, projectUuidStr) + if frame: + Header.AddFrameId(builder, frameStr) + if timeStamp: + Header.AddStamp(builder, timeStamp) + if projectUuid: + Header.AddUuidProject(builder, projectUuidStr) return Header.End(builder) @@ -204,22 +206,56 @@ def addToPointFieldVector(builder, pointFieldList): return builder.EndVector() -def createQuery(builder, projectUuids, timeInterval, generalLabels, withoutData=False): - # add project uuids - Query.StartProjectuuidVector(builder, len(projectUuids)) - for projectUuid in reversed(projectUuids): - builder.PrependUOffsetTRelative(projectUuid) - projectUuidsOffset = builder.EndVector() +def createQuery( + builder, + boundingBox=None, + timeInterval=None, + labels=None, + projectUuids=None, + instanceUuids=None, + dataUuids=None, + withoutData=False, +): + '''Create a query, all parameters are optional''' - Query.StartLabelVector(builder, len(generalLabels)) - for label in reversed(generalLabels): - builder.PrependUOffsetTRelative(label) - labelsOffset = builder.EndVector() + # Note: reverse because we prepend + if projectUuids: + Query.StartProjectuuidVector(builder, len(projectUuids)) + for projectUuid in reversed(projectUuids): + builder.PrependUOffsetTRelative(projectUuid) + projectUuidsOffset = builder.EndVector() + + if labels: + Query.StartLabelVector(builder, len(labels)) + for label in reversed(labels): + builder.PrependUOffsetTRelative(label) + labelsOffset = builder.EndVector() + + if instanceUuids: + Query.StartInstanceuuidVector(builder, len(instanceUuids)) + for instance in reversed(instanceUuids): + builder.PrependUOffsetTRelative(instance) + instanceOffset = builder.EndVector() + + if dataUuids: + Query.StartDatauuidVector(builder, len(dataUuids)) + for dataUuid in reversed(dataUuids): + builder.PrependUOffsetTRelative(dataUuid) + dataUuidOffset = builder.EndVector() Query.Start(builder) - Query.AddProjectuuid(builder, projectUuidsOffset) - Query.AddTimeinterval(builder, timeInterval) - Query.AddLabel(builder, labelsOffset) + if boundingBox: + Query.AddBoundingbox(builder, boundingBox) + if timeInterval: + Query.AddTimeinterval(builder, timeInterval) + if labels: + Query.AddLabel(builder, labelsOffset) + if projectUuids: + Query.AddProjectuuid(builder, projectUuidsOffset) + if instanceUuids: + Query.AddInstanceuuid(builder, instanceOffset) + if dataUuids: + Query.QueryAddDatauuid(builder, dataUuidOffset) Query.AddWithoutdata(builder, withoutData) return Query.End(builder) From 5d1c8e745a4d147106e8867e61691c47c2289db6 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Wed, 12 Oct 2022 07:46:36 +0000 Subject: [PATCH 42/54] fix: bounding box indecies for flatbuffer point clouds This is currently the only way to get the bounding box into the indecies without looping over the x,y,z data twice --- .../include/seerep-hdf5-fb/hdf5-fb-pointcloud.h | 8 ++++++-- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 10 ++++++---- seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp | 8 -------- seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp | 12 +++++++++++- 4 files changed, 23 insertions(+), 15 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index 4014e7fdd..e8f5409d5 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -38,8 +38,10 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral * * @param uuid the uuid of the point cloud * @param pointcloud2 the received PointCloud2 message + * @param boundingBox reference to an vector which get's the computed boundingBox during write of x,y,z */ - void writePointCloud2(const std::string& uuid, const seerep::fb::PointCloud2& pointcloud2); + void writePointCloud2(const std::string& uuid, const seerep::fb::PointCloud2& pointcloud2, + std::vector& boundingBox); /** * @brief Method for reading a flatbuffers PointCloud2 message from hdf5 @@ -105,9 +107,11 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral * @param width the width of the point cloud * @param groupPtr shared pointer to the data group of the point cloud. * (Used to add a bounding box of x,y,z as an attribute of the point cloud) + * @param boundingBox reference to vector to write the computed boundingBox */ void writePoints(const std::string& id, const std::vector& offsets, const uint8_t* data, uint32_t pointStep, - uint32_t height, uint32_t width, const std::shared_ptr& groupPtr); + uint32_t height, uint32_t width, const std::shared_ptr& groupPtr, + std::vector& boundingBox); /** * @brief Write r,g,b of a point cloud to a /colors dataset diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 378975aea..5dcd10db2 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -11,7 +11,8 @@ Hdf5FbPointCloud::Hdf5FbPointCloud(std::shared_ptr& file, std::s // TODO refactor again, still not happy with the API -void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb::PointCloud2& cloud) +void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb::PointCloud2& cloud, + std::vector& boundingBox) { const std::scoped_lock lock(*m_write_mtx); @@ -67,7 +68,8 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: xyzOffsets.push_back(getOffset(cloud, field)); } - writePoints(id, xyzOffsets, cloud.data()->data(), cloud.point_step(), cloud.height(), cloud.width(), dataGroupPtr); + writePoints(id, xyzOffsets, cloud.data()->data(), cloud.point_step(), cloud.height(), cloud.width(), dataGroupPtr, + boundingBox); } if (info.has_rgb) @@ -103,7 +105,7 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: void Hdf5FbPointCloud::writePoints(const std::string& id, const std::vector& offsets, const uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width, - const std::shared_ptr& groupPtr) + const std::shared_ptr& groupPtr, std::vector& boundingBox) { std::string hdf5PointsPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id + "/points"; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "writing dataset to: " << hdf5PointsPath; @@ -168,7 +170,7 @@ void Hdf5FbPointCloud::writePoints(const std::string& id, const std::vector(0); - dataForIndices.boundingbox.min_corner().set<1>(0); - dataForIndices.boundingbox.min_corner().set<2>(0); - dataForIndices.boundingbox.max_corner().set<0>(0); - dataForIndices.boundingbox.max_corner().set<1>(0); - dataForIndices.boundingbox.max_corner().set<2>(0); - // semantic int labelSizeAll = 0; if (cloud.labels_general()) diff --git a/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp index 1765b8b3f..4020f67e2 100644 --- a/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp +++ b/seerep-srv/seerep-core-fb/src/core-fb-pointcloud.cpp @@ -34,12 +34,22 @@ void CoreFbPointCloud::getData(const seerep::fb::Query* query, } } +// TODO refactor only a temporary solution boost::uuids::uuid CoreFbPointCloud::addData(const seerep::fb::PointCloud2& pc) { + std::vector boundingBox; + seerep_core_msgs::DatasetIndexable dataForIndices = CoreFbConversion::fromFb(pc); auto hdf5io = CoreFbGeneral::getHdf5(dataForIndices.header.uuidProject, m_seerepCore, m_hdf5IoMap); - hdf5io->writePointCloud2(boost::lexical_cast(dataForIndices.header.uuidData), pc); + hdf5io->writePointCloud2(boost::lexical_cast(dataForIndices.header.uuidData), pc, boundingBox); + + dataForIndices.boundingbox.min_corner().set<0>(boundingBox[0]); + dataForIndices.boundingbox.min_corner().set<1>(boundingBox[1]); + dataForIndices.boundingbox.min_corner().set<2>(boundingBox[2]); + dataForIndices.boundingbox.max_corner().set<0>(boundingBox[3]); + dataForIndices.boundingbox.max_corner().set<1>(boundingBox[4]); + dataForIndices.boundingbox.max_corner().set<2>(boundingBox[5]); m_seerepCore->addDataset(dataForIndices); From e4bb7bd742ea5a3dc33b926507c265579279b4c3 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Wed, 12 Oct 2022 08:02:55 +0000 Subject: [PATCH 43/54] remove TODO in seerep-core-fb-conversions --- seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp b/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp index 6f6d5cb2e..4405c94da 100644 --- a/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp +++ b/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp @@ -276,7 +276,6 @@ void CoreFbConversion::fromFbDataHeader(const seerep::fb::Header* header, seerep coreHeader.uuidData = boost::uuids::random_generator()(); } - // ToDo change datatype accordingly coreHeader.datatype = datatype; coreHeader.frameId = header->frame_id()->str(); coreHeader.timestamp.seconds = header->stamp()->seconds(); From fdce344cd9a05270e7c0de11ad4372b51812a637 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Thu, 13 Oct 2022 07:02:53 +0000 Subject: [PATCH 44/54] add checks for empty flatbuffers message fields inside write function stop using references here so that the arguments can be null and checked inside the function --- .../include/seerep-hdf5-fb/hdf5-fb-general.h | 10 +-- .../seerep-hdf5-fb/impl/hdf5-fb-general.hpp | 43 ++++++----- .../seerep-hdf5-fb/src/hdf5-fb-general.cpp | 77 ++++++++++--------- .../seerep-hdf5-fb/src/hdf5-fb-image.cpp | 8 +- .../seerep-hdf5-fb/src/hdf5-fb-point.cpp | 8 +- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 13 +--- 6 files changed, 79 insertions(+), 80 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h index 8f2a02442..94f5d6bfb 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h @@ -37,13 +37,13 @@ class Hdf5FbGeneral : public seerep_hdf5_core::Hdf5CoreGeneral // Attributes //################ void writeAttributeMap(const std::shared_ptr dataSetPtr, - const flatbuffers::Vector>& attributes); + const flatbuffers::Vector>* attributes); template flatbuffers::Offset>> readAttributeMap(HighFive::AnnotateTraits& object, flatbuffers::grpc::MessageBuilder& builder); template - void writeHeaderAttributes(HighFive::AnnotateTraits& object, const seerep::fb::Header& header); + void writeHeaderAttributes(HighFive::AnnotateTraits& object, const seerep::fb::Header* header); template flatbuffers::Offset readHeaderAttributes(flatbuffers::grpc::MessageBuilder& builder, @@ -55,17 +55,17 @@ class Hdf5FbGeneral : public seerep_hdf5_core::Hdf5CoreGeneral //################ void writeBoundingBoxLabeled( const std::string& datatypeGroup, const std::string& uuid, - const flatbuffers::Vector>& boundingboxLabeled); + const flatbuffers::Vector>* boundingboxLabeled); void writeBoundingBox2DLabeled( const std::string& datatypeGroup, const std::string& uuid, - const flatbuffers::Vector>& boundingbox2DLabeled); + const flatbuffers::Vector>* boundingbox2DLabeled); //################ // Labels General //################ void writeLabelsGeneral(const std::string& datatypeGroup, const std::string& uuid, - const flatbuffers::Vector>& labelsGeneral); + const flatbuffers::Vector>* labelsGeneral); }; } // namespace seerep_hdf5_fb diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-general.hpp b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-general.hpp index e64587589..7476f6a2e 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-general.hpp +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/impl/hdf5-fb-general.hpp @@ -35,27 +35,30 @@ Hdf5FbGeneral::readAttributeMap(HighFive::AnnotateTraits& object, flatbuffers } template -void Hdf5FbGeneral::writeHeaderAttributes(HighFive::AnnotateTraits& object, const seerep::fb::Header& header) +void Hdf5FbGeneral::writeHeaderAttributes(HighFive::AnnotateTraits& object, const seerep::fb::Header* header) { - if (!object.hasAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_STAMP_SECONDS)) - object.createAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_STAMP_SECONDS, header.stamp()->seconds()); - else - object.getAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_STAMP_SECONDS).write(header.stamp()->seconds()); - - if (!object.hasAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_STAMP_NANOS)) - object.createAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_STAMP_NANOS, header.stamp()->nanos()); - else - object.getAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_STAMP_NANOS).write(header.stamp()->nanos()); - - if (!object.hasAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_FRAME_ID)) - object.createAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_FRAME_ID, header.frame_id()->str()); - else - object.getAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_FRAME_ID).write(header.frame_id()->str()); - - if (!object.hasAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_SEQ)) - object.createAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_SEQ, header.seq()); - else - object.getAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_SEQ).write(header.seq()); + if (header) + { + if (!object.hasAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_STAMP_SECONDS)) + object.createAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_STAMP_SECONDS, header->stamp()->seconds()); + else + object.getAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_STAMP_SECONDS).write(header->stamp()->seconds()); + + if (!object.hasAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_STAMP_NANOS)) + object.createAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_STAMP_NANOS, header->stamp()->nanos()); + else + object.getAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_STAMP_NANOS).write(header->stamp()->nanos()); + + if (!object.hasAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_FRAME_ID)) + object.createAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_FRAME_ID, header->frame_id()->str()); + else + object.getAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_FRAME_ID).write(header->frame_id()->str()); + + if (!object.hasAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_SEQ)) + object.createAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_SEQ, header->seq()); + else + object.getAttribute(seerep_hdf5_core::Hdf5CoreGeneral::HEADER_SEQ).write(header->seq()); + } } template diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp index 25e37271b..76552a642 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-general.cpp @@ -11,53 +11,56 @@ Hdf5FbGeneral::Hdf5FbGeneral(std::shared_ptr& file, std::shared_ void Hdf5FbGeneral::writeAttributeMap( const std::shared_ptr dataSetPtr, - const flatbuffers::Vector>& attributes) + const flatbuffers::Vector>* attributes) { - for (auto attribute : attributes) + if (attributes) { - if (attribute->value_type() == seerep::fb::Datatypes_Boolean) + for (auto attribute : *attributes) { - writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), - static_cast(attribute->value())->data()); - } - else if (attribute->value_type() == seerep::fb::Datatypes_Integer) - { - writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), - static_cast(attribute->value())->data()); - } - else if (attribute->value_type() == seerep::fb::Datatypes_Double) - { - writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), - static_cast(attribute->value())->data()); - } - else if (attribute->value_type() == seerep::fb::Datatypes_String) - { - writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), - static_cast(attribute->value())->data()->str()); - } - else - { - std::stringstream errorMsg; - errorMsg << "type " << attribute->value_type() << " of attribute " << attribute->key()->c_str() - << " not implemented in hdf5-io."; - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << errorMsg.str(); - throw std::invalid_argument(errorMsg.str()); + if (attribute->value_type() == seerep::fb::Datatypes_Boolean) + { + writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), + static_cast(attribute->value())->data()); + } + else if (attribute->value_type() == seerep::fb::Datatypes_Integer) + { + writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), + static_cast(attribute->value())->data()); + } + else if (attribute->value_type() == seerep::fb::Datatypes_Double) + { + writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), + static_cast(attribute->value())->data()); + } + else if (attribute->value_type() == seerep::fb::Datatypes_String) + { + writeAttributeToHdf5(*dataSetPtr, attribute->key()->str(), + static_cast(attribute->value())->data()->str()); + } + else + { + std::stringstream errorMsg; + errorMsg << "type " << attribute->value_type() << " of attribute " << attribute->key()->c_str() + << " not implemented in hdf5-io."; + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::error) << errorMsg.str(); + throw std::invalid_argument(errorMsg.str()); + } } } } void Hdf5FbGeneral::writeBoundingBoxLabeled( const std::string& datatypeGroup, const std::string& uuid, - const flatbuffers::Vector>& boundingboxLabeled) + const flatbuffers::Vector>* boundingboxLabeled) { - if (!boundingboxLabeled.size() == 0) + if (boundingboxLabeled && boundingboxLabeled->size() != 0) { std::string id = datatypeGroup + "/" + uuid; std::vector labels; std::vector> boundingBoxes; std::vector instances; - for (auto label : boundingboxLabeled) + for (auto label : *boundingboxLabeled) { labels.push_back(label->labelWithInstance()->label()->str()); std::vector box{ label->bounding_box()->point_min()->x(), label->bounding_box()->point_min()->y(), @@ -86,16 +89,16 @@ void Hdf5FbGeneral::writeBoundingBoxLabeled( void Hdf5FbGeneral::writeBoundingBox2DLabeled( const std::string& datatypeGroup, const std::string& uuid, - const flatbuffers::Vector>& boundingbox2DLabeled) + const flatbuffers::Vector>* boundingbox2DLabeled) { std::string id = datatypeGroup + "/" + uuid; - if (!boundingbox2DLabeled.size() == 0) + if (boundingbox2DLabeled && boundingbox2DLabeled->size() != 0) { std::vector labels; std::vector> boundingBoxes; std::vector instances; - for (auto label : boundingbox2DLabeled) + for (auto label : *boundingbox2DLabeled) { labels.push_back(label->labelWithInstance()->label()->str()); std::vector box{ label->bounding_box()->point_min()->x(), label->bounding_box()->point_min()->y(), @@ -123,15 +126,15 @@ void Hdf5FbGeneral::writeBoundingBox2DLabeled( void Hdf5FbGeneral::writeLabelsGeneral( const std::string& datatypeGroup, const std::string& uuid, - const flatbuffers::Vector>& labelsGeneral) + const flatbuffers::Vector>* labelsGeneral) { std::string id = datatypeGroup + "/" + uuid; - if (labelsGeneral.size() != 0) + if (labelsGeneral && labelsGeneral->size() != 0) { std::vector labels; std::vector instances; - for (auto label : labelsGeneral) + for (auto label : *labelsGeneral) { labels.push_back(label->label()->str()); diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp index eb8d2db51..4e5e49e0d 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-image.cpp @@ -73,10 +73,10 @@ void Hdf5FbImage::writeImage(const std::string& id, const seerep::fb::Image& ima } data_set_ptr->write(tmp); - writeHeaderAttributes(*data_set_ptr, *image.header()); + writeHeaderAttributes(*data_set_ptr, image.header()); - writeBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, *image.labels_bb()); - writeLabelsGeneral(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, *image.labels_general()); + writeBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, image.labels_bb()); + writeLabelsGeneral(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, image.labels_general()); m_file->flush(); } @@ -86,7 +86,7 @@ void Hdf5FbImage::writeImageBoundingBox2DLabeled(const std::string& id, { const std::scoped_lock lock(*m_write_mtx); - writeBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, *bb2dLabeledStamped.labels_bb()); + writeBoundingBox2DLabeled(seerep_hdf5_core::Hdf5CoreImage::HDF5_GROUP_IMAGE, id, bb2dLabeledStamped.labels_bb()); } std::optional> Hdf5FbImage::readImage(const std::string& id, diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp index eb87a5e83..4cb2a853b 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-point.cpp @@ -39,11 +39,11 @@ void Hdf5FbPoint::writePoint(const std::string& id, const seerep::fb::PointStamp } data_set_ptr->write(data); - writeHeaderAttributes(*data_set_ptr, *point->header()); + writeHeaderAttributes(*data_set_ptr, point->header()); - writeAttributeMap(data_set_ptr, *point->attribute()); + writeAttributeMap(data_set_ptr, point->attribute()); - writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePoint::HDF5_GROUP_POINT, id, *point->labels_general()); + writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePoint::HDF5_GROUP_POINT, id, point->labels_general()); m_file->flush(); } @@ -60,7 +60,7 @@ void Hdf5FbPoint::writeAdditionalPointAttributes(const seerep::fb::AttributesSta std::shared_ptr data_set_ptr = std::make_shared(m_file->getDataSet(hdf5DatasetRawDataPath)); - writeAttributeMap(data_set_ptr, *attributeStamped.attribute()); + writeAttributeMap(data_set_ptr, attributeStamped.attribute()); } catch (const std::exception& e) { diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 5dcd10db2..427d4474c 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -43,19 +43,12 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP, cloud.row_step()); writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE, cloud.is_dense()); - writeHeaderAttributes(*dataGroupPtr, *cloud.header()); + writeHeaderAttributes(*dataGroupPtr, cloud.header()); writePointFieldAttributes(*dataGroupPtr, *cloud.fields()); - if (cloud.labels_bb() != nullptr) - { - writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, *cloud.labels_bb()); - } - - if (cloud.labels_general() != nullptr) - { - writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, *cloud.labels_general()); - } + writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, cloud.labels_bb()); + writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, cloud.labels_general()); CloudInfo info = getCloudInfo(cloud); From 5ce375b7a3bb2831d6c1fc1e33131be96c90bc97 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 17 Oct 2022 11:35:18 +0000 Subject: [PATCH 45/54] add null check in write of point field attributes --- .../include/seerep-hdf5-fb/hdf5-fb-general.h | 2 +- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 34 +++++++++++-------- .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 2 +- 3 files changed, 21 insertions(+), 17 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h index 94f5d6bfb..87ee9dddd 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-general.h @@ -50,7 +50,7 @@ class Hdf5FbGeneral : public seerep_hdf5_core::Hdf5CoreGeneral HighFive::AnnotateTraits& object, std::string uuidMsg); - //################s + //################ // BoundingBoxes //################ void writeBoundingBoxLabeled( diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index e8f5409d5..9ac2a9240 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -74,26 +74,30 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral */ template void writePointFieldAttributes(HighFive::AnnotateTraits& object, - const flatbuffers::Vector>& pointFields) + const flatbuffers::Vector>* pointFields) { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "writing point field attributes to hdf5"; - std::vector names; - std::vector offsets, counts; - std::vector datatypes; - - for (auto pointField : pointFields) + if (pointFields) { - names.push_back(pointField->name()->str()); - offsets.push_back(pointField->offset()); - datatypes.push_back(static_cast(pointField->datatype())); - counts.push_back(pointField->count()); - } + std::vector names; + std::vector offsets, counts; + std::vector datatypes; - writeAttributeToHdf5>(object, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME, names); - writeAttributeToHdf5>(object, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET, offsets); - writeAttributeToHdf5>(object, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_DATATYPE, datatypes); - writeAttributeToHdf5>(object, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_COUNT, counts); + for (auto pointField : *pointFields) + { + names.push_back(pointField->name()->str()); + offsets.push_back(pointField->offset()); + datatypes.push_back(static_cast(pointField->datatype())); + counts.push_back(pointField->count()); + } + + writeAttributeToHdf5>(object, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_NAME, names); + writeAttributeToHdf5>(object, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_OFFSET, offsets); + writeAttributeToHdf5>(object, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_DATATYPE, + datatypes); + writeAttributeToHdf5>(object, seerep_hdf5_core::Hdf5CorePointCloud::FIELD_COUNT, counts); + } } /** diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 427d4474c..2cab7186e 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -45,7 +45,7 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: writeHeaderAttributes(*dataGroupPtr, cloud.header()); - writePointFieldAttributes(*dataGroupPtr, *cloud.fields()); + writePointFieldAttributes(*dataGroupPtr, cloud.fields()); writeBoundingBoxLabeled(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, cloud.labels_bb()); writeLabelsGeneral(seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD, id, cloud.labels_general()); From 02bc3aa15eadfbf3d9491b2b0f28f63ba1128ba7 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 17 Oct 2022 11:45:54 +0000 Subject: [PATCH 46/54] rename conversion functions in core-fb-conversion --- .../include/seerep-core-fb/core-fb-conversion.h | 8 ++++---- seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-conversion.h b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-conversion.h index 905f3dcb2..c790c8055 100644 --- a/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-conversion.h +++ b/seerep-srv/seerep-core-fb/include/seerep-core-fb/core-fb-conversion.h @@ -173,16 +173,16 @@ class CoreFbConversion * @param labelWithInstance the BoundingBox2DLabeled with instances in the data message in seerep core format */ static void - fromFbDataLabelsGeneral(const flatbuffers::Vector>* labelsBB2d, - std::vector& labelWithInstance); + fromFbDataLabelsBb2d(const flatbuffers::Vector>* labelsBB2d, + std::vector& labelWithInstance); /** * @brief converts the BoundingBoxLabeled with instances of the flatbuffer data message to seerep core specific message * @param labelsBB the BoundingBoxLabeled with instances in the flatbuffer data message * @param labelWithInstance the BoundingBoxLabeled with instances in the data message in seerep core format */ static void - fromFbDataLabelsGeneral(const flatbuffers::Vector>* labelsBB, - std::vector& labelWithInstance); + fromFbDataLabelsBb(const flatbuffers::Vector>* labelsBB, + std::vector& labelWithInstance); }; } // namespace seerep_core_fb diff --git a/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp b/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp index 4405c94da..a3780168e 100644 --- a/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp +++ b/seerep-srv/seerep-core-fb/src/core-fb-conversion.cpp @@ -71,7 +71,7 @@ seerep_core_msgs::DatasetIndexable CoreFbConversion::fromFb(const seerep::fb::Im dataForIndices.labelsWithInstances.reserve(labelSizeAll); fromFbDataLabelsGeneral(img.labels_general(), dataForIndices.labelsWithInstances); - fromFbDataLabelsGeneral(img.labels_bb(), dataForIndices.labelsWithInstances); + fromFbDataLabelsBb2d(img.labels_bb(), dataForIndices.labelsWithInstances); return dataForIndices; } @@ -120,7 +120,7 @@ seerep_core_msgs::DatasetIndexable CoreFbConversion::fromFb(const seerep::fb::Po dataForIndices.labelsWithInstances.reserve(labelSizeAll); fromFbDataLabelsGeneral(cloud.labels_general(), dataForIndices.labelsWithInstances); - fromFbDataLabelsGeneral(cloud.labels_bb(), dataForIndices.labelsWithInstances); + fromFbDataLabelsBb(cloud.labels_bb(), dataForIndices.labelsWithInstances); return dataForIndices; } @@ -324,7 +324,7 @@ void CoreFbConversion::fromFbDataLabelsGeneral( } } -void CoreFbConversion::fromFbDataLabelsGeneral( +void CoreFbConversion::fromFbDataLabelsBb2d( const flatbuffers::Vector>* labelsBB2d, std::vector& labelWithInstance) { @@ -349,7 +349,7 @@ void CoreFbConversion::fromFbDataLabelsGeneral( } } -void CoreFbConversion::fromFbDataLabelsGeneral( +void CoreFbConversion::fromFbDataLabelsBb( const flatbuffers::Vector>* labelsBB, std::vector& labelWithInstance) { From 059ea3041af2e03da916dd439c2771d498f96f17 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 17 Oct 2022 15:09:26 +0000 Subject: [PATCH 47/54] separate calculation of rgba offset in hdf5-fb-pointcloud --- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 14 ++ .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 147 +++++++----------- 2 files changed, 66 insertions(+), 95 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index 9ac2a9240..e46c5d9b9 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -280,6 +280,20 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral */ uint32_t getOffset(const std::vector& names, const std::vector& offsets, const std::string& fieldName, bool isBigendian); + + /** + * @brief Get the offset for the special case of rgba(a) + * + * The rgb(a) channel is encoded in a single 32 bit integer. + * To address an individual channel (r, g, b or a ), + * we need to get it's offset from the start of the rgb(a) field. + * + * @param fieldName name of the field to get the offset for i.e "r", "g", "b" or "a" + * @param offset the offset for the start of the rgb(a) channel + * @param isBigendian endianness of the point cloud + * @return uint32_t the offset for the fieldName + */ + uint32_t rgbaOffset(const std::string& fieldName, uint32_t offset, bool isBigendian); }; } // namespace seerep_hdf5_fb diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 2cab7186e..1f61c267b 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -36,6 +36,7 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: dataGroupPtr = std::make_shared(m_file->createGroup(hdf5GroupPath)); } + // TODO encapsulate in method writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT, cloud.height()); writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::WIDTH, cloud.width()); writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN, cloud.is_bigendian()); @@ -54,6 +55,7 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: if (info.has_points) { + // TODO create method for calculating the offsets const std::vector fields = { "x", "y", "z" }; std::vector xyzOffsets; for (auto field : fields) @@ -675,8 +677,6 @@ uint32_t Hdf5FbPointCloud::getOffset(const std::vector& names, cons } } - // Handle the special case of rgb(a), since the rgb(a) channel is encoded in a single 32 Bit integer - std::set fieldNames = { "r", "g", "b", "a" }; if (fieldNames.find(fieldName) != fieldNames.end()) @@ -685,50 +685,7 @@ uint32_t Hdf5FbPointCloud::getOffset(const std::vector& names, cons { if (names[i] == "rgb" || names[i] == "rgba") { - if (fieldName == "r") - { - if (isBigendian) - { - return offsets[i] + 1; - } - else - { - return offsets[i] + 2; - } - } - if (fieldName == "g") - { - if (isBigendian) - { - return offsets[i] + 2; - } - else - { - return offsets[i] + 1; - } - } - if (fieldName == "b") - { - if (isBigendian) - { - return offsets[i] + 3; - } - else - { - return offsets[i] + 0; - } - } - if (fieldName == "a") - { - if (isBigendian) - { - return offsets[i] + 0; - } - else - { - return offsets[i] + 3; - } - } + return rgbaOffset(fieldName, offsets[i], isBigendian); } } } @@ -737,8 +694,6 @@ uint32_t Hdf5FbPointCloud::getOffset(const std::vector& names, cons uint32_t Hdf5FbPointCloud::getOffset(const seerep::fb::PointCloud2& cloud, const std::string& fieldName) { - bool isBigendian = cloud.is_bigendian(); - for (size_t i = 0; i < cloud.fields()->size(); i++) { const seerep::fb::PointField& field = *cloud.fields()->Get(i); @@ -748,9 +703,6 @@ uint32_t Hdf5FbPointCloud::getOffset(const seerep::fb::PointCloud2& cloud, const } } - // // Handle the special case of r,g,b,a (we assume they are understood as the - // // channels of an rgb or rgba field) - std::set field_names = { "r", "g", "b", "a" }; if (field_names.find(fieldName) != field_names.end()) @@ -761,54 +713,59 @@ uint32_t Hdf5FbPointCloud::getOffset(const seerep::fb::PointCloud2& cloud, const if (field.name()->str() == "rgb" || field.name()->str() == "rgba") { - if (fieldName == "r") - { - if (isBigendian) - { - return field.offset() + 1; - } - else - { - return field.offset() + 2; - } - } - if (fieldName == "g") - { - if (isBigendian) - { - return field.offset() + 2; - } - else - { - return field.offset() + 1; - } - } - if (fieldName == "b") - { - if (isBigendian) - { - return field.offset() + 3; - } - else - { - return field.offset() + 0; - } - } - if (fieldName == "a") - { - if (isBigendian) - { - return field.offset() + 0; - } - else - { - return field.offset() + 3; - } - } + return rgbaOffset(fieldName, field.offset(), cloud.is_bigendian()); } } } throw std::runtime_error("Field " + fieldName + " does not exist!"); } +uint32_t Hdf5FbPointCloud::rgbaOffset(const std::string& fieldName, uint32_t offset, bool isBigendian) +{ + if (fieldName == "r") + { + if (isBigendian) + { + return offset + 1; + } + else + { + return offset + 2; + } + } + if (fieldName == "g") + { + if (isBigendian) + { + return offset + 2; + } + else + { + return offset + 1; + } + } + if (fieldName == "b") + { + if (isBigendian) + { + return offset + 3; + } + else + { + return offset + 0; + } + } + if (fieldName == "a") + { + if (isBigendian) + { + return offset + 0; + } + else + { + return offset + 3; + } + } +} + } // namespace seerep_hdf5_fb From 328d98b720c8ecf8aab13d12c69ba5d8e48b9a52 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Mon, 17 Oct 2022 15:29:32 +0000 Subject: [PATCH 48/54] add method for writing general attributs to the hdf5 data group in hdf5-fb-pointcloud" --- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 8 ++++++++ .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 19 ++++++++++++------- 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index e46c5d9b9..b2a61ede6 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -143,6 +143,14 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral void writeColorsRGBA(const std::string& id, const std::vector& offsets, const uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width); + /** + * @brief Write general attributes of the point cloud to the hdf5 group + * + * @param dataGroupPtr shared point to the data group + * @param cloud the point cloud to store information from + */ + void writeGeneralAttributes(std::shared_ptr& dataGroupPtr, const seerep::fb::PointCloud2& cloud); + /** * @brief Reads general attributes from hdf5 * diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 1f61c267b..3395ed00c 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -36,13 +36,7 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: dataGroupPtr = std::make_shared(m_file->createGroup(hdf5GroupPath)); } - // TODO encapsulate in method - writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT, cloud.height()); - writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::WIDTH, cloud.width()); - writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN, cloud.is_bigendian()); - writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP, cloud.point_step()); - writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP, cloud.row_step()); - writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE, cloud.is_dense()); + writeGeneralAttributes(dataGroupPtr, cloud); writeHeaderAttributes(*dataGroupPtr, cloud.header()); @@ -263,6 +257,17 @@ void Hdf5FbPointCloud::writeColorsRGBA(const std::string& id, const std::vector< colorsDatasetPtr->write(colorsData); } +void Hdf5FbPointCloud::writeGeneralAttributes(std::shared_ptr& dataGroupPtr, + const seerep::fb::PointCloud2& cloud) +{ + writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::HEIGHT, cloud.height()); + writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::WIDTH, cloud.width()); + writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::IS_BIGENDIAN, cloud.is_bigendian()); + writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::POINT_STEP, cloud.point_step()); + writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::ROW_STEP, cloud.row_step()); + writeAttributeToHdf5(*dataGroupPtr, seerep_hdf5_core::Hdf5CorePointCloud::IS_DENSE, cloud.is_dense()); +} + // TODO partial point cloud read std::optional> Hdf5FbPointCloud::readPointCloud2(const std::string& id, const bool withoutData) From 03f6215deaaa1056f880655c5941e0720895d094 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 18 Oct 2022 08:33:24 +0000 Subject: [PATCH 49/54] add hdf5 data set and data group accessors in hdf5-core-general --- .../seerep-hdf5-core/hdf5-core-general.h | 30 ++++++++- .../impl/hdf5-core-general.hpp | 18 +++++ .../src/hdf5-core-general.cpp | 17 +++++ .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 67 ++----------------- 4 files changed, 68 insertions(+), 64 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-general.h b/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-general.h index a747c6b37..3d15e4917 100644 --- a/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-general.h +++ b/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-general.h @@ -12,6 +12,7 @@ #include #include #include +#include // logging #include @@ -78,15 +79,38 @@ class Hdf5CoreGeneral void readLabelsGeneral(const std::string& datatypeGroup, const std::string& uuid, std::vector& labels, std::vector& instances); - // //################ - // // Project - // //################ + //################ + // Project + //################ void writeProjectname(const std::string& projectname); std::string readProjectname(); void writeProjectFrameId(const std::string& frameId); std::string readProjectFrameId(); + //################ + // Hdf5 + //################ + /** + * @brief Get a shared pointer to a hdf5 group specified by the hdf5GroupPath. + * + * If a group with the path already exists we use that, otherwise a new one is created. + * + * @param hdf5GroupPath path to the data group + * @return std::shared_ptr shared pointer to the group + */ + std::shared_ptr getHdf5Group(const std::string& hdf5GroupPath); + /** + * @brief Get a shared pointer to a hdf5 data set specified by the hdf5DataSetPath + * + * @tparam T type of the dataset + * @param hdf5DataSetPath path to the dataset + * @param dataSpace the data space to specify the dimensions of the dataset + * @return std::shared_ptr shared pointer to the data set + */ + template + std::shared_ptr getHdf5DataSet(const std::string& hdf5DataSetPath, HighFive::DataSpace& dataSpace); + private: void readLabel(const std::string& id, const std::string labelType, std::vector& labels); void readBoundingBoxes(const std::string& id, const std::string boundingBoxType, diff --git a/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/impl/hdf5-core-general.hpp b/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/impl/hdf5-core-general.hpp index b2bb07a8e..b8e54fd6c 100644 --- a/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/impl/hdf5-core-general.hpp +++ b/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/impl/hdf5-core-general.hpp @@ -61,4 +61,22 @@ void Hdf5CoreGeneral::writeTimeToAnnotateTraits(const int64_t& value, HighFive:: m_file->flush(); } +template +std::shared_ptr Hdf5CoreGeneral::getHdf5DataSet(const std::string& hdf5DataSetPath, + HighFive::DataSpace& dataSpace) +{ + try + { + checkExists(hdf5DataSetPath); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) + << "hdf5 group" << hdf5DataSetPath << " already exists!"; + return std::make_shared(m_file->getDataSet(hdf5DataSetPath)); + } + catch (std::invalid_argument const& e) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) + << "hdf5 group " << hdf5DataSetPath << " does not exist! Creating a new group"; + return std::make_shared(m_file->createDataSet(hdf5DataSetPath, dataSpace)); + } +} } // namespace seerep_hdf5_core diff --git a/seerep-hdf5/seerep-hdf5-core/src/hdf5-core-general.cpp b/seerep-hdf5/seerep-hdf5-core/src/hdf5-core-general.cpp index 77e59aafd..3e0815289 100644 --- a/seerep-hdf5/seerep-hdf5-core/src/hdf5-core-general.cpp +++ b/seerep-hdf5/seerep-hdf5-core/src/hdf5-core-general.cpp @@ -366,4 +366,21 @@ void Hdf5CoreGeneral::readInstances(const std::string& id, const std::string ins datasetInstances.read(instances); } +std::shared_ptr Hdf5CoreGeneral::getHdf5Group(const std::string& hdf5GroupPath) +{ + try + { + checkExists(hdf5GroupPath); + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) + << "hdf5 group" << hdf5GroupPath << " already exists!"; + return std::make_shared(m_file->getGroup(hdf5GroupPath)); + } + catch (std::invalid_argument const& e) + { + BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) + << "hdf5 group " << hdf5GroupPath << " does not exist! Creating a new group"; + return std::make_shared(m_file->createGroup(hdf5GroupPath)); + } +} + } // namespace seerep_hdf5_core diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 3395ed00c..f1e92bf7f 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -16,25 +16,11 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: { const std::scoped_lock lock(*m_write_mtx); - std::string hdf5GroupPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id; + const std::string hdf5GroupPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "writing flatbuffers point cloud to: " << hdf5GroupPath; - std::shared_ptr dataGroupPtr; - - try - { - checkExists(hdf5GroupPath); - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) - << "hdf5 group" << hdf5GroupPath << " already exists!"; - dataGroupPtr = std::make_shared(m_file->getGroup(hdf5GroupPath)); - } - catch (std::invalid_argument const& e) - { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) - << "hdf5 group " << hdf5GroupPath << " does not exist! Creating a new group"; - dataGroupPtr = std::make_shared(m_file->createGroup(hdf5GroupPath)); - } + std::shared_ptr dataGroupPtr = getHdf5Group(hdf5GroupPath); writeGeneralAttributes(dataGroupPtr, cloud); @@ -96,25 +82,11 @@ void Hdf5FbPointCloud::writePoints(const std::string& id, const std::vector& groupPtr, std::vector& boundingBox) { - std::string hdf5PointsPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id + "/points"; + const std::string hdf5PointsPath = seerep_hdf5_core::Hdf5CorePointCloud::HDF5_GROUP_POINTCLOUD + "/" + id + "/points"; BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "writing dataset to: " << hdf5PointsPath; HighFive::DataSpace dataSpace({ height, width, 3 }); - std::shared_ptr pointsDatasetPtr; - - try - { - checkExists(hdf5PointsPath); - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) - << "hdf5 dataset: " << hdf5PointsPath << " already exists!"; - pointsDatasetPtr = std::make_shared(m_file->getDataSet(hdf5PointsPath)); - } - catch (std::invalid_argument const& e) - { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) - << "hdf5 dataset: " << hdf5PointsPath << " does not exist! Creating a new dataset"; - pointsDatasetPtr = std::make_shared(m_file->createDataSet(hdf5PointsPath, dataSpace)); - } + std::shared_ptr pointsDatasetPtr = getHdf5DataSet(hdf5PointsPath, dataSpace); std::vector>> pointData; pointData.resize(height); @@ -174,21 +146,7 @@ void Hdf5FbPointCloud::writeColorsRGB(const std::string& id, const std::vector colorsDatasetPtr; - - try - { - checkExists(hdf5ColorsPath); - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) - << "hdf5 dataset: " << hdf5ColorsPath << " already exists!"; - colorsDatasetPtr = std::make_shared(m_file->getDataSet(hdf5ColorsPath)); - } - catch (std::invalid_argument const& e) - { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) - << "hdf5 dataset: " << hdf5ColorsPath << " does not exist! Creating a new dataset"; - colorsDatasetPtr = std::make_shared(m_file->createDataSet(hdf5ColorsPath, dataSpace)); - } + std::shared_ptr colorsDatasetPtr = getHdf5DataSet(hdf5ColorsPath, dataSpace); std::vector>> colorsData; colorsData.resize(height); @@ -218,20 +176,7 @@ void Hdf5FbPointCloud::writeColorsRGBA(const std::string& id, const std::vector< HighFive::DataSpace dataSpace({ height, width, 4 }); - std::shared_ptr colorsDatasetPtr; - try - { - checkExists(hdf5ColorsPath); - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) - << "hdf5 dataset: " << hdf5ColorsPath << " already exists!"; - colorsDatasetPtr = std::make_shared(m_file->getDataSet(hdf5ColorsPath)); - } - catch (std::invalid_argument const& e) - { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) - << "hdf5 dataset: " << hdf5ColorsPath << " does not exist! Creating a new dataset"; - colorsDatasetPtr = std::make_shared(m_file->createDataSet(hdf5ColorsPath, dataSpace)); - } + std::shared_ptr colorsDatasetPtr = getHdf5DataSet(hdf5ColorsPath, dataSpace); std::vector>> colorsData; colorsData.resize(height); From 8cba06abdc615adf2012c6d919a7593c62ef12ef Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 18 Oct 2022 08:57:47 +0000 Subject: [PATCH 50/54] add method to compute bounding box in hdf5-fb-pointcloud --- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 12 +++++++ .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 35 +++++++++++-------- 2 files changed, 32 insertions(+), 15 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index b2a61ede6..902e4fe6b 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -100,6 +100,18 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral } } + /** + * @brief Computes the bounding box while iterating over the point cloud + * + * @param min reference to an array to store the min x, y, z + * @param max reference to an array to store the max x, y, z + * @param x current x + * @param y current y + * @param z current z + */ + void computeBoundingBox(std::array& min, std::array& max, const float& x, const float& y, + const float& z); + /** * @brief Write x,y,z of a point cloud to a /points dataset * diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index f1e92bf7f..070ccba66 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -78,6 +78,25 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: m_file->flush(); } +void Hdf5FbPointCloud::computeBoundingBox(std::array& min, std::array& max, const float& x, + const float& y, const float& z) +{ + if (x < min[0]) + min[0] = x; + if (x > max[0]) + max[0] = x; + + if (y < min[1]) + min[1] = y; + if (y > max[1]) + max[1] = y; + + if (z < min[2]) + min[2] = z; + if (z > max[2]) + max[2] = z; +} + void Hdf5FbPointCloud::writePoints(const std::string& id, const std::vector& offsets, const uint8_t* data, uint32_t pointStep, uint32_t height, uint32_t width, const std::shared_ptr& groupPtr, std::vector& boundingBox) @@ -108,21 +127,7 @@ void Hdf5FbPointCloud::writePoints(const std::string& id, const std::vector max[0]) - max[0] = x; - - if (y < min[1]) - min[1] = y; - if (y > max[1]) - max[1] = y; - - if (z < min[2]) - min[2] = z; - if (z > max[2]) - max[2] = z; + computeBoundingBox(min, max, x, y, z); pointData[row].push_back(std::vector{ x, y, z }); From 0764d5f26787c61ef61154bcd494f7c78c0d209a Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 18 Oct 2022 09:54:36 +0000 Subject: [PATCH 51/54] add methods to calculate multiple offsets for point clouds --- .../seerep-hdf5-fb/hdf5-fb-pointcloud.h | 5 ++ .../seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 87 ++++++++----------- 2 files changed, 40 insertions(+), 52 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h index 902e4fe6b..bb4717963 100644 --- a/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h +++ b/seerep-hdf5/seerep-hdf5-fb/include/seerep-hdf5-fb/hdf5-fb-pointcloud.h @@ -289,6 +289,8 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral */ uint32_t getOffset(const seerep::fb::PointCloud2& cloud, const std::string& fieldName); + std::vector getOffsets(const seerep::fb::PointCloud2& cloud, const std::vector& fields); + /** * @brief Get the offset of a field from hdf5 * @@ -301,6 +303,9 @@ class Hdf5FbPointCloud : public Hdf5FbGeneral uint32_t getOffset(const std::vector& names, const std::vector& offsets, const std::string& fieldName, bool isBigendian); + std::vector getOffsets(const std::vector& names, const std::vector& offsets, + bool isBigendian, const std::vector& fields); + /** * @brief Get the offset for the special case of rgba(a) * diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 070ccba66..001329c9e 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -9,8 +9,6 @@ Hdf5FbPointCloud::Hdf5FbPointCloud(std::shared_ptr& file, std::s { } -// TODO refactor again, still not happy with the API - void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb::PointCloud2& cloud, std::vector& boundingBox) { @@ -35,40 +33,20 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: if (info.has_points) { - // TODO create method for calculating the offsets - const std::vector fields = { "x", "y", "z" }; - std::vector xyzOffsets; - for (auto field : fields) - { - xyzOffsets.push_back(getOffset(cloud, field)); - } - - writePoints(id, xyzOffsets, cloud.data()->data(), cloud.point_step(), cloud.height(), cloud.width(), dataGroupPtr, - boundingBox); + writePoints(id, getOffsets(cloud, std::vector{ "x", "y", "z" }), cloud.data()->data(), + cloud.point_step(), cloud.height(), cloud.width(), dataGroupPtr, boundingBox); } if (info.has_rgb) { - const std::vector fields = { "r", "g", "b" }; - std::vector rgbOffsets; - for (auto field : fields) - { - rgbOffsets.push_back(getOffset(cloud, field)); - } - - writeColorsRGBA(id, rgbOffsets, cloud.data()->data(), cloud.point_step(), cloud.height(), cloud.width()); + writeColorsRGBA(id, getOffsets(cloud, std::vector{ "r", "g", "b" }), cloud.data()->data(), + cloud.point_step(), cloud.height(), cloud.width()); } if (info.has_rgba) { - const std::vector fields = { "r", "g", "b", "a" }; - std::vector rgbaOffsets; - for (auto field : fields) - { - rgbaOffsets.push_back(getOffset(cloud, field)); - } - - writeColorsRGBA(id, rgbaOffsets, cloud.data()->data(), cloud.point_step(), cloud.height(), cloud.width()); + writeColorsRGBA(id, getOffsets(cloud, std::vector{ "r", "g", "b", "a" }), cloud.data()->data(), + cloud.point_step(), cloud.height(), cloud.width()); } // TODO normals @@ -300,38 +278,20 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id, const bool withoutData) if (info.has_points) { - const std::vector fields = { "x", "y", "z" }; - std::vector xyzOffsets; - for (auto field : fields) - { - xyzOffsets.push_back(getOffset(names, offsets, field, isBigendian)); - } - - readPoints(id, xyzOffsets, data, pointStep, height, width); + readPoints(id, getOffsets(names, offsets, isBigendian, std::vector{ "x", "y", "z" }), data, + pointStep, height, width); } if (info.has_rgb) { - const std::vector fields = { "r", "g", "b" }; - std::vector rgbOffsets; - for (auto field : fields) - { - rgbOffsets.push_back(getOffset(names, offsets, field, isBigendian)); - } - - readColorsRGB(id, rgbOffsets, data, pointStep, height, width); + readColorsRGB(id, getOffsets(names, offsets, isBigendian, std::vector{ "r", "g", "b" }), data, + pointStep, height, width); } if (info.has_rgba) { - const std::vector fields = { "r", "g", "b", "a" }; - std::vector rgbaOffsets; - for (auto field : fields) - { - rgbaOffsets.push_back(getOffset(names, offsets, field, isBigendian)); - } - - readPoints(id, rgbaOffsets, data, pointStep, height, width); + readPoints(id, getOffsets(names, offsets, isBigendian, std::vector{ "r", "g", "b", "a" }), data, + pointStep, height, width); } } @@ -647,6 +607,18 @@ uint32_t Hdf5FbPointCloud::getOffset(const std::vector& names, cons throw std::runtime_error("Field " + fieldName + " does not exist!"); } +std::vector Hdf5FbPointCloud::getOffsets(const std::vector& names, + const std::vector& offsets, bool isBigendian, + const std::vector& fields) +{ + std::vector calcOffsets; + for (auto field : fields) + { + calcOffsets.push_back(getOffset(names, offsets, field, isBigendian)); + } + return calcOffsets; +} + uint32_t Hdf5FbPointCloud::getOffset(const seerep::fb::PointCloud2& cloud, const std::string& fieldName) { for (size_t i = 0; i < cloud.fields()->size(); i++) @@ -675,6 +647,17 @@ uint32_t Hdf5FbPointCloud::getOffset(const seerep::fb::PointCloud2& cloud, const throw std::runtime_error("Field " + fieldName + " does not exist!"); } +std::vector Hdf5FbPointCloud::getOffsets(const seerep::fb::PointCloud2& cloud, + const std::vector& fields) +{ + std::vector calcOffsets; + for (auto field : fields) + { + calcOffsets.push_back(getOffset(cloud, field)); + } + return calcOffsets; +} + uint32_t Hdf5FbPointCloud::rgbaOffset(const std::string& fieldName, uint32_t offset, bool isBigendian) { if (fieldName == "r") From 1b155b861eba106eb4a12522ce083b4c450bb1e3 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 18 Oct 2022 09:55:40 +0000 Subject: [PATCH 52/54] add precision for numpy output in point cloud examples --- examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py | 2 ++ examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py | 2 ++ 2 files changed, 4 insertions(+) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py index 1f576602d..59d4df128 100644 --- a/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_queryPointCloud.py @@ -4,6 +4,8 @@ import flatbuffers import numpy as np + +np.set_printoptions(precision=7) from fb import point_cloud_service_grpc_fb as pointCloudService from fb.PointCloud2 import PointCloud2 diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py index 788eb96ed..b209a00b2 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py @@ -7,6 +7,8 @@ import flatbuffers import numpy as np + +np.set_printoptions(precision=7) from fb import PointCloud2 from fb import point_cloud_service_grpc_fb as pointCloudService From f077a9f5032f2b61ef582b94747fdd365664d77e Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Tue, 18 Oct 2022 11:57:32 +0000 Subject: [PATCH 53/54] refactor data group accessor method in hdf5-core-general --- .../include/seerep-hdf5-core/hdf5-core-general.h | 7 +++---- .../seerep-hdf5-core/src/hdf5-core-general.cpp | 11 +++++++++-- seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 11 ++--------- 3 files changed, 14 insertions(+), 15 deletions(-) diff --git a/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-general.h b/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-general.h index 3d15e4917..80fbbbf87 100644 --- a/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-general.h +++ b/seerep-hdf5/seerep-hdf5-core/include/seerep-hdf5-core/hdf5-core-general.h @@ -92,14 +92,13 @@ class Hdf5CoreGeneral // Hdf5 //################ /** - * @brief Get a shared pointer to a hdf5 group specified by the hdf5GroupPath. - * - * If a group with the path already exists we use that, otherwise a new one is created. + * @brief Get a shared pointer to a hdf5 group * * @param hdf5GroupPath path to the data group + * @param create create a new group if the group path can't be found? * @return std::shared_ptr shared pointer to the group */ - std::shared_ptr getHdf5Group(const std::string& hdf5GroupPath); + std::shared_ptr getHdf5Group(const std::string& hdf5GroupPath, bool create = true); /** * @brief Get a shared pointer to a hdf5 data set specified by the hdf5DataSetPath * diff --git a/seerep-hdf5/seerep-hdf5-core/src/hdf5-core-general.cpp b/seerep-hdf5/seerep-hdf5-core/src/hdf5-core-general.cpp index 3e0815289..c047a3577 100644 --- a/seerep-hdf5/seerep-hdf5-core/src/hdf5-core-general.cpp +++ b/seerep-hdf5/seerep-hdf5-core/src/hdf5-core-general.cpp @@ -366,7 +366,7 @@ void Hdf5CoreGeneral::readInstances(const std::string& id, const std::string ins datasetInstances.read(instances); } -std::shared_ptr Hdf5CoreGeneral::getHdf5Group(const std::string& hdf5GroupPath) +std::shared_ptr Hdf5CoreGeneral::getHdf5Group(const std::string& hdf5GroupPath, bool create) { try { @@ -379,7 +379,14 @@ std::shared_ptr Hdf5CoreGeneral::getHdf5Group(const std::string { BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::trace) << "hdf5 group " << hdf5GroupPath << " does not exist! Creating a new group"; - return std::make_shared(m_file->createGroup(hdf5GroupPath)); + if (create) + { + return std::make_shared(m_file->createGroup(hdf5GroupPath)); + } + else + { + return nullptr; + } } } diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 001329c9e..3103827bb 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -206,17 +206,10 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id, const bool withoutData) BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::debug) << "loading flatbuffers point cloud from: " << hdf5GroupPath; - std::shared_ptr dataGroupPtr; + std::shared_ptr dataGroupPtr = getHdf5Group(hdf5GroupPath, false); - try - { - checkExists(hdf5GroupPath); - dataGroupPtr = std::make_shared(m_file->getGroup(hdf5GroupPath)); - } - catch (std::invalid_argument const& e) + if (!dataGroupPtr) { - BOOST_LOG_SEV(m_logger, boost::log::trivial::severity_level::warning) - << "hdf5 group " << hdf5GroupPath << " does not exist!"; return std::nullopt; } From c44fd92b448a85cccd56121d66a44d6853fc9712 Mon Sep 17 00:00:00 2001 From: Julian Arkenau Date: Thu, 20 Oct 2022 12:45:36 +0000 Subject: [PATCH 54/54] fix write of rgba field in flatbuffers point cloud --- .../python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py | 2 +- seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp | 9 +++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py index b209a00b2..d5a093c75 100755 --- a/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py +++ b/examples/python/gRPC/pointcloud/gRPC_fb_sendPointCloud.py @@ -38,7 +38,7 @@ def createPointCloud(builder, header, height=960, width=1280): '''Creates a flatbuffers point cloud message''' - pointFields = createPointFields(builder, ['x', 'y', 'z', 'rgb'], 7, 4, 1) + pointFields = createPointFields(builder, ['x', 'y', 'z', 'rgba'], 7, 4, 1) pointFieldsVector = addToPointFieldVector(builder, pointFields) # create general labels diff --git a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp index 3103827bb..fcdbb75b1 100644 --- a/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp +++ b/seerep-hdf5/seerep-hdf5-fb/src/hdf5-fb-pointcloud.cpp @@ -39,8 +39,8 @@ void Hdf5FbPointCloud::writePointCloud2(const std::string& id, const seerep::fb: if (info.has_rgb) { - writeColorsRGBA(id, getOffsets(cloud, std::vector{ "r", "g", "b" }), cloud.data()->data(), - cloud.point_step(), cloud.height(), cloud.width()); + writeColorsRGB(id, getOffsets(cloud, std::vector{ "r", "g", "b" }), cloud.data()->data(), + cloud.point_step(), cloud.height(), cloud.width()); } if (info.has_rgba) @@ -283,8 +283,8 @@ Hdf5FbPointCloud::readPointCloud2(const std::string& id, const bool withoutData) if (info.has_rgba) { - readPoints(id, getOffsets(names, offsets, isBigendian, std::vector{ "r", "g", "b", "a" }), data, - pointStep, height, width); + readColorsRGBA(id, getOffsets(names, offsets, isBigendian, std::vector{ "r", "g", "b", "a" }), data, + pointStep, height, width); } } @@ -697,6 +697,7 @@ uint32_t Hdf5FbPointCloud::rgbaOffset(const std::string& fieldName, uint32_t off return offset + 3; } } + throw std::runtime_error("Field " + fieldName + " does not exist!"); } } // namespace seerep_hdf5_fb