diff --git a/test/visualization/test_visualization.cpp b/test/visualization/test_visualization.cpp index fd21aa7e4dc..ec8597defba 100644 --- a/test/visualization/test_visualization.cpp +++ b/test/visualization/test_visualization.cpp @@ -44,6 +44,9 @@ #include #include #include +#include +#include +#include using namespace pcl; using namespace pcl::io; @@ -221,6 +224,142 @@ TEST(PCL, PCLVisualizer_updateColorHandlerIndex) { EXPECT_TRUE(viewer_ptr->updateColorHandlerIndex(cloud_name, 0)); } +struct PointWithRandomTypes { + PCL_ADD_POINT4D + double a; + int b; + std::int16_t c; + bool d; + PCL_MAKE_ALIGNED_OPERATOR_NEW +}; + +// define a point type with a random collection of types +POINT_CLOUD_REGISTER_POINT_STRUCT(PointWithRandomTypes, + (float, x, x)(float, y, y)(float, z, z)(double, a, a)( + int, b, b)(std::int16_t, c, c)(bool, d, d)) + +TEST(PCL, ColorHandlerGenericField_datatypes) +{ + auto cloud = pcl::make_shared>(); + + // use random values for the points, some negative, some positive + std::vector xs{0, 1, 2, 3}, ys{0, 1, 2, 3}, zs{0, 1, 2, 3}; + std::vector as{2.5, 4.5, -2.5, -4.5}; + std::vector bs{1, -1, 0, 99}; + std::vector cs{2, 3, -10, std::numeric_limits::max()}; + std::vector ds{true, false, true, false}; + + // fill point cloud + for (size_t i = 0; i < xs.size(); ++i) { + PointWithRandomTypes p; + p.x = xs[i]; + p.y = ys[i]; + p.z = zs[i]; + p.a = as[i]; + p.b = bs[i]; + p.c = cs[i]; + p.d = ds[i]; + cloud->push_back(p); + } + + // Now we test if PointCloudColorHandlerGenericField::getColor() transforms all types + // to reasonable values + constexpr float max_error = 1e-8; + using ColorHandlerPointCloud = + pcl::visualization::PointCloudColorHandlerGenericField; + + // scope for testing regular point cloud + { + // test float field "y" + ColorHandlerPointCloud color_handler_x(cloud, "x"); + vtkSmartPointer colors_x = color_handler_x.getColor(); + + for (size_t i = 0; i < xs.size(); ++i) { + EXPECT_NEAR(*(colors_x->GetTuple(i)), static_cast(xs[i]), max_error); + } + + // test double field "a" + ColorHandlerPointCloud color_handler_a(cloud, "a"); + vtkSmartPointer colors_a = color_handler_a.getColor(); + + for (size_t i = 0; i < xs.size(); ++i) { + EXPECT_NEAR(*(colors_a->GetTuple(i)), static_cast(as[i]), max_error); + } + + // test int field "b" + ColorHandlerPointCloud color_handler_b(cloud, "b"); + vtkSmartPointer colors_b = color_handler_b.getColor(); + + for (size_t i = 0; i < xs.size(); ++i) { + EXPECT_NEAR(*(colors_b->GetTuple(i)), static_cast(bs[i]), max_error); + } + + // test int16_t field "c" + ColorHandlerPointCloud color_handler_c(cloud, "c"); + vtkSmartPointer colors_c = color_handler_c.getColor(); + + for (size_t i = 0; i < xs.size(); ++i) { + EXPECT_NEAR(*(colors_c->GetTuple(i)), static_cast(cs[i]), max_error); + } + + // test bool field "d" + ColorHandlerPointCloud color_handler_d(cloud, "d"); + vtkSmartPointer colors_d = color_handler_d.getColor(); + + for (size_t i = 0; i < xs.size(); ++i) { + EXPECT_NEAR(*(colors_d->GetTuple(i)), static_cast(ds[i]), max_error); + } + } + + using ColorHandlerPCLPointCloud2 = + pcl::visualization::PointCloudColorHandlerGenericField; + PCLPointCloud2::Ptr cloud_pcl = pcl::make_shared(); + toPCLPointCloud2(*cloud, *cloud_pcl); + + // scope for testing PCLPointCloud2 + { + // test float field "y" + ColorHandlerPCLPointCloud2 color_handler_x(cloud_pcl, "x"); + vtkSmartPointer colors_x = color_handler_x.getColor(); + + for (size_t i = 0; i < xs.size(); ++i) { + EXPECT_NEAR(*(colors_x->GetTuple(i)), static_cast(xs[i]), max_error); + } + + // test double field "a" + ColorHandlerPCLPointCloud2 color_handler_a(cloud_pcl, "a"); + vtkSmartPointer colors_a = color_handler_a.getColor(); + + for (size_t i = 0; i < xs.size(); ++i) { + EXPECT_NEAR(*(colors_a->GetTuple(i)), static_cast(as[i]), max_error); + } + + // test int field "b" + ColorHandlerPCLPointCloud2 color_handler_b(cloud_pcl, "b"); + vtkSmartPointer colors_b = color_handler_b.getColor(); + + for (size_t i = 0; i < xs.size(); ++i) { + EXPECT_NEAR(*(colors_b->GetTuple(i)), static_cast(bs[i]), max_error); + } + + // test int16_t field "c" + ColorHandlerPCLPointCloud2 color_handler_c(cloud_pcl, "c"); + vtkSmartPointer colors_c = color_handler_c.getColor(); + + for (size_t i = 0; i < xs.size(); ++i) { + EXPECT_NEAR(*(colors_c->GetTuple(i)), static_cast(cs[i]), max_error); + } + + // test bool field "d" + ColorHandlerPCLPointCloud2 color_handler_d(cloud_pcl, "d"); + vtkSmartPointer colors_d = color_handler_d.getColor(); + + for (size_t i = 0; i < xs.size(); ++i) { + EXPECT_NEAR(*(colors_d->GetTuple(i)), static_cast(ds[i]), max_error); + } + } +} + /* ---[ */ int main (int argc, char** argv) diff --git a/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp b/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp index 4e5aa57b5c4..820c1398be7 100644 --- a/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp +++ b/visualization/include/pcl/visualization/impl/point_cloud_color_handlers.hpp @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -361,86 +362,6 @@ PointCloudColorHandlerHSVField::getColor () const return scalars; } - -template void -PointCloudColorHandlerGenericField::setInputCloud ( - const PointCloudConstPtr &cloud) -{ - PointCloudColorHandler::setInputCloud (cloud); - field_idx_ = pcl::getFieldIndex (field_name_, fields_); - if (field_idx_ == -1) { - capable_ = false; - return; - } - if (fields_[field_idx_].datatype != pcl::PCLPointField::PointFieldTypes::FLOAT32) { - capable_ = false; - PCL_ERROR("[pcl::PointCloudColorHandlerGenericField::setInputCloud] This currently only works with float32 fields, but field %s has a different type.\n", field_name_.c_str()); - return; - } - capable_ = true; -} - - -template vtkSmartPointer -PointCloudColorHandlerGenericField::getColor () const -{ - if (!capable_ || !cloud_) - return nullptr; - - auto scalars = vtkSmartPointer::New (); - scalars->SetNumberOfComponents (1); - - vtkIdType nr_points = cloud_->size (); - scalars->SetNumberOfTuples (nr_points); - - using FieldList = typename pcl::traits::fieldList::type; - - float* colors = new float[nr_points]; - float field_data; - - int j = 0; - // If XYZ present, check if the points are invalid - int x_idx = -1; - for (std::size_t d = 0; d < fields_.size (); ++d) - if (fields_[d].name == "x") - x_idx = static_cast (d); - - if (x_idx != -1) - { - // Color every point - for (vtkIdType cp = 0; cp < nr_points; ++cp) - { - // Copy the value at the specified field - if (!pcl::isXYZFinite((*cloud_)[cp])) - continue; - - const std::uint8_t* pt_data = reinterpret_cast (&(*cloud_)[cp]); - memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype)); - - colors[j] = field_data; - j++; - } - } - else - { - // Color every point - for (vtkIdType cp = 0; cp < nr_points; ++cp) - { - const std::uint8_t* pt_data = reinterpret_cast (&(*cloud_)[cp]); - memcpy (&field_data, pt_data + fields_[field_idx_].offset, pcl::getFieldSize (fields_[field_idx_].datatype)); - - if (!std::isfinite (field_data)) - continue; - - colors[j] = field_data; - j++; - } - } - scalars->SetArray (colors, j, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE); - return scalars; -} - - template void PointCloudColorHandlerRGBAField::setInputCloud ( const PointCloudConstPtr &cloud) @@ -564,6 +485,232 @@ PointCloudColorHandlerLabelField::getColor () const return scalars; } +// ---- +// template specializations for PointCloud and PCLPointCloud2 to access certain +// data from PointCloudColorHandlerGenericField without needing to know the +// cloud type + +// Get point fields from cloud. Could not get it to work with existing +// pcl::getFields +template inline std::vector +getFields(const CloudT& cloud) +{ + return pcl::getFields(); +} + +template <> inline std::vector +getFields(const pcl::PCLPointCloud2& cloud) { + return cloud.fields; +} + + +// Get point step. Does not directly exist in pcl::PointCloud +template inline int getPointStep(const CloudT&) +{ + return sizeof(typename CloudT::PointType); +} + +template <> inline int +getPointStep(const pcl::PCLPointCloud2& cloud) { + return cloud.point_step; +} + +// Get cloud data blob +template inline const std::uint8_t* getCloudData(const CloudT& cloud) +{ + return reinterpret_cast(cloud.points.data()); +} + +template <> inline const std::uint8_t* getCloudData(const typename pcl::PCLPointCloud2& cloud) { + return reinterpret_cast(cloud.data.data()); +} + + +// copy of pcl::getFieldIndex() from impl/io.hpp, without the unused template +// parameter +static int getFieldIndex(const std::string& field_name, + const std::vector& fields) +{ + const auto result = + std::find_if(fields.begin(), fields.end(), [&field_name](const auto& field) { + return field.name == field_name; + }); + if (result == fields.end()) { + return -1; + } + return std::distance(fields.begin(), result); +} + +// Cloud type agnostic isXYZFinite wrappers to check if pointcloud or PCLPointCloud2 at +// given index is finite +template inline bool isXYZFiniteAt(const CloudT& cloud, int index) +{ + return pcl::isXYZFinite(cloud.at(index)); +} + +template <> inline bool isXYZFiniteAt(const PCLPointCloud2& cloud, int index) +{ + // get x,y,z field indices + const auto x_field_idx = getFieldIndex("x", cloud.fields); + const auto y_field_idx = getFieldIndex("y", cloud.fields); + const auto z_field_idx = getFieldIndex("z", cloud.fields); + + // if any missing, error + if (x_field_idx == -1 || y_field_idx == -1 || z_field_idx == -1) { + throw std::out_of_range("getXData(): input cloud missing at least one of x, y, z fields"); + } + // get x,y,z field values + const auto position_x = index * cloud.point_step + cloud.fields[x_field_idx].offset; + const auto position_y = index * cloud.point_step + cloud.fields[y_field_idx].offset; + const auto position_z = index * cloud.point_step + cloud.fields[z_field_idx].offset; + if (cloud.data.size () >= (position_x + sizeof(float)) && + cloud.data.size () >= (position_y + sizeof(float)) && + cloud.data.size () >= (position_z + sizeof(float))) { + const float x = *reinterpret_cast(&cloud.data[position_x]); + const float y = *reinterpret_cast(&cloud.data[position_y]); + const float z = *reinterpret_cast(&cloud.data[position_z]); + return isXYZFinite(PointXYZ(x, y, z)); + } else { + // the last of the three is out of range + throw std::out_of_range("getXData(): requested for index larger than number of points"); + } +} + +inline const std::uint8_t* getCloudData(const typename pcl::PCLPointCloud2& cloud) +{ + return reinterpret_cast(cloud.data.data()); +} + +template RType reinterpret_and_cast(const std::uint8_t* p) +{ + return static_cast(*reinterpret_cast(p)); +} + +/** + * @brief Get the value of a point field from raw data pointer and field type. + * + * @tparam T return type the field will be cast as + * @param data data pointer + * @param type point field type + * + * @return field value + */ +template T point_field_as(const std::uint8_t* data, const std::uint8_t type) +{ + switch (type) { + case pcl::PCLPointField::PointFieldTypes::FLOAT32: + return reinterpret_and_cast(data); + break; + case pcl::PCLPointField::PointFieldTypes::UINT8: + return reinterpret_and_cast(data); + break; + case pcl::PCLPointField::PointFieldTypes::UINT16: + return reinterpret_and_cast(data); + break; + case pcl::PCLPointField::PointFieldTypes::UINT32: + return reinterpret_and_cast(data); + break; + case pcl::PCLPointField::PointFieldTypes::UINT64: + return reinterpret_and_cast(data); + break; + case pcl::PCLPointField::PointFieldTypes::BOOL: + return reinterpret_and_cast(data); + break; + case pcl::PCLPointField::PointFieldTypes::FLOAT64: + return reinterpret_and_cast(data); + break; + case pcl::PCLPointField::PointFieldTypes::INT16: + return reinterpret_and_cast(data); + break; + case pcl::PCLPointField::PointFieldTypes::INT32: + return reinterpret_and_cast(data); + break; + case pcl::PCLPointField::PointFieldTypes::INT64: + return reinterpret_and_cast(data); + break; + default: + return 0; + break; + } +} + +template +PointCloudColorHandlerGenericField::PointCloudColorHandlerGenericField( + const PointCloudConstPtr& cloud, const std::string& field_name) +: PointCloudColorHandler(cloud), field_name_(field_name) +{ + this->setInputCloud(cloud); +} + +template +PointCloudColorHandlerGenericField::PointCloudColorHandlerGenericField(const std::string& field_name) + : PointCloudColorHandler(), field_name_(field_name) {} + +template void PointCloudColorHandlerGenericField::setInputCloud( + const PointCloudConstPtr& cloud) +{ + PointCloudColorHandler::setInputCloud(cloud); + this->fields_ = getFields(*cloud); + this->field_idx_ = getFieldIndex(field_name_, this->fields_); + this->capable_ = this->field_idx_ != -1; +} + +template std::string PointCloudColorHandlerGenericField::getFieldName() const +{ + return field_name_; +} + +template vtkSmartPointer PointCloudColorHandlerGenericField::getColor() const +{ + if (!this->capable_ || !this->cloud_) { + return nullptr; + } + + auto scalars = vtkSmartPointer::New(); + scalars->SetNumberOfComponents(1); + + const vtkIdType nr_points = this->cloud_->width * this->cloud_->height; + scalars->SetNumberOfTuples(nr_points); + + // per-point color as a float value. vtk turns that into rgb somehow + float* colors = new float[nr_points]; + + // Find X channel + int x_channel_idx = -1; + for (std::size_t channel_idx = 0; channel_idx < this->fields_.size(); ++channel_idx) { + if (this->fields_[channel_idx].name == "x") { + x_channel_idx = static_cast(channel_idx); + } + } + + int point_offset = this->fields_[this->field_idx_].offset; + const int point_step = getPointStep(*this->cloud_); + const std::uint8_t* p_data = getCloudData(*this->cloud_); + const std::uint8_t field_type = this->fields_[this->field_idx_].datatype; + + // current index into colors array + int pt_idx = 0; + + // Color every point + for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += point_step) { + + if (x_channel_idx != -1 && !isXYZFiniteAt(*this->cloud_, cp)) { + // no x channel in the cloud, or point is infinite + continue; + } else { + // point data at index, field offset already baked into point_offset + const std::uint8_t* pt_data = &p_data[point_offset]; + + colors[pt_idx] = point_field_as(pt_data, field_type); + + } + + ++pt_idx; + } + + scalars->SetArray(colors, pt_idx, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE); + return scalars; +} + } // namespace visualization } // namespace pcl - diff --git a/visualization/include/pcl/visualization/point_cloud_color_handlers.h b/visualization/include/pcl/visualization/point_cloud_color_handlers.h index af2b57212f1..731ce5b81c9 100644 --- a/visualization/include/pcl/visualization/point_cloud_color_handlers.h +++ b/visualization/include/pcl/visualization/point_cloud_color_handlers.h @@ -336,62 +336,47 @@ namespace pcl ////////////////////////////////////////////////////////////////////////////////////// /** \brief Generic field handler class for colors. Uses an user given field to extract - * 1D data and display the color at each point using a min-max lookup table. - * \author Radu B. Rusu - * \ingroup visualization - */ + * 1D data and display the color at each point. + * \ingroup visualization + */ template - class PointCloudColorHandlerGenericField : public PointCloudColorHandler - { + class PointCloudColorHandlerGenericField : public PointCloudColorHandler { using PointCloud = typename PointCloudColorHandler::PointCloud; using PointCloudPtr = typename PointCloud::Ptr; using PointCloudConstPtr = typename PointCloud::ConstPtr; - public: - using Ptr = shared_ptr >; - using ConstPtr = shared_ptr >; + public: + using Ptr = std::shared_ptr>; + using ConstPtr = std::shared_ptr>; - /** \brief Constructor. */ - PointCloudColorHandlerGenericField (const std::string &field_name) - : field_name_ (field_name) - { - capable_ = false; - } + /** \brief constructor */ + PointCloudColorHandlerGenericField(const PointCloudConstPtr& cloud, + const std::string& field_name); - /** \brief Constructor. */ - PointCloudColorHandlerGenericField (const PointCloudConstPtr &cloud, - const std::string &field_name) - : PointCloudColorHandler (cloud) - , field_name_ (field_name) - { - setInputCloud (cloud); - } + PointCloudColorHandlerGenericField(const std::string& field_name); - /** \brief Get the name of the field used. */ - virtual std::string getFieldName () const { return (field_name_); } + /** \brief Set the input cloud to be used. + * \param[in] cloud the input cloud to be used by the handler + */ + void setInputCloud(const PointCloudConstPtr& cloud); - vtkSmartPointer - getColor () const override; + vtkSmartPointer getColor() const override; - /** \brief Set the input cloud to be used. - * \param[in] cloud the input cloud to be used by the handler - */ - virtual void - setInputCloud (const PointCloudConstPtr &cloud); + protected: - protected: - /** \brief Class getName method. */ - virtual std::string - getName () const { return ("PointCloudColorHandlerGenericField"); } + /** \brief Name of the field used to create the color handler. */ + std::string field_name_; - private: - using PointCloudColorHandler::cloud_; - using PointCloudColorHandler::capable_; - using PointCloudColorHandler::field_idx_; - using PointCloudColorHandler::fields_; + std::vector fields_; - /** \brief Name of the field used to create the color handler. */ - std::string field_name_; + /** \brief Get the name of the field used. */ + std::string getFieldName() const override; + + /** \brief Class getName method. */ + std::string getName() const override + { + return ("PointCloudColorHandlerGenericField"); + } }; @@ -728,44 +713,6 @@ namespace pcl int v_field_idx_; }; - ////////////////////////////////////////////////////////////////////////////////////// - /** \brief Generic field handler class for colors. Uses an user given field to extract - * 1D data and display the color at each point using a min-max lookup table. - * \author Radu B. Rusu - * \ingroup visualization - */ - template <> - class PCL_EXPORTS PointCloudColorHandlerGenericField : public PointCloudColorHandler - { - using PointCloud = PointCloudColorHandler::PointCloud; - using PointCloudPtr = PointCloud::Ptr; - using PointCloudConstPtr = PointCloud::ConstPtr; - - public: - using Ptr = shared_ptr >; - using ConstPtr = shared_ptr >; - - /** \brief Constructor. */ - PointCloudColorHandlerGenericField (const PointCloudConstPtr &cloud, - const std::string &field_name); - - vtkSmartPointer - getColor () const override; - - protected: - /** \brief Get the name of the class. */ - virtual std::string - getName () const { return ("PointCloudColorHandlerGenericField"); } - - /** \brief Get the name of the field used. */ - virtual std::string - getFieldName () const { return (field_name_); } - - private: - /** \brief Name of the field used to create the color handler. */ - std::string field_name_; - }; - ////////////////////////////////////////////////////////////////////////////////////// /** \brief RGBA handler class for colors. Uses the data present in the "rgba" field as * the color at each point. Transparency is handled. diff --git a/visualization/src/point_cloud_handlers.cpp b/visualization/src/point_cloud_handlers.cpp index 00dedb9b5ae..7f7946230e2 100644 --- a/visualization/src/point_cloud_handlers.cpp +++ b/visualization/src/point_cloud_handlers.cpp @@ -411,82 +411,6 @@ pcl::visualization::PointCloudColorHandlerHSVField::getColo return scalars; } -/////////////////////////////////////////////////////////////////////////////////////////// -pcl::visualization::PointCloudColorHandlerGenericField::PointCloudColorHandlerGenericField ( - const pcl::visualization::PointCloudColorHandler::PointCloudConstPtr &cloud, - const std::string &field_name) : - pcl::visualization::PointCloudColorHandler::PointCloudColorHandler (cloud), - field_name_ (field_name) -{ - field_idx_ = pcl::getFieldIndex (*cloud, field_name); - capable_ = field_idx_ != -1; - if (field_idx_ != -1 && cloud_->fields[field_idx_].datatype != pcl::PCLPointField::PointFieldTypes::FLOAT32) { - capable_ = false; - PCL_ERROR("[pcl::PointCloudColorHandlerGenericField] This currently only works with float32 fields, but field %s has a different type.\n", field_name.c_str()); - } -} - -/////////////////////////////////////////////////////////////////////////////////////////// -vtkSmartPointer -pcl::visualization::PointCloudColorHandlerGenericField::getColor () const -{ - if (!capable_ || !cloud_) - return nullptr; - - auto scalars = vtkSmartPointer::New (); - scalars->SetNumberOfComponents (1); - - vtkIdType nr_points = cloud_->width * cloud_->height; - scalars->SetNumberOfTuples (nr_points); - - float* colors = new float[nr_points]; - float field_data; - int j = 0; - int point_offset = cloud_->fields[field_idx_].offset; - - // If XYZ present, check if the points are invalid - int x_idx = pcl::getFieldIndex (*cloud_, "x"); - if (x_idx != -1) - { - float x_data, y_data, z_data; - int x_point_offset = cloud_->fields[x_idx].offset; - - // Color every point - for (vtkIdType cp = 0; cp < nr_points; ++cp, - point_offset += cloud_->point_step, - x_point_offset += cloud_->point_step) - { - memcpy (&x_data, &cloud_->data[x_point_offset], sizeof (float)); - memcpy (&y_data, &cloud_->data[x_point_offset + sizeof (float)], sizeof (float)); - memcpy (&z_data, &cloud_->data[x_point_offset + 2 * sizeof (float)], sizeof (float)); - if (!std::isfinite (x_data) || !std::isfinite (y_data) || !std::isfinite (z_data)) - continue; - - // Copy the value at the specified field - memcpy (&field_data, &cloud_->data[point_offset], pcl::getFieldSize (cloud_->fields[field_idx_].datatype)); - colors[j] = field_data; - j++; - } - } - // No XYZ data checks - else - { - // Color every point - for (vtkIdType cp = 0; cp < nr_points; ++cp, point_offset += cloud_->point_step) - { - // Copy the value at the specified field - //memcpy (&field_data, &cloud_->data[point_offset], sizeof (float)); - memcpy (&field_data, &cloud_->data[point_offset], pcl::getFieldSize (cloud_->fields[field_idx_].datatype)); - - if (!std::isfinite (field_data)) - continue; - colors[j] = field_data; - j++; - } - } - scalars->SetArray (colors, j, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE); - return scalars; -} /////////////////////////////////////////////////////////////////////////////////////////// pcl::visualization::PointCloudColorHandlerRGBAField::PointCloudColorHandlerRGBAField (