From 78ba8c82f6de8f9e3a103241c6e8ed48f9688e7b Mon Sep 17 00:00:00 2001 From: Nizar Sallem Date: Tue, 4 Mar 2014 19:07:15 +0100 Subject: [PATCH 1/3] Add: pcl::BadArgumentsException class to be thrown from namespace For void free functions when arguments are wrong throw this exception --- common/include/pcl/exceptions.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/common/include/pcl/exceptions.h b/common/include/pcl/exceptions.h index 57c8ba65ffc..9038572c01b 100644 --- a/common/include/pcl/exceptions.h +++ b/common/include/pcl/exceptions.h @@ -251,6 +251,15 @@ namespace pcl : pcl::PCLException (error_description, file_name, function_name, line_number) { } }; + class BadArgumentsException : public PCLException + { + public: + BadArgumentsException (const std::string& error_description, + const std::string& file_name = "", + const std::string& function_name = "" , + unsigned line_number = 0) throw () + : pcl::PCLException (error_description, file_name, function_name, line_number) { } + }; } From 8afd5aa75de5bea2c9fc4c1e2529c98bca439f6a Mon Sep 17 00:00:00 2001 From: Nizar Sallem Date: Tue, 4 Mar 2014 19:09:24 +0100 Subject: [PATCH 2/3] Add function to compute interpolated index from current index, length and border type --- common/CMakeLists.txt | 1 + common/include/pcl/common/utils.h | 22 ++++++++++++++++ common/src/utils.cpp | 43 +++++++++++++++++++++++++++++++ 3 files changed, 66 insertions(+) create mode 100644 common/src/utils.cpp diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt index 293f273278a..aae98996743 100644 --- a/common/CMakeLists.txt +++ b/common/CMakeLists.txt @@ -42,6 +42,7 @@ if(build) src/projection_matrix.cpp src/time_trigger.cpp src/gaussian.cpp + src/utils.cpp ${range_image_srcs} ) diff --git a/common/include/pcl/common/utils.h b/common/include/pcl/common/utils.h index 179a4bf0616..c472f8522d4 100644 --- a/common/include/pcl/common/utils.h +++ b/common/include/pcl/common/utils.h @@ -56,6 +56,28 @@ namespace pcl { return (fabs (val1 - val2) < eps); } + + enum + { + BORDER_CONSTANT = 0, BORDER_REPLICATE = 1, + BORDER_REFLECT = 2, BORDER_WRAP = 3, + BORDER_REFLECT_101 = 4, BORDER_TRANSPARENT = 5, + BORDER_DEFAULT = BORDER_REFLECT_101 + }; + + /** \brief \return the right index according to the interpolation type. + * \note this is adapted from OpenCV + * \param p the index to interpolate + * \param length the top/bottom row or left/right column index + * \param type + * BORDER_REPLICATE: aaaaaa|abcdefgh|hhhhhhh + * BORDER_REFLECT: fedcba|abcdefgh|hgfedcb + * BORDER_REFLECT_101: gfedcb|abcdefgh|gfedcba + * BORDER_WRAP: cdefgh|abcdefgh|abcdefg + * BORDER_CONSTANT: iiiiii|abcdefgh|iiiiiii with some specified 'i' + */ + int + interpolate (int p, int length, int type); } } diff --git a/common/src/utils.cpp b/common/src/utils.cpp new file mode 100644 index 00000000000..b3ee54a9023 --- /dev/null +++ b/common/src/utils.cpp @@ -0,0 +1,43 @@ +#include +#include + +int +pcl::utils::interpolate (int p, int len, int type) +{ + if (static_cast (p) >= static_cast (len)) + { + if (type == BORDER_REPLICATE) + p = p < 0 ? 0 : len - 1; + else if (type == BORDER_REFLECT || type == BORDER_REFLECT_101) + { + int delta = type == BORDER_REFLECT_101; + if (len == 1) + return 0; + do + { + if (p < 0) + p = -p - 1 + delta; + else + p = len - 1 - (p - len) - delta; + } + while (static_cast (p) >= static_cast (len)); + } + else if (type == BORDER_WRAP) + { + if (p < 0) + p -= ((p-len+1)/len)*len; + if (p >= len) + p %= len; + } + else if (type == BORDER_CONSTANT) + p = -1; + else + { + PCL_THROW_EXCEPTION (BadArgumentsException, + "[pcl::interpolate] error: Unhandled interpolation type " + << type << " !"); + } + } + + return (p); +} From 9c2dcecf1090cab9db969708b06eacb32cbeb19f Mon Sep 17 00:00:00 2001 From: Nizar Sallem Date: Tue, 4 Mar 2014 19:15:21 +0100 Subject: [PATCH 3/3] Add: possibility to copy a cloud A inside cloud B Add function that allows to opy a point cloud A inside a bigger point cloud B. The position where to copy is given by top, bottom, left and right margins. The empty lines/columns are filled by interpolating original cloud lines/columns The aim is to replace spring stuff with faster method --- common/CMakeLists.txt | 1 - common/include/pcl/common/impl/io.hpp | 126 ++++++++++++++++++ common/include/pcl/common/io.h | 39 ++++++ common/include/pcl/common/utils.h | 22 ---- common/include/pcl/exceptions.h | 13 +- common/src/io.cpp | 41 ++++++ common/src/utils.cpp | 43 ------- test/common/CMakeLists.txt | 1 + test/common/test_copy_make_borders.cpp | 171 +++++++++++++++++++++++++ 9 files changed, 386 insertions(+), 71 deletions(-) delete mode 100644 common/src/utils.cpp create mode 100644 test/common/test_copy_make_borders.cpp diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt index aae98996743..293f273278a 100644 --- a/common/CMakeLists.txt +++ b/common/CMakeLists.txt @@ -42,7 +42,6 @@ if(build) src/projection_matrix.cpp src/time_trigger.cpp src/gaussian.cpp - src/utils.cpp ${range_image_srcs} ) diff --git a/common/include/pcl/common/impl/io.hpp b/common/include/pcl/common/impl/io.hpp index 4fa068e7b22..c6173ec96bd 100644 --- a/common/include/pcl/common/impl/io.hpp +++ b/common/include/pcl/common/impl/io.hpp @@ -372,5 +372,131 @@ pcl::concatenateFields (const pcl::PointCloud &cloud1_in, } } +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::copyPointCloud (const pcl::PointCloud &cloud_in, pcl::PointCloud &cloud_out, + int top, int bottom, int left, int right, pcl::InterpolationType border_type, const PointT& value) +{ + if (top < 0 || left < 0 || bottom < 0 || right < 0) + { + std::string faulty = (top < 0) ? "top" : (left < 0) ? "left" : (bottom < 0) ? "bottom" : "right"; + PCL_THROW_EXCEPTION (pcl::BadArgumentException, "[pcl::copyPointCloud] error: " << faulty << " must be positive!"); + return; + } + + if (top == 0 && left == 0 && bottom == 0 && right == 0) + cloud_out = cloud_in; + else + { + // Allocate enough space and copy the basics + cloud_out.header = cloud_in.header; + cloud_out.width = cloud_in.width + left + right; + cloud_out.height = cloud_in.height + top + bottom; + if (cloud_out.size () != cloud_out.width * cloud_out.height) + cloud_out.resize (cloud_out.width * cloud_out.height); + cloud_out.is_dense = cloud_in.is_dense; + cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; + cloud_out.sensor_origin_ = cloud_in.sensor_origin_; + + if (border_type == pcl::BORDER_TRANSPARENT) + { + const PointT* in = &(cloud_in.points[0]); + PointT* out = &(cloud_out.points[0]); + PointT* out_inner = out + cloud_out.width*top + left; + for (int i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width) + { + if (out_inner != in) + memcpy (out_inner, in, cloud_in.width * sizeof (PointT)); + } + } + else + { + // Copy the data + if (border_type != pcl::BORDER_CONSTANT) + { + try + { + std::vector padding (cloud_out.width - cloud_in.width); + int right = cloud_out.width - cloud_in.width - left; + int bottom = cloud_out.height - cloud_in.height - top; + + for (int i = 0; i < left; i++) + padding[i] = pcl::interpolatePointIndex (i-left, cloud_in.width, border_type); + + for (int i = 0; i < right; i++) + padding[i+left] = pcl::interpolatePointIndex (cloud_in.width+i, cloud_in.width, border_type); + + const PointT* in = &(cloud_in.points[0]); + PointT* out = &(cloud_out.points[0]); + PointT* out_inner = out + cloud_out.width*top + left; + + for (int i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width) + { + if (out_inner != in) + memcpy (out_inner, in, cloud_in.width * sizeof (PointT)); + + for (int j = 0; j < left; j++) + out_inner[j - left] = in[padding[j]]; + + for (int j = 0; j < right; j++) + out_inner[j + cloud_in.width] = in[padding[j + left]]; + } + + for (int i = 0; i < top; i++) + { + int j = pcl::interpolatePointIndex (i - top, cloud_in.height, border_type); + memcpy (out + i*cloud_out.width, + out + (j+top) * cloud_out.width, + sizeof (PointT) * cloud_out.width); + } + + for (int i = 0; i < bottom; i++) + { + int j = pcl::interpolatePointIndex (i + cloud_in.height, cloud_in.height, border_type); + memcpy (out + (i + cloud_in.height + top)*cloud_out.width, + out + (j+top)*cloud_out.width, + cloud_out.width * sizeof (PointT)); + } + } + catch (pcl::BadArgumentException &e) + { + PCL_ERROR ("[pcl::copyPointCloud] Unhandled interpolation type %d!\n", border_type); + } + } + else + { + int right = cloud_out.width - cloud_in.width - left; + int bottom = cloud_out.height - cloud_in.height - top; + std::vector buff (cloud_out.width, value); + PointT* buff_ptr = &(buff[0]); + const PointT* in = &(cloud_in.points[0]); + PointT* out = &(cloud_out.points[0]); + PointT* out_inner = out + cloud_out.width*top + left; + + for (int i = 0; i < cloud_in.height; i++, out_inner += cloud_out.width, in += cloud_in.width) + { + if (out_inner != in) + memcpy (out_inner, in, cloud_in.width * sizeof (PointT)); + + memcpy (out_inner - left, buff_ptr, left * sizeof (PointT)); + memcpy (out_inner + cloud_in.width, buff_ptr, right * sizeof (PointT)); + } + + for (int i = 0; i < top; i++) + { + memcpy (out + i*cloud_out.width, buff_ptr, cloud_out.width * sizeof (PointT)); + } + + for (int i = 0; i < bottom; i++) + { + memcpy (out + (i + cloud_in.height + top)*cloud_out.width, + buff_ptr, + cloud_out.width * sizeof (PointT)); + } + } + } + } +} + #endif // PCL_IO_IMPL_IO_H_ diff --git a/common/include/pcl/common/io.h b/common/include/pcl/common/io.h index f2c415ff7eb..bbc5a66436f 100644 --- a/common/include/pcl/common/io.h +++ b/common/include/pcl/common/io.h @@ -45,6 +45,7 @@ #include #include #include +#include #include namespace pcl @@ -223,6 +224,24 @@ namespace pcl } } + typedef enum + { + BORDER_CONSTANT = 0, BORDER_REPLICATE = 1, + BORDER_REFLECT = 2, BORDER_WRAP = 3, + BORDER_REFLECT_101 = 4, BORDER_TRANSPARENT = 5, + BORDER_DEFAULT = BORDER_REFLECT_101 + } InterpolationType; + + /** \brief \return the right index according to the interpolation type. + * \note this is adapted from OpenCV + * \param p the index of point to interpolate + * \param length the top/bottom row or left/right column index + * \param type the requested interpolation + * \throws pcl::BadArgumentException if type is unknown + */ + PCL_EXPORTS int + interpolatePointIndex (int p, int length, InterpolationType type); + /** \brief Concatenate two pcl::PCLPointCloud2. * \param[in] cloud1 the first input point cloud dataset * \param[in] cloud2 the second input point cloud dataset @@ -380,6 +399,26 @@ namespace pcl const std::vector &indices, pcl::PointCloud &cloud_out); + /** \brief Copy a point cloud inside a larger one interpolating borders. + * \param[in] cloud_in the input point cloud dataset + * \param[out] cloud_out the resultant output point cloud dataset + * Position of cloud_in inside cloud_out is given by \a top, \a left, \a bottom \a right. + * \param[in] border_type the interpolating method (pcl::BORDER_XXX) + * BORDER_REPLICATE: aaaaaa|abcdefgh|hhhhhhh + * BORDER_REFLECT: fedcba|abcdefgh|hgfedcb + * BORDER_REFLECT_101: gfedcb|abcdefgh|gfedcba + * BORDER_WRAP: cdefgh|abcdefgh|abcdefg + * BORDER_CONSTANT: iiiiii|abcdefgh|iiiiiii with some specified 'i' + * BORDER_TRANSPARENT: mnopqr|abcdefgh|tuvwxyz where m-r and t-z are orignal values of cloud_out + * \throw pcl::BadArgumentException if any of top, bottom, left or right is negative. + * \ingroup common + */ + template void + copyPointCloud (const pcl::PointCloud &cloud_in, + pcl::PointCloud &cloud_out, + int top, int bottom, int left, int right, + pcl::InterpolationType border_type, const PointT& value); + /** \brief Concatenate two datasets representing different fields. * * \note If the input datasets have overlapping fields (i.e., both contain diff --git a/common/include/pcl/common/utils.h b/common/include/pcl/common/utils.h index c472f8522d4..179a4bf0616 100644 --- a/common/include/pcl/common/utils.h +++ b/common/include/pcl/common/utils.h @@ -56,28 +56,6 @@ namespace pcl { return (fabs (val1 - val2) < eps); } - - enum - { - BORDER_CONSTANT = 0, BORDER_REPLICATE = 1, - BORDER_REFLECT = 2, BORDER_WRAP = 3, - BORDER_REFLECT_101 = 4, BORDER_TRANSPARENT = 5, - BORDER_DEFAULT = BORDER_REFLECT_101 - }; - - /** \brief \return the right index according to the interpolation type. - * \note this is adapted from OpenCV - * \param p the index to interpolate - * \param length the top/bottom row or left/right column index - * \param type - * BORDER_REPLICATE: aaaaaa|abcdefgh|hhhhhhh - * BORDER_REFLECT: fedcba|abcdefgh|hgfedcb - * BORDER_REFLECT_101: gfedcb|abcdefgh|gfedcba - * BORDER_WRAP: cdefgh|abcdefgh|abcdefg - * BORDER_CONSTANT: iiiiii|abcdefgh|iiiiiii with some specified 'i' - */ - int - interpolate (int p, int length, int type); } } diff --git a/common/include/pcl/exceptions.h b/common/include/pcl/exceptions.h index 9038572c01b..b3462da9d6a 100644 --- a/common/include/pcl/exceptions.h +++ b/common/include/pcl/exceptions.h @@ -251,13 +251,16 @@ namespace pcl : pcl::PCLException (error_description, file_name, function_name, line_number) { } }; - class BadArgumentsException : public PCLException + /** \class BadArgumentException + * \brief An exception that is thrown when the argments number or type is wrong/unhandled. + */ + class BadArgumentException : public PCLException { public: - BadArgumentsException (const std::string& error_description, - const std::string& file_name = "", - const std::string& function_name = "" , - unsigned line_number = 0) throw () + BadArgumentException (const std::string& error_description, + const std::string& file_name = "", + const std::string& function_name = "" , + unsigned line_number = 0) throw () : pcl::PCLException (error_description, file_name, function_name, line_number) { } }; } diff --git a/common/src/io.cpp b/common/src/io.cpp index 561d0b13875..cd260873b49 100644 --- a/common/src/io.cpp +++ b/common/src/io.cpp @@ -474,3 +474,44 @@ pcl::copyPointCloud (const pcl::PCLPointCloud2 &cloud_in, cloud_out.data = cloud_in.data; } +//////////////////////////////////////////////////////////////////////////////// +int +pcl::interpolatePointIndex (int p, int len, InterpolationType type) +{ + if (static_cast (p) >= static_cast (len)) + { + if (type == BORDER_REPLICATE) + p = p < 0 ? 0 : len - 1; + else if (type == BORDER_REFLECT || type == BORDER_REFLECT_101) + { + int delta = type == BORDER_REFLECT_101; + if (len == 1) + return 0; + do + { + if (p < 0) + p = -p - 1 + delta; + else + p = len - 1 - (p - len) - delta; + } + while (static_cast (p) >= static_cast (len)); + } + else if (type == BORDER_WRAP) + { + if (p < 0) + p -= ((p-len+1)/len)*len; + if (p >= len) + p %= len; + } + else if (type == BORDER_CONSTANT) + p = -1; + else + { + PCL_THROW_EXCEPTION (BadArgumentException, + "[pcl::interpolate_point_index] error: Unhandled interpolation type " + << type << " !"); + } + } + + return (p); +} diff --git a/common/src/utils.cpp b/common/src/utils.cpp deleted file mode 100644 index b3ee54a9023..00000000000 --- a/common/src/utils.cpp +++ /dev/null @@ -1,43 +0,0 @@ -#include -#include - -int -pcl::utils::interpolate (int p, int len, int type) -{ - if (static_cast (p) >= static_cast (len)) - { - if (type == BORDER_REPLICATE) - p = p < 0 ? 0 : len - 1; - else if (type == BORDER_REFLECT || type == BORDER_REFLECT_101) - { - int delta = type == BORDER_REFLECT_101; - if (len == 1) - return 0; - do - { - if (p < 0) - p = -p - 1 + delta; - else - p = len - 1 - (p - len) - delta; - } - while (static_cast (p) >= static_cast (len)); - } - else if (type == BORDER_WRAP) - { - if (p < 0) - p -= ((p-len+1)/len)*len; - if (p >= len) - p %= len; - } - else if (type == BORDER_CONSTANT) - p = -1; - else - { - PCL_THROW_EXCEPTION (BadArgumentsException, - "[pcl::interpolate] error: Unhandled interpolation type " - << type << " !"); - } - } - - return (p); -} diff --git a/test/common/CMakeLists.txt b/test/common/CMakeLists.txt index d9b6f751555..6de740923eb 100644 --- a/test/common/CMakeLists.txt +++ b/test/common/CMakeLists.txt @@ -14,5 +14,6 @@ PCL_ADD_TEST(common_eigen test_eigen FILES test_eigen.cpp LINK_WITH pcl_gtest pc PCL_ADD_TEST(common_intensity test_intensity FILES test_intensity.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_generator test_generator FILES test_generator.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_io test_common_io FILES test_io.cpp LINK_WITH pcl_gtest pcl_common) +PCL_ADD_TEST(common_copy_make_borders test_copy_make_borders FILES test_copy_make_borders.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_point_type_conversion test_common_point_type_conversion FILES test_point_type_conversion.cpp LINK_WITH pcl_gtest pcl_common) diff --git a/test/common/test_copy_make_borders.cpp b/test/common/test_copy_make_borders.cpp new file mode 100644 index 00000000000..e1e67b89970 --- /dev/null +++ b/test/common/test_copy_make_borders.cpp @@ -0,0 +1,171 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, 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. + * + */ + +#include +#include +#include +#include +#include + +using namespace pcl; +using namespace pcl::test; + +pcl::PointCloud cloud (64,48); +int top = 2, bottom = 2, left = 2, right = 2; +const pcl::PointXYZ constant (0, 0, 0); + +TEST (CopyPointCloud, constant) +{ + pcl::PointCloud dst (cloud.width + left + right, cloud.height + top + bottom); + pcl::copyPointCloud (cloud, dst, top, bottom, left, right, pcl::BORDER_CONSTANT, constant); + + for (int j = 0; j < top; ++j) + for (int i = 0; i < dst.width; ++i) + EXPECT_XYZ_EQ (dst (i,j), constant); + + for (int j = top; j < cloud.height+top; ++j) + { + for (int i = 0; i < dst.width; ++i) + { + if (i < left) + EXPECT_XYZ_EQ (dst (i,j), constant); + else + { + if (i >= (cloud.width + left)) + EXPECT_XYZ_EQ (dst (i,j), constant); + else + EXPECT_XYZ_EQ (dst (i,j), cloud (i - left, j -top)); + } + } + } + + for (int j = cloud.height+top; j < dst.height; ++j) + for (int i = 0; i < dst.width; ++i) + EXPECT_XYZ_EQ (dst (i,j), constant); +} + +TEST (CopyPointCloud, replicate) +{ + pcl::PointCloud dst (cloud.width + left + right, cloud.height + top + bottom); + pcl::copyPointCloud (cloud, dst, top, bottom, left, right, pcl::BORDER_REPLICATE, constant); + + for (int j = 0; j < top; ++j) + { + for (int i = 0; i < left; ++i) + EXPECT_XYZ_EQ (dst (i,j), cloud (0,0)); + for (int i = left; i < cloud.width+left; ++i) + EXPECT_XYZ_EQ (dst (i,j), cloud (i-left,0)); + for (int i = cloud.width+left; i < dst.width; ++i) + EXPECT_XYZ_EQ (dst (i,j), cloud (cloud.width-1,0)); + } + + for (int j = top; j < cloud.height+top; ++j) + { + for (int i = 0; i < dst.width; ++i) + { + if (i < left) + EXPECT_XYZ_EQ (dst (i,j), cloud (0,j-top)); + else + { + if (i >= (cloud.width + left)) + EXPECT_XYZ_EQ (dst (i,j), cloud (cloud.width-1,j-top)); + else + EXPECT_XYZ_EQ (dst (i,j), cloud (i - left, j -top)); + } + } + } + + for (int j = cloud.height+top; j < dst.height; ++j) + { + for (int i = 0; i < left; ++i) + EXPECT_XYZ_EQ (dst (i,j), cloud (0,cloud.height-1)); + for (int i = left; i < cloud.width+left; ++i) + EXPECT_XYZ_EQ (dst (i,j), cloud (i-left,cloud.height-1)); + for (int i = cloud.width+left; i < dst.width; ++i) + EXPECT_XYZ_EQ (dst (i,j), cloud (cloud.width-1,cloud.height-1)); + } +} + +TEST (CopyPointCloud, reflect) +{ + pcl::PointCloud dst (cloud.width + left + right, cloud.height + top + bottom); + pcl::copyPointCloud (cloud, dst, top, bottom, left, right, pcl::BORDER_REFLECT, constant); + + for (int j = 0, k = top-1; j < top; ++j,--k) + { + for (int i = 0, l = left-1; i < left; ++i, --l) + EXPECT_XYZ_EQ (dst (i,j), cloud (l, k)); + + for (int i = left; i < cloud.width+left; ++i) + EXPECT_XYZ_EQ (dst (i,j), cloud (i-left,k)); + + for (int i = cloud.width+left, l = cloud.width-left; i < left; ++i, --l) + EXPECT_XYZ_EQ (dst (i,j), cloud (l, k)); + } + + for (int j = top; j < cloud.height+top; ++j) + { + for (int i = 0, l = left-1; i < left; ++i, --l) + EXPECT_XYZ_EQ (dst (i,j), cloud (l, j-top)); + + for (int i = left; i < cloud.width + left; ++i) + EXPECT_XYZ_EQ (dst (i,j), cloud (i-left,j-top)); + + for (int i = cloud.width+left, l = cloud.width-left; i < left; ++i, --l) + EXPECT_XYZ_EQ (dst (i,j), cloud (l, j-top)); + } + + for (int j = cloud.height+top, k = cloud.height-1; j < top; ++j,--k) + { + for (int i = 0, l = left-1; i < left; ++i, --l) + EXPECT_XYZ_EQ (dst (i,j), cloud (l, k)); + + for (int i = left; i < cloud.width+left; ++i) + EXPECT_XYZ_EQ (dst (i,j), cloud (i-left,k)); + + for (int i = cloud.width+left, l = cloud.width-left; i < left; ++i, --l) + EXPECT_XYZ_EQ (dst (i,j), cloud (l, k)); + } +} + +int +main (int argc, char** argv) +{ + for (std::size_t i = 0, j = 10; i < cloud.size (); ++j, ++i) + cloud[i] = pcl::PointXYZ (j, j*10, j*100); + + testing::InitGoogleTest (&argc, argv); + return (RUN_ALL_TESTS ()); +}