From dc2490c7a7ebf831e6f57609fbcf82fe3b532f84 Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Thu, 23 Jan 2014 19:42:30 +0100 Subject: [PATCH 1/4] Add a function to copy data between points of different types --- common/CMakeLists.txt | 2 + common/include/pcl/common/copy_point.h | 62 ++++++++ common/include/pcl/common/impl/copy_point.hpp | 145 ++++++++++++++++++ 3 files changed, 209 insertions(+) create mode 100644 common/include/pcl/common/copy_point.h create mode 100644 common/include/pcl/common/impl/copy_point.hpp diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt index 32c9ac90284..293f273278a 100644 --- a/common/CMakeLists.txt +++ b/common/CMakeLists.txt @@ -84,6 +84,7 @@ if(build) include/pcl/common/common_headers.h include/pcl/common/distances.h include/pcl/common/eigen.h + include/pcl/common/copy_point.h include/pcl/common/io.h include/pcl/common/file_io.h include/pcl/common/intersections.h @@ -116,6 +117,7 @@ if(build) include/pcl/common/impl/centroid.hpp include/pcl/common/impl/common.hpp include/pcl/common/impl/eigen.hpp + include/pcl/common/impl/copy_point.hpp include/pcl/common/impl/io.hpp include/pcl/common/impl/file_io.hpp include/pcl/common/impl/norms.hpp diff --git a/common/include/pcl/common/copy_point.h b/common/include/pcl/common/copy_point.h new file mode 100644 index 00000000000..0e80cfe733a --- /dev/null +++ b/common/include/pcl/common/copy_point.h @@ -0,0 +1,62 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * 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(s) 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 OWNER 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. + * + */ + +#ifndef PCL_COMMON_COPY_POINT_H_ +#define PCL_COMMON_COPY_POINT_H_ + +namespace pcl +{ + + /** \brief Copy the fields of a source point into a target point. + * + * If the source and the target point types are the same, then a complete + * copy is made. Otherwise only those fields that the two point types share + * in common are copied. + * + * \param[in] point_in the source point + * \param[out] point_out the target point + * + * \ingroup common */ + template void + copyPoint (const PointInT& point_in, PointOutT& point_out); + +} + +#include + +#endif // PCL_COMMON_COPY_POINT_H_ + diff --git a/common/include/pcl/common/impl/copy_point.hpp b/common/include/pcl/common/impl/copy_point.hpp new file mode 100644 index 00000000000..b4070a0e902 --- /dev/null +++ b/common/include/pcl/common/impl/copy_point.hpp @@ -0,0 +1,145 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * 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(s) 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 OWNER 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. + * + */ + +#ifndef PCL_COMMON_IMPL_COPY_POINT_HPP_ +#define PCL_COMMON_IMPL_COPY_POINT_HPP_ + +#include +#include +#include +#include + +namespace pcl +{ + + namespace detail + { + + /* CopyPointHelper and its specializations copy the contents of a source + * point to a target point. There are three cases: + * + * - Points have the same type. + * In this case a single `memcpy` is used. + * + * - Points have different types and one of the following is true: + * * both have RGB fields; + * * both have RGBA fields; + * * one or both have no RGB/RGBA fields. + * In this case we find the list of common fields and copy their + * contents one by one with `NdConcatenateFunctor`. + * + * - Points have different types and one of these types has RGB field, and + * the other has RGBA field. + * In this case we also find the list of common fields and copy their + * contents. In order to account for the fact that RGB and RGBA do not + * match we have an additional `memcpy` to copy the contents of one into + * another. + * + * An appropriate version of CopyPointHelper is instantiated during + * compilation time automatically, so there is absolutely no run-time + * overhead. */ + + template + struct CopyPointHelper { }; + + template + struct CopyPointHelper >::type> + { + void operator () (const PointInT& point_in, PointOutT& point_out) const + { + memcpy (&point_out, &point_in, sizeof (PointInT)); + } + }; + + template + struct CopyPointHelper >, + boost::mpl::or_ >, + boost::mpl::not_ >, + boost::mpl::and_, + pcl::traits::has_field >, + boost::mpl::and_, + pcl::traits::has_field > > > >::type> + { + void operator () (const PointInT& point_in, PointOutT& point_out) const + { + typedef typename pcl::traits::fieldList::type FieldListInT; + typedef typename pcl::traits::fieldList::type FieldListOutT; + typedef typename pcl::intersect::type FieldList; + pcl::for_each_type (pcl::NdConcatenateFunctor (point_in, point_out)); + } + }; + + template + struct CopyPointHelper >, + boost::mpl::or_, + pcl::traits::has_field >, + boost::mpl::and_, + pcl::traits::has_field > > > >::type> + { + void operator () (const PointInT& point_in, PointOutT& point_out) const + { + typedef typename pcl::traits::fieldList::type FieldListInT; + typedef typename pcl::traits::fieldList::type FieldListOutT; + typedef typename pcl::intersect::type FieldList; + const uint32_t offset_in = boost::mpl::if_, + pcl::traits::offset, + pcl::traits::offset >::type::value; + const uint32_t offset_out = boost::mpl::if_, + pcl::traits::offset, + pcl::traits::offset >::type::value; + pcl::for_each_type (pcl::NdConcatenateFunctor (point_in, point_out)); + memcpy (reinterpret_cast (&point_out) + offset_out, + reinterpret_cast (&point_in) + offset_in, + 4); + } + }; + + } + +} + +template void +pcl::copyPoint (const PointInT& point_in, PointOutT& point_out) +{ + detail::CopyPointHelper copy; + copy (point_in, point_out); +} + +#endif //PCL_COMMON_IMPL_COPY_POINT_HPP_ + From f55bb9ba02aac624c72741586804b9c5b277bc7e Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Thu, 23 Jan 2014 19:42:56 +0100 Subject: [PATCH 2/4] Add a unit test for `copyPoint()` function --- test/common/CMakeLists.txt | 1 + test/common/test_copy_point.cpp | 188 ++++++++++++++++++++++++++++++++ 2 files changed, 189 insertions(+) create mode 100644 test/common/test_copy_point.cpp diff --git a/test/common/CMakeLists.txt b/test/common/CMakeLists.txt index bafa5fd8b9d..d9b6f751555 100644 --- a/test/common/CMakeLists.txt +++ b/test/common/CMakeLists.txt @@ -3,6 +3,7 @@ PCL_ADD_TEST(common_test_wrappers test_wrappers FILES test_wrappers.cpp LINK_WIT PCL_ADD_TEST(common_test_macros test_macros FILES test_macros.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_vector_average test_vector_average FILES test_vector_average.cpp LINK_WITH pcl_gtest) PCL_ADD_TEST(common_common test_common FILES test_common.cpp LINK_WITH pcl_gtest pcl_common) +PCL_ADD_TEST(common_copy_point test_copy_point FILES test_copy_point.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_int test_plane_intersection FILES test_plane_intersection.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_pca test_pca FILES test_pca.cpp LINK_WITH pcl_gtest pcl_common) #PCL_ADD_TEST(common_spring test_spring FILES test_spring.cpp LINK_WITH pcl_gtest pcl_common) diff --git a/test/common/test_copy_point.cpp b/test/common/test_copy_point.cpp new file mode 100644 index 00000000000..bdbee4f5c6c --- /dev/null +++ b/test/common/test_copy_point.cpp @@ -0,0 +1,188 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, 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(s) 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 OWNER 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. + * + */ + +#include + +#include + +TEST (CopyPointTest, SameTypeWithoutColor) +{ + { + pcl::PointXYZ p1 (1, 2, 3), p2 (4, 5, 6); + pcl::copyPoint (p1, p2); + EXPECT_FLOAT_EQ (p1.x, p2.x); + EXPECT_FLOAT_EQ (p1.y, p2.y); + EXPECT_FLOAT_EQ (p1.z, p2.z); + } + { + pcl::Normal p1 (1, 2, 3), p2 (4, 5, 6); + pcl::copyPoint (p1, p2); + EXPECT_FLOAT_EQ (p1.normal_x, p2.normal_x); + EXPECT_FLOAT_EQ (p1.normal_y, p2.normal_y); + EXPECT_FLOAT_EQ (p1.normal_z, p2.normal_z); + } +} + +TEST (CopyPointTest, SameTypeWithColor) +{ + { + pcl::PointXYZRGBA p1, p2; + p1.getVector3fMap () << 1, 2, 3; p1.rgba = 0xFF0000FF; + pcl::copyPoint (p1, p2); + EXPECT_FLOAT_EQ (p1.x, p2.x); + EXPECT_FLOAT_EQ (p1.y, p2.y); + EXPECT_FLOAT_EQ (p1.z, p2.z); + EXPECT_EQ (p1.rgba, p2.rgba); + } +} + +TEST (CopyPointTest, DifferentTypesWithoutColor) +{ + { + pcl::PointXYZ p1 (1, 2, 3); + pcl::PointXYZL p2; + p2.getVector3fMap () << 4, 5, 5; p2.label = 1; + pcl::copyPoint (p1, p2); + EXPECT_FLOAT_EQ (p1.x, p2.x); + EXPECT_FLOAT_EQ (p1.y, p2.y); + EXPECT_FLOAT_EQ (p1.z, p2.z); + EXPECT_EQ (1, p2.label); + } + { + pcl::PointXY p1; p1.x = 1; p1.y = 2; + pcl::PointWithRange p2; p2.getVector3fMap () << 4, 5, 6; p2.range = 8; + pcl::copyPoint (p1, p2); + EXPECT_FLOAT_EQ (p1.x, p2.x); + EXPECT_FLOAT_EQ (p1.y, p2.y); + EXPECT_FLOAT_EQ (6, p2.z); + EXPECT_FLOAT_EQ (8, p2.range); + } +} + +TEST (CopyPointTest, DifferentTypesOneWithColorAnotherWithout) +{ + // Source without color + { + pcl::PointXYZ p1 (1, 2, 3); + pcl::PointXYZRGB p2; + p2.getVector3fMap () << 4, 5, 5; p2.rgba = 0xFFFF00; + pcl::copyPoint (p1, p2); + EXPECT_FLOAT_EQ (p1.x, p2.x); + EXPECT_FLOAT_EQ (p1.y, p2.y); + EXPECT_FLOAT_EQ (p1.z, p2.z); + EXPECT_EQ (0xFFFF00, p2.rgba); + } + // Target without color + { + pcl::PointXYZRGBNormal p1; p1.getVector3fMap () << 1, 2, 3; p1.rgba = 0xFF0000; + pcl::PointWithRange p2; p2.getVector3fMap () << 4, 5, 6; p2.range = 8; + pcl::copyPoint (p1, p2); + EXPECT_FLOAT_EQ (p1.x, p2.x); + EXPECT_FLOAT_EQ (p1.y, p2.y); + EXPECT_FLOAT_EQ (p1.z, p2.z); + EXPECT_FLOAT_EQ (8, p2.range); + } +} + +TEST (CopyPointTest, DifferentTypesWithDifferentColor) +{ + { + pcl::RGB p1; + pcl::PointXYZRGB p2; p2.getVector3fMap () << 4, 5, 6; p2.r = 7; p2.g = 8; p2.b = 9, p2.a = 10; + pcl::copyPoint (p1, p2); + EXPECT_EQ (p1.rgba, p2.rgba); + EXPECT_FLOAT_EQ (4, p2.x); + EXPECT_FLOAT_EQ (5, p2.y); + EXPECT_FLOAT_EQ (6, p2.z); + } + { + pcl::PointXYZRGBNormal p1; p1.getVector3fMap () << 1, 2, 3; p1.r = 7; p1.g = 8; p1.b = 9; + pcl::PointXYZRGBL p2; p2.getVector3fMap () << 4, 5, 6; p2.r = 3; p2.g = 2; p2.b = 1; p2.label = 8; + pcl::copyPoint (p1, p2); + EXPECT_FLOAT_EQ (p1.x, p2.x); + EXPECT_FLOAT_EQ (p1.y, p2.y); + EXPECT_FLOAT_EQ (p1.z, p2.z); + EXPECT_EQ (p1.rgba, p2.rgba); + EXPECT_EQ (8, p2.label); + } + { + pcl::PointXYZRGBA p1; p1.getVector3fMap () << 1, 2, 3; p1.rgba = 0xFF00FF; + pcl::PointXYZRGB p2; p2.getVector3fMap () << 4, 5, 6; p2.rgba = 0x00FF00; + pcl::copyPoint (p1, p2); + EXPECT_FLOAT_EQ (p1.x, p2.x); + EXPECT_FLOAT_EQ (p1.y, p2.y); + EXPECT_FLOAT_EQ (p1.z, p2.z); + EXPECT_EQ (p1.rgba, p2.rgba); + } +} + +TEST (CopyPointTest, DifferentTypesWithSameColor) +{ + { + pcl::RGB p1; + pcl::PointXYZRGBA p2; p2.getVector3fMap () << 4, 5, 6; p2.r = 7; p2.g = 8; p2.b = 9, p2.a = 10; + pcl::copyPoint (p1, p2); + EXPECT_EQ (p1.rgba, p2.rgba); + EXPECT_FLOAT_EQ (4, p2.x); + EXPECT_FLOAT_EQ (5, p2.y); + EXPECT_FLOAT_EQ (6, p2.z); + } + { + pcl::PointXYZRGBNormal p1; p1.getVector3fMap () << 1, 2, 3; p1.r = 7; p1.g = 8; p1.b = 9; + pcl::PointXYZRGB p2; p2.getVector3fMap () << 4, 5, 6; p2.r = 3; p2.g = 2; p2.b = 1; + pcl::copyPoint (p1, p2); + EXPECT_FLOAT_EQ (p1.x, p2.x); + EXPECT_FLOAT_EQ (p1.y, p2.y); + EXPECT_FLOAT_EQ (p1.z, p2.z); + EXPECT_EQ (p1.rgba, p2.rgba); + } +} + +int +main (int argc, char **argv) +{ + try + { + ::testing::InitGoogleTest (&argc, argv); + ::testing::FLAGS_gtest_death_test_style = "threadsafe"; + return RUN_ALL_TESTS (); + } + catch (std::exception& e) + { + std::cerr << "Unhandled exception: " << e.what () << "\n"; + } + return 1; +} + From 9612e6fefc8011bbe096c202f304b857f286cf39 Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Thu, 23 Jan 2014 20:09:14 +0100 Subject: [PATCH 3/4] Update `copyPointCloud()` functions to use `copyPoint` --- common/include/pcl/common/impl/io.hpp | 327 ++------------------------ 1 file changed, 17 insertions(+), 310 deletions(-) diff --git a/common/include/pcl/common/impl/io.hpp b/common/include/pcl/common/impl/io.hpp index a728f704394..4fa068e7b22 100644 --- a/common/include/pcl/common/impl/io.hpp +++ b/common/include/pcl/common/impl/io.hpp @@ -42,6 +42,7 @@ #define PCL_IO_IMPL_IO_HPP_ #include +#include #include ////////////////////////////////////////////////////////////////////////////////////////////// @@ -107,14 +108,10 @@ pcl::getFieldsList (const pcl::PointCloud &) ////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::copyPointCloud (const pcl::PointCloud &cloud_in, +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out) { - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - + // Allocate enough space and copy the basics cloud_out.header = cloud_in.header; cloud_out.width = cloud_in.width; cloud_out.height = cloud_in.height; @@ -123,57 +120,13 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, cloud_out.sensor_origin_ = cloud_in.sensor_origin_; cloud_out.points.resize (cloud_in.points.size ()); - // If the point types are the same, don't copy one by one if (isSamePointType ()) - { + // Copy the whole memory block memcpy (&cloud_out.points[0], &cloud_in.points[0], cloud_in.points.size () * sizeof (PointInT)); - return; - } - - std::vector fields_in, fields_out; - pcl::for_each_type (pcl::detail::FieldAdder (fields_in)); - pcl::for_each_type (pcl::detail::FieldAdder (fields_out)); - - // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and - // fix it manually - int rgb_idx_in = -1, rgb_idx_out = -1; - for (size_t i = 0; i < fields_in.size (); ++i) - if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") - { - rgb_idx_in = int (i); - break; - } - for (size_t i = 0; i < fields_out.size (); ++i) - if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") - { - rgb_idx_out = int (i); - break; - } - - // We have one of the two cases: RGB vs RGBA or RGBA vs RGB - if (rgb_idx_in != -1 && rgb_idx_out != -1 && - fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) - { - size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), - field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); - - if (field_size_in == field_size_out) - { - for (size_t i = 0; i < cloud_in.points.size (); ++i) - { - // Copy the rest - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[i], cloud_out.points[i])); - // Copy RGB<->RGBA - memcpy (reinterpret_cast (&cloud_out.points[i]) + fields_out[rgb_idx_out].offset, reinterpret_cast (&cloud_in.points[i]) + fields_in[rgb_idx_in].offset, field_size_in); - } - return; - } - } - - // Iterate over each point if no RGB/RGBA or if their size is different - for (size_t i = 0; i < cloud_in.points.size (); ++i) - // Iterate over each dimension - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[i], cloud_out.points[i])); + else + // Iterate over each point + for (size_t i = 0; i < cloud_in.points.size (); ++i) + copyPoint (cloud_in.points[i], cloud_out.points[i]); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -232,7 +185,7 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, ////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::copyPointCloud (const pcl::PointCloud &cloud_in, +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, const std::vector &indices, pcl::PointCloud &cloud_out) { @@ -245,69 +198,14 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; cloud_out.sensor_origin_ = cloud_in.sensor_origin_; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - - // If the point types are the same, don't copy one by one - if (isSamePointType ()) - { - // Iterate over each point - for (size_t i = 0; i < indices.size (); ++i) - memcpy (&cloud_out.points[i], &cloud_in.points[indices[i]], sizeof (PointInT)); - return; - } - - std::vector fields_in, fields_out; - pcl::for_each_type (pcl::detail::FieldAdder (fields_in)); - pcl::for_each_type (pcl::detail::FieldAdder (fields_out)); - - // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and - // fix it manually - int rgb_idx_in = -1, rgb_idx_out = -1; - for (size_t i = 0; i < fields_in.size (); ++i) - if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") - { - rgb_idx_in = int (i); - break; - } - for (size_t i = 0; i < fields_out.size (); ++i) - if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") - { - rgb_idx_out = int (i); - break; - } - - // We have one of the two cases: RGB vs RGBA or RGBA vs RGB - if (rgb_idx_in != -1 && rgb_idx_out != -1 && - fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) - { - size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), - field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); - - if (field_size_in == field_size_out) - { - for (size_t i = 0; i < indices.size (); ++i) - { - // Copy the rest - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices[i]], cloud_out.points[i])); - // Copy RGB<->RGBA - memcpy (reinterpret_cast (&cloud_out.points[i]) + fields_out[rgb_idx_out].offset, reinterpret_cast (&cloud_in.points[indices[i]]) + fields_in[rgb_idx_in].offset, field_size_in); - } - return; - } - } - - // Iterate over each point if no RGB/RGBA or if their size is different + // Iterate over each point for (size_t i = 0; i < indices.size (); ++i) - // Iterate over each dimension - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices[i]], cloud_out.points[i])); + copyPoint (cloud_in.points[indices[i]], cloud_out.points[i]); } ////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::copyPointCloud (const pcl::PointCloud &cloud_in, +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, const std::vector > &indices, pcl::PointCloud &cloud_out) { @@ -320,64 +218,9 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; cloud_out.sensor_origin_ = cloud_in.sensor_origin_; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - - // If the point types are the same, don't copy one by one - if (isSamePointType ()) - { - // Iterate over each point - for (size_t i = 0; i < indices.size (); ++i) - memcpy (&cloud_out.points[i], &cloud_in.points[indices[i]], sizeof (PointInT)); - return; - } - - std::vector fields_in, fields_out; - pcl::for_each_type (pcl::detail::FieldAdder (fields_in)); - pcl::for_each_type (pcl::detail::FieldAdder (fields_out)); - - // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and - // fix it manually - int rgb_idx_in = -1, rgb_idx_out = -1; - for (size_t i = 0; i < fields_in.size (); ++i) - if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") - { - rgb_idx_in = int (i); - break; - } - for (size_t i = 0; i < fields_out.size (); ++i) - if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") - { - rgb_idx_out = int (i); - break; - } - - // We have one of the two cases: RGB vs RGBA or RGBA vs RGB - if (rgb_idx_in != -1 && rgb_idx_out != -1 && - fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) - { - size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), - field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); - - if (field_size_in == field_size_out) - { - for (size_t i = 0; i < indices.size (); ++i) - { - // Copy the rest - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices[i]], cloud_out.points[i])); - // Copy RGB<->RGBA - memcpy (reinterpret_cast (&cloud_out.points[i]) + fields_out[rgb_idx_out].offset, reinterpret_cast (&cloud_in.points[indices[i]]) + fields_in[rgb_idx_in].offset, field_size_in); - } - return; - } - } - - // Iterate over each point if no RGB/RGBA or if their size is different + // Iterate over each point for (size_t i = 0; i < indices.size (); ++i) - // Iterate over each dimension - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices[i]], cloud_out.points[i])); + copyPoint (cloud_in.points[indices[i]], cloud_out.points[i]); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -409,77 +252,11 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, /////////////////////////////////////////////////////////////////////////////////////////////// template void -pcl::copyPointCloud (const pcl::PointCloud &cloud_in, +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, const pcl::PointIndices &indices, pcl::PointCloud &cloud_out) { - // Allocate enough space and copy the basics - cloud_out.points.resize (indices.indices.size ()); - cloud_out.header = cloud_in.header; - cloud_out.width = indices.indices.size (); - cloud_out.height = 1; - cloud_out.is_dense = cloud_in.is_dense; - cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; - cloud_out.sensor_origin_ = cloud_in.sensor_origin_; - - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - - // If the point types are the same, don't copy one by one - if (isSamePointType ()) - { - // Iterate over each point - for (size_t i = 0; i < indices.indices.size (); ++i) - memcpy (&cloud_out.points[i], &cloud_in.points[indices.indices[i]], sizeof (PointInT)); - return; - } - - std::vector fields_in, fields_out; - pcl::for_each_type (pcl::detail::FieldAdder (fields_in)); - pcl::for_each_type (pcl::detail::FieldAdder (fields_out)); - - // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and - // fix it manually - int rgb_idx_in = -1, rgb_idx_out = -1; - for (size_t i = 0; i < fields_in.size (); ++i) - if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") - { - rgb_idx_in = int (i); - break; - } - for (size_t i = 0; i < fields_out.size (); ++i) - if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") - { - rgb_idx_out = int (i); - break; - } - - // We have one of the two cases: RGB vs RGBA or RGBA vs RGB - if (rgb_idx_in != -1 && rgb_idx_out != -1 && - fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) - { - size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), - field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); - - if (field_size_in == field_size_out) - { - for (size_t i = 0; i < indices.indices.size (); ++i) - { - // Copy the rest - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices.indices[i]], cloud_out.points[i])); - // Copy RGB<->RGBA - memcpy (reinterpret_cast (&cloud_out.points[i]) + fields_out[rgb_idx_out].offset, reinterpret_cast (&cloud_in.points[indices.indices[i]]) + fields_in[rgb_idx_in].offset, field_size_in); - } - return; - } - } - - // Iterate over each point if no RGB/RGBA or if their size is different - for (size_t i = 0; i < indices.indices.size (); ++i) - // Iterate over each dimension - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices.indices[i]], cloud_out.points[i])); + copyPointCloud (cloud_in, indices.indices, cloud_out); } ////////////////////////////////////////////////////////////////////////////////////////////// @@ -548,75 +325,6 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; cloud_out.sensor_origin_ = cloud_in.sensor_origin_; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - - // If the point types are the same, don't copy one by one - if (isSamePointType ()) - { - // Iterate over each cluster - int cp = 0; - for (size_t cc = 0; cc < indices.size (); ++cc) - { - // Iterate over each idx - for (size_t i = 0; i < indices[cc].indices.size (); ++i) - { - cloud_out.points[cp] = cloud_in.points[indices[cc].indices[i]]; - ++cp; - } - } - return; - } - - std::vector fields_in, fields_out; - pcl::for_each_type (pcl::detail::FieldAdder (fields_in)); - pcl::for_each_type (pcl::detail::FieldAdder (fields_out)); - - // RGB vs RGBA is an official missmatch until PCL 2.0, so we need to search for it and - // fix it manually - int rgb_idx_in = -1, rgb_idx_out = -1; - for (size_t i = 0; i < fields_in.size (); ++i) - if (fields_in[i].name == "rgb" || fields_in[i].name == "rgba") - { - rgb_idx_in = int (i); - break; - } - for (size_t i = 0; i < fields_out.size (); ++i) - if (fields_out[i].name == "rgb" || fields_out[i].name == "rgba") - { - rgb_idx_out = int (i); - break; - } - - // We have one of the two cases: RGB vs RGBA or RGBA vs RGB - if (rgb_idx_in != -1 && rgb_idx_out != -1 && - fields_in[rgb_idx_in].name != fields_out[rgb_idx_out].name) - { - size_t field_size_in = getFieldSize (fields_in[rgb_idx_in].datatype), - field_size_out = getFieldSize (fields_out[rgb_idx_out].datatype); - - if (field_size_in == field_size_out) - { - // Iterate over each cluster - int cp = 0; - for (size_t cc = 0; cc < indices.size (); ++cc) - { - // Iterate over each idx - for (size_t i = 0; i < indices[cc].indices.size (); ++i) - { - // Iterate over each dimension - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp])); - // Copy RGB<->RGBA - memcpy (reinterpret_cast (&cloud_out.points[cp]) + fields_out[rgb_idx_out].offset, reinterpret_cast (&cloud_in.points[indices[cp].indices[i]]) + fields_in[rgb_idx_in].offset, field_size_in); - ++cp; - } - } - return; - } - } - // Iterate over each cluster int cp = 0; for (size_t cc = 0; cc < indices.size (); ++cc) @@ -624,8 +332,7 @@ pcl::copyPointCloud (const pcl::PointCloud &cloud_in, // Iterate over each idx for (size_t i = 0; i < indices[cc].indices.size (); ++i) { - // Iterate over each dimension - pcl::for_each_type (pcl::NdConcatenateFunctor (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp])); + copyPoint (cloud_in.points[indices[cc].indices[i]], cloud_out.points[cp]); ++cp; } } From a8d5fa739ffec50e24f49595d25ba7a5fc3b28f2 Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Thu, 23 Jan 2014 20:09:35 +0100 Subject: [PATCH 4/4] Update various PCL classes to use `copyPoint()` --- .../include/pcl/filters/conditional_removal.h | 2 -- .../pcl/filters/impl/conditional_removal.hpp | 10 ++++----- kdtree/include/pcl/kdtree/kdtree.h | 13 +++--------- .../impl/correspondence_estimation.hpp | 21 +++++-------------- ...rrespondence_estimation_backprojection.hpp | 13 ++++-------- ...respondence_estimation_normal_shooting.hpp | 13 ++++-------- search/include/pcl/search/search.h | 13 +++--------- .../segmentation/impl/segment_differences.hpp | 8 ++----- surface/include/pcl/surface/impl/mls.hpp | 8 ++----- 9 files changed, 27 insertions(+), 74 deletions(-) diff --git a/filters/include/pcl/filters/conditional_removal.h b/filters/include/pcl/filters/conditional_removal.h index fc32a74b467..7905ab6a27a 100644 --- a/filters/include/pcl/filters/conditional_removal.h +++ b/filters/include/pcl/filters/conditional_removal.h @@ -682,8 +682,6 @@ namespace pcl void applyFilter (PointCloud &output); - typedef typename pcl::traits::fieldList::type FieldList; - /** \brief True if capable. */ bool capable_; diff --git a/filters/include/pcl/filters/impl/conditional_removal.hpp b/filters/include/pcl/filters/impl/conditional_removal.hpp index cae746b1ba7..9a20beff4e6 100644 --- a/filters/include/pcl/filters/impl/conditional_removal.hpp +++ b/filters/include/pcl/filters/impl/conditional_removal.hpp @@ -39,6 +39,7 @@ #define PCL_FILTER_IMPL_FIELD_VAL_CONDITION_H_ #include +#include #include ////////////////////////////////////////////////////////////////////////// @@ -725,10 +726,7 @@ pcl::ConditionalRemoval::applyFilter (PointCloud &output) if (condition_->evaluate (input_->points[(*Filter < PointT > ::indices_)[cp]])) { - pcl::for_each_type ( - pcl::NdConcatenateFunctor ( - input_->points[(*Filter < PointT > ::indices_)[cp]], - output.points[nr_p])); + copyPoint (input_->points[(*Filter < PointT > ::indices_)[cp]], output.points[nr_p]); nr_p++; } else @@ -762,8 +760,8 @@ pcl::ConditionalRemoval::applyFilter (PointCloud &output) } // copy all the fields - pcl::for_each_type (pcl::NdConcatenateFunctor (input_->points[cp], - output.points[cp])); + copyPoint (input_->points[cp], output.points[cp]); + if (!condition_->evaluate (input_->points[cp])) { output.points[cp].getVector4fMap ().setConstant (user_filter_value_); diff --git a/kdtree/include/pcl/kdtree/kdtree.h b/kdtree/include/pcl/kdtree/kdtree.h index a554991a7a0..586e4945e1f 100644 --- a/kdtree/include/pcl/kdtree/kdtree.h +++ b/kdtree/include/pcl/kdtree/kdtree.h @@ -44,6 +44,7 @@ #include #include #include +#include namespace pcl { @@ -175,11 +176,7 @@ namespace pcl std::vector &k_indices, std::vector &k_sqr_distances) const { PointT p; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - pcl::for_each_type (pcl::NdConcatenateFunctor (point, p)); + copyPoint (point, p); return (nearestKSearch (p, k, k_indices, k_sqr_distances)); } @@ -271,11 +268,7 @@ namespace pcl std::vector &k_sqr_distances, unsigned int max_nn = 0) const { PointT p; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - pcl::for_each_type (pcl::NdConcatenateFunctor (point, p)); + copyPoint (point, p); return (radiusSearch (p, radius, k_indices, k_sqr_distances, max_nn)); } diff --git a/registration/include/pcl/registration/impl/correspondence_estimation.hpp b/registration/include/pcl/registration/impl/correspondence_estimation.hpp index 4b02d68095d..bdc2e87bfff 100644 --- a/registration/include/pcl/registration/impl/correspondence_estimation.hpp +++ b/registration/include/pcl/registration/impl/correspondence_estimation.hpp @@ -40,8 +40,8 @@ #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ -#include #include +#include /////////////////////////////////////////////////////////////////////////////////////////// template void @@ -132,7 +132,6 @@ pcl::registration::CorrespondenceEstimation::d double max_dist_sqr = max_distance * max_distance; - typedef typename pcl::traits::fieldList::type FieldListTarget; correspondences.resize (indices_->size ()); std::vector index (1); @@ -165,9 +164,7 @@ pcl::registration::CorrespondenceEstimation::d for (std::vector::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) { // Copy the source data to a target PointTarget format so we can search in the tree - pcl::for_each_type (pcl::NdConcatenateFunctor ( - input_->points[*idx], - pt)); + copyPoint (input_->points[*idx], pt); tree_->nearestKSearch (pt, 1, index, distance); if (distance[0] > max_dist_sqr) @@ -190,11 +187,7 @@ pcl::registration::CorrespondenceEstimation::d { if (!initCompute ()) return; - - typedef typename pcl::traits::fieldList::type FieldListSource; - typedef typename pcl::traits::fieldList::type FieldListTarget; - typedef typename pcl::intersect::type FieldList; - + // setup tree for reciprocal search // Set the internal point representation of choice if (!initComputeReciprocal()) @@ -242,9 +235,7 @@ pcl::registration::CorrespondenceEstimation::d for (std::vector::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx) { // Copy the source data to a target PointTarget format so we can search in the tree - pcl::for_each_type (pcl::NdConcatenateFunctor ( - input_->points[*idx], - pt_src)); + copyPoint (input_->points[*idx], pt_src); tree_->nearestKSearch (pt_src, 1, index, distance); if (distance[0] > max_dist_sqr) @@ -253,9 +244,7 @@ pcl::registration::CorrespondenceEstimation::d target_idx = index[0]; // Copy the target data to a target PointSource format so we can search in the tree_reciprocal - pcl::for_each_type (pcl::NdConcatenateFunctor ( - target_->points[target_idx], - pt_tgt)); + copyPoint (target_->points[target_idx], pt_tgt); tree_reciprocal_->nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal); if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0]) diff --git a/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp b/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp index dad8e1a66ad..7284f3e7c30 100644 --- a/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp +++ b/registration/include/pcl/registration/impl/correspondence_estimation_backprojection.hpp @@ -39,6 +39,8 @@ #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_ +#include + /////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::registration::CorrespondenceEstimationBackProjection::initCompute () @@ -60,7 +62,6 @@ pcl::registration::CorrespondenceEstimationBackProjection::type FieldListTarget; correspondences.resize (indices_->size ()); std::vector nn_indices (k_); @@ -125,9 +126,7 @@ pcl::registration::CorrespondenceEstimationBackProjection (pcl::NdConcatenateFunctor ( - input_->points[*idx_i], - pt_src)); + copyPoint (input_->points[*idx_i], pt_src); float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x + source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + @@ -161,8 +160,6 @@ pcl::registration::CorrespondenceEstimationBackProjection::type FieldListTarget; - // Set the internal point representation of choice if(!initComputeReciprocal()) return; @@ -241,9 +238,7 @@ pcl::registration::CorrespondenceEstimationBackProjection (pcl::NdConcatenateFunctor ( - input_->points[*idx_i], - pt_src)); + copyPoint (input_->points[*idx_i], pt_src); float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x + source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y + diff --git a/registration/include/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp b/registration/include/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp index b6fdb33d250..4402c5460e0 100644 --- a/registration/include/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp +++ b/registration/include/pcl/registration/impl/correspondence_estimation_normal_shooting.hpp @@ -40,6 +40,8 @@ #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_ #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_ +#include + /////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::registration::CorrespondenceEstimationNormalShooting::initCompute () @@ -61,7 +63,6 @@ pcl::registration::CorrespondenceEstimationNormalShooting::type FieldListTarget; correspondences.resize (indices_->size ()); std::vector nn_indices (k_); @@ -134,9 +135,7 @@ pcl::registration::CorrespondenceEstimationNormalShooting (pcl::NdConcatenateFunctor ( - input_->points[*idx_i], - pt_src)); + copyPoint (input_->points[*idx_i], pt_src); // computing the distance between a point and a line in 3d. // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html @@ -178,8 +177,6 @@ pcl::registration::CorrespondenceEstimationNormalShooting::type FieldListTarget; - // setup tree for reciprocal search // Set the internal point representation of choice if (!initComputeReciprocal ()) @@ -268,9 +265,7 @@ pcl::registration::CorrespondenceEstimationNormalShooting (pcl::NdConcatenateFunctor ( - input_->points[*idx_i], - pt_src)); + copyPoint (input_->points[*idx_i], pt_src); // computing the distance between a point and a line in 3d. // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html diff --git a/search/include/pcl/search/search.h b/search/include/pcl/search/search.h index 6110c18c05f..1d362c1a841 100644 --- a/search/include/pcl/search/search.h +++ b/search/include/pcl/search/search.h @@ -42,6 +42,7 @@ #include #include #include +#include namespace pcl { @@ -159,11 +160,7 @@ namespace pcl std::vector &k_indices, std::vector &k_sqr_distances) const { PointT p; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - pcl::for_each_type (pcl::NdConcatenateFunctor (point, p)); + copyPoint (point, p); return (nearestKSearch (p, k, k_indices, k_sqr_distances)); } @@ -291,11 +288,7 @@ namespace pcl std::vector &k_sqr_distances, unsigned int max_nn = 0) const { PointT p; - // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldListInT; - typedef typename pcl::traits::fieldList::type FieldListOutT; - typedef typename pcl::intersect::type FieldList; - pcl::for_each_type (pcl::NdConcatenateFunctor (point, p)); + copyPoint (point, p); return (radiusSearch (p, radius, k_indices, k_sqr_distances, max_nn)); } diff --git a/segmentation/include/pcl/segmentation/impl/segment_differences.hpp b/segmentation/include/pcl/segmentation/impl/segment_differences.hpp index 8b457b46305..c409ba67ca0 100644 --- a/segmentation/include/pcl/segmentation/impl/segment_differences.hpp +++ b/segmentation/include/pcl/segmentation/impl/segment_differences.hpp @@ -39,7 +39,7 @@ #define PCL_SEGMENTATION_IMPL_SEGMENT_DIFFERENCES_H_ #include -#include +#include ////////////////////////////////////////////////////////////////////////// template void @@ -85,11 +85,7 @@ pcl::getPointCloudDifference ( //output.is_dense = false; // Copy all the data fields from the input cloud to the output one - typedef typename pcl::traits::fieldList::type FieldList; - // Iterate over each point - for (size_t i = 0; i < src_indices.size (); ++i) - // Iterate over each dimension - pcl::for_each_type (NdConcatenateFunctor (src.points[src_indices[i]], output.points[i])); + copyPointCloud (src, src_indices, output); } ////////////////////////////////////////////////////////////////////////// diff --git a/surface/include/pcl/surface/impl/mls.hpp b/surface/include/pcl/surface/impl/mls.hpp index 6f9b5679b7e..578c0d8528f 100644 --- a/surface/include/pcl/surface/impl/mls.hpp +++ b/surface/include/pcl/surface/impl/mls.hpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -752,13 +753,8 @@ template void pcl::MovingLeastSquares::copyMissingFields (const PointInT &point_in, PointOutT &point_out) const { - typedef typename pcl::traits::fieldList::type FieldListInput; - typedef typename pcl::traits::fieldList::type FieldListOutput; - typedef typename pcl::intersect::type FieldList; - PointOutT temp = point_out; - pcl::for_each_type (pcl::NdConcatenateFunctor (point_in, - point_out)); + copyPoint (point_in, point_out); point_out.x = temp.x; point_out.y = temp.y; point_out.z = temp.z;