diff --git a/common/include/pcl/common/common.h b/common/include/pcl/common/common.h index ef61328c29d..0cd19603a72 100644 --- a/common/include/pcl/common/common.h +++ b/common/include/pcl/common/common.h @@ -201,6 +201,18 @@ namespace pcl PCL_EXPORTS void getMeanStdDev (const std::vector &values, double &mean, double &stddev); + /** \brief Compute the axis aligned bounding box. + * \param[in] cloud the point cloud + * \param[out] center the resultant center of bounding box. + * \param[out] size the resultant size of bounding box. + * \return the resultant axis aligned boundingbox + * \ingroup common + */ + template void + getAxisAlignedBoundingBox (const pcl::PointCloud &cloud, + Eigen::Vector4f& center, + Eigen::Vector4f& size); + } /*@}*/ #include diff --git a/common/include/pcl/common/impl/common.hpp b/common/include/pcl/common/impl/common.hpp index 520f20008e5..68cc534e902 100644 --- a/common/include/pcl/common/impl/common.hpp +++ b/common/include/pcl/common/impl/common.hpp @@ -423,5 +423,16 @@ pcl::calculatePolygonArea (const pcl::PointCloud &polygon) return (area*0.5); } +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::getAxisAlignedBoundingBox (const pcl::PointCloud &cloud, + Eigen::Vector4f& center, + Eigen::Vector4f& size) +{ + Eigen::Vector4f min, max; + getMinMax3D (cloud, min, max); + size = max - min; + center = .5f * (max + min); +} #endif //#ifndef PCL_COMMON_IMPL_H_ diff --git a/doc/tutorials/content/good_estimation.rst b/doc/tutorials/content/good_estimation.rst new file mode 100644 index 00000000000..cb2aa50e0cc --- /dev/null +++ b/doc/tutorials/content/good_estimation.rst @@ -0,0 +1,271 @@ +:ref:`features_tutorial` + +.. _good_estimation: + +GOOD: A Global Orthographic Object Descriptor for 3D Object Recognition and Manipulation +------------------------------------------------------------------------------------------- + +The Global Orthographic Object Descriptor (**GOOD**) has been designed to be robust, descriptive and efficient to compute and use. GOOD descriptor has two outstanding characteristics: + + (1) Providing a good trade-off among: + + * **descriptiveness**, + * **robustness**, + * **computation time**, + * **memory usage**. + + (2) Allowing concurrent object recognition and pose estimation for manipulation. + +The performance of the proposed object descriptor is compared with the main state-of-the-art descriptors. Experimental results show that the overall classification performance obtained with GOOD is comparable to the best performances obtained with the state-of-the-art descriptors. Concerning memory and computation time, GOOD clearly outperforms the other descriptors (For more details please have a look at `our_paper +`_ ). Therefore, GOOD is especially suited for real-time applications. The current implementation of GOOD descriptor supports several functionalities for **3D Object Recognition** and **Object Manipulation**. + +To show all the functionalities and properties of the GOOD descriptor, a demonstration in real time was performed. +A video of this demonstration is available at: https://youtu.be/iEq9TAaY9u8 + +.. raw:: html + + + +Theoretical Primer +-------------------------- +Object representation is one of the most challenging tasks in robotics because it must provide reliable information in real-time to enable the robot to physically interact with the objects in its environment. To ensure robustness, a global object descriptor must be computed based on a unique and repeatable object reference frame. Moreover, the descriptor should contain enough information enabling to recognize the same or similar objects seen from different perspectives. + +This work presents a new object descriptor named Global Orthographic Object Descriptor (**GOOD**) designed to be robust, descriptive and efficient to compute and use. It consists of the following steps: + + * We propose a novel sign disambiguation method, for computing a unique reference frame from the eigenvectors obtained through Principal Component Analysis of the point cloud of the target object view captured by a 3D sensor. + + * Three principal orthographic projections and their distribution matrices are computed by exploiting the object reference frame. + + * The descriptor is finally obtained by concatenating the distribution matrices in a sequence determined by entropy and variance features of the projections. + +The following figure illustrates an example of the producing a GOOD shape description for a mug object: + + +.. image:: images/good_descriptor.jpg + :align: center + +**An illustrative example of the producing a GOOD shape description for a mug object (number of bin = 5):** (**a**) The mug object and its bounding box, reference frame, and three projected views; the object's points are then projected onto three planes; therefore, XoZ (**b**), YoZ (**c**) and XoY projections (**d**) are created. Each plane is partitioned +into bins and the number of points falling into each bin is counted. Accordingly, three distribution matrices are obtained for the projections; afterward, each distribution matrix is converted to a distribution vector, (i.e. (**e**), (**f**) and (**g**)) and two statistic features including entropy and variance are then calculated for each distribution vector; +(**h**) the distribution vectors are consequently concatenated together using the statistics features, to form a single description for the given object. The ordering of the three distribution vectors is first by decreasing values of entropy. Afterward, the second and third vectors are sorted again by increasing values of variance. + +Estimating GOOD features +-------------------------- +The GOOD descriptor is implemented in PCL as part of the `pcl_features +`_ library. +This tutorial exemplifies how to use the GOOD description. First of all, you will need a point cloud in PCD or PLY format. For your convenience, we have provided a sample point cloud in both PCD format in the following path "**../pcl/test/milk.pcd**" + +The code +-------- + +It should be noted that the corresponding code is provided as an example in the PCL repository (i.e. ../pcl/examples/features/example_good_descriptor.cpp) and will be compiled if examples are enabled during compilation. + +.. code-block:: cpp + :linenos: + #include + #include + + typedef pcl::PointXYZRGBA PointT; + typedef pcl::PointCloud::Ptr PointCloudInPtr; + + int main (int argc, char* argv[]) + { + if (argc != 2) + { + std::cerr << "\n Syntax is: example_good_descriptor " << std::endl; + return 0; + } + + std::string object_path = argv[1]; + pcl::PointCloud::Ptr object (new pcl::PointCloud); + if (pcl::io::load (object_path, *object) == -1) + { + std::cerr << "\n Failed to parse the file provided. Syntax is: example_good_descriptor or example_good_descriptor " << std::endl; + return -1; + } + + /*____________________________ + | | + | Setup the GOOD descriptor | + |_____________________________| */ + + const int NUMBER_OF_BINS = 15; + typedef pcl::GOODEstimation::Descriptor Descriptor; + pcl::PointCloud object_description; + pcl::GOODEstimation test_GOOD_descriptor; + test_GOOD_descriptor.setThreshold (0.0015); + + ///NOTE: GOOD descriptor can be setup in a line: pcl::GOODEstimation test_GOOD_descriptor (0.0015); + test_GOOD_descriptor.setInputCloud (object); // pass original point cloud + test_GOOD_descriptor.compute (object_description); // Actually compute the GOOD discriptor for the given object + + ///Printing GOOD_descriptor for the given point cloud, + ///NOTE: the descriptor is only the first point. + std::cout << "\n GOOD = " << object_description.points[0] << std::endl; + + /*__________________________________________________ + | | + | Usefull Functionalities for Object Manipulation | + |___________________________________________________| */ + + ///NOTE: The following functionalities of the GOOD descriptor are useful for manipulation tasks: + + /// Get objec point cloud in local reference frame + PointCloudInPtr transformed_object = test_GOOD_descriptor.getTransformedObject (); + + /// Get three orthographic projects and transformation matrix + std::vector vector_of_projected_views = test_GOOD_descriptor.getOrthographicProjections (); + Eigen::Matrix4f transformation = test_GOOD_descriptor.getTransformationMatrix (); + std::cout << "\n transofrmation matrix = \n" << transformation << std::endl; + + /// Get object bounding box information + pcl::PointXYZ center_of_bounding_box = test_GOOD_descriptor.getCenterOfObjectBoundingBox (); + Eigen::Vector3f bounding_box_dimensions = test_GOOD_descriptor.getObjectBoundingBoxDimensions (); + std::cout<<"\n center_of_bounding_box = " << center_of_bounding_box << std::endl; + std::cout<<"\n bounding_box_dimensions = " << bounding_box_dimensions << std::endl; + + /// Get the order of projection views programatically + const char *plane_name = test_GOOD_descriptor.getNameOfNthProjectedPlane (0); // input param can be {0, 1, 2} + std::cout << "\n the first projection view is " << plane_name << std::endl; + + return 0; + } + +The explanation +--------------- +Now let's study out what is the purpose of this code. + +The following lines are simply checking and loading the cloud from the .pcd or .ply file. + +.. code-block:: cpp + :linenos: + if (argc != 2) + { + std::cerr << "\n Syntax is: example_good_descriptor " << std::endl; + return 0; + } + + std::string object_path = argv[1]; + pcl::PointCloud::Ptr object (new pcl::PointCloud); + if (pcl::io::load (object_path, *object) == -1) + { + std::cerr << "\n Failed to parse the file provided. Syntax is: example_good_descriptor or example_good_descriptor " << std::endl; + return -1; + } + if (argc != 2) + { + std::cerr << "\n Syntax is: example_good_descriptor " << std::endl; + return 0; + } + + std::string object_path = argv[1]; + pcl::PointCloud::Ptr object (new pcl::PointCloud); + if (pcl::io::load (object_path, *object) == -1) + { + std::cerr << "\n Failed to parse the file provided. Syntax is: example_good_descriptor or example_good_descriptor " << std::endl; + return -1; + } + +The following code will set up the GOOD descriptor; GOOD descriptor has two important parameters including: + + * **NUMBER_OF_BINS:** each projection plane is divided into NUMBER_OF_BINS * NUMBER_OF_BINS square bins. Therefore the size of GOOD descriptor will be 3 * NUMBER_OF_BINS * NUMBER_OF_BINS. + * **Threshold:** this parameter is used in constructing local reference frame, especially in the sign disambiguation procedure. + + +.. code-block:: cpp + :linenos: + const int NUMBER_OF_BINS = 15; + typedef pcl::GOODEstimation::Descriptor Descriptor; + pcl::PointCloud object_description; + pcl::GOODEstimation test_GOOD_descriptor; + test_GOOD_descriptor.setThreshold (0.0015); + +Alternatively, the GOOD descriptor can be set up in a line: + +.. code-block:: cpp + :linenos: + pcl::GOODEstimation test_GOOD_descriptor (0.0015); + +Finally, we pass the input point cloud and compute the GOOD descriptor for the given point cloud. + +.. code-block:: cpp + :linenos: + test_GOOD_descriptor.setInputCloud(object); + test_GOOD_descriptor.compute(object_description); + +GOOD also provides a set of functionalities that are useful for manipulation tasks. They are including: + + * Get objec point cloud in local reference frame + * Get three orthographic projects and transformation matrix + * Get the order of the three projected planes + +To get more information about how to use the GOOD descriptor for manipulation purposes, please have a look at `our_paper `_ + +.. code-block:: cpp + :linenos: + /// Get objec point cloud in local reference frame + PointCloudInPtr transformed_object = test_GOOD_descriptor.getTransformedObject (); + + /// Get three orthographic projects and transformation matrix + std::vector vector_of_projected_views = test_GOOD_descriptor.getOrthographicProjections (); + Eigen::Matrix4f transformation = test_GOOD_descriptor.getTransformationMatrix (); + std::cout << "\n transofrmation matrix = \n" << transformation << std::endl; + + /// Get object bounding box information + pcl::PointXYZ center_of_bounding_box = test_GOOD_descriptor.getCenterOfObjectBoundingBox (); + Eigen::Vector3f bounding_box_dimensions = test_GOOD_descriptor.getObjectBoundingBoxDimensions (); + std::cout<<"\n center_of_bounding_box = " << center_of_bounding_box << std::endl; + std::cout<<"\n bounding_box_dimensions = " << bounding_box_dimensions << std::endl; + + /// Get the order of projection views programatically + const char *plane_name = test_GOOD_descriptor.getNameOfNthProjectedPlane (0); + std::cout << "\n the first plane is " << plane_name << std::endl; + +Output and Visualization +--------------------------------- +We are almost at the end of this tutorial. +The syntax for running the sample code is : + +.. code-block:: bash + ./pcl/build/bin/pcl_example_good_descriptor ../test/milk.pcd + + +.. image:: images/output_good.jpg + :align: center + + +CITING +------- + +The GOOD descriptor has been presented in the following papers. Please adequately refer to the papers any time this code is being used. If you do publish a paper where GOOD descriptor helped your research, we encourage you to cite the following papers in your publications. + +.. code-block:: bash + + @article{GOODPRL, + title = "GOOD: A global orthographic object descriptor for 3D object recognition and manipulation", + journal = "Pattern Recognition Letters", + volume = "83", + pages = "312 - 320", + year = "2016", + note = "Efficient Shape Representation, Matching, Ranking, and its Applications", + issn = "0167-8655", + doi = "http://dx.doi.org/10.1016/j.patrec.2016.07.006", + url = "http://www.sciencedirect.com/science/article/pii/S0167865516301684", + author = "S. Hamidreza Kasaei and Ana Maria Tomé and Luís Seabra Lopes and Miguel Oliveira",} + + @INPROCEEDINGS{GOODIROS, + author={S. H. Kasaei and L. Seabra Lopes and A. M. Tomé and M. Oliveira}, + booktitle={2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, + title={An orthographic descriptor for 3D object learning and recognition}, + year={2016}, + pages={4158-4163}, + doi={10.1109/IROS.2016.7759612}, + url="http://ieeexplore.ieee.org/document/7759612/" + month={Oct},} + + @inproceedings{GOODObjectManipulation, + title={Object learning and grasping capabilities for robotic home assistants}, + author={Kasaei, S Hamidreza and Shafii, Nima and Lopes, Lu{\'\i}s Seabra and Tom{\'e}, Ana Maria}, + booktitle={Robot World Cup}, + pages={279--293}, + year={2016}, + organization={Springer}} diff --git a/doc/tutorials/content/images/good_descriptor.jpg b/doc/tutorials/content/images/good_descriptor.jpg new file mode 100644 index 00000000000..1cef051b54c Binary files /dev/null and b/doc/tutorials/content/images/good_descriptor.jpg differ diff --git a/doc/tutorials/content/images/output_good.jpg b/doc/tutorials/content/images/output_good.jpg new file mode 100644 index 00000000000..195dc26e597 Binary files /dev/null and b/doc/tutorials/content/images/output_good.jpg differ diff --git a/examples/features/CMakeLists.txt b/examples/features/CMakeLists.txt index 7b1c895940e..14e0d19fd21 100644 --- a/examples/features/CMakeLists.txt +++ b/examples/features/CMakeLists.txt @@ -21,3 +21,6 @@ PCL_ADD_EXAMPLE(pcl_example_rift_estimation FILES example_rift_estimation.cpp PCL_ADD_EXAMPLE(pcl_example_difference_of_normals FILES example_difference_of_normals.cpp LINK_WITH pcl_common pcl_kdtree pcl_search pcl_features pcl_io pcl_segmentation pcl_sample_consensus) + +PCL_ADD_EXAMPLE(pcl_example_good_descriptor FILES example_good_descriptor.cpp + LINK_WITH pcl_common pcl_features pcl_io pcl_sample_consensus) diff --git a/examples/features/example_good_descriptor.cpp b/examples/features/example_good_descriptor.cpp new file mode 100644 index 00000000000..30e012f5d12 --- /dev/null +++ b/examples/features/example_good_descriptor.cpp @@ -0,0 +1,147 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2017-, 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. + * + */ + +/** \brief GOOD: a Global Orthographic Object Descriptor for 3D object recognition and manipulation. + * GOOD descriptor has been designed to be robust, descriptive and efficient to compute and use. + * It has two outstanding characteristics: + * + * (1) Providing a good trade-off among : + * - descriptiveness, + * - robustness, + * - computation time, + * - memory usage. + * + * (2) Allowing concurrent object recognition and pose estimation for manipulation. + * + * \note This is an implementation of the GOOD descriptor which has been presented in the following papers: + * + * [1] Kasaei, S. Hamidreza, Ana Maria Tome, Luis Seabra Lopes, Miguel Oliveira + * "GOOD: A global orthographic object descriptor for 3D object recognition and manipulation." + * Pattern Recognition Letters 83 (2016): 312-320.http://dx.doi.org/10.1016/j.patrec.2016.07.006 + * + * [2] Kasaei, S. Hamidreza, Luis Seabra Lopes, Ana Maria Tome, Miguel Oliveira + * "An orthographic descriptor for 3D object learning and recognition." + * 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Daejeon, 2016, + * pp. 4158-4163. doi: 10.1109/IROS.2016.7759612 + * + * Please adequately refer to this work any time this code is being used by citing above papers. + * If you do publish a paper where GOOD descriptor helped your research, we encourage you to cite the above papers in your publications. + * + * \author Hamidreza Kasaei (Seyed.Hamidreza[at]ua[dot]pt) + */ + +#include +#include + +typedef pcl::PointXYZRGBA PointT; +typedef pcl::PointCloud::Ptr PointCloudInPtr; + +int main (int argc, char* argv[]) +{ + if (argc != 2) + { + std::cerr << "\n Syntax is: example_good_descriptor " << std::endl; + return 0; + } + + std::string object_path = argv[1]; + pcl::PointCloud::Ptr object (new pcl::PointCloud); + if (pcl::io::load (object_path, *object) == -1) + { + std::cerr << "\n Failed to parse the file provided. Syntax is: example_good_descriptor or example_good_descriptor " << std::endl; + return -1; + } + + /*____________________________ + | | + | Setup the GOOD descriptor | + |_____________________________| */ + + const int NUMBER_OF_BINS = 15; + typedef pcl::GOODEstimation::Descriptor Descriptor; + pcl::PointCloud object_description; + pcl::GOODEstimation test_GOOD_descriptor; + test_GOOD_descriptor.setThreshold (0.0015); + ///NOTE: GOOD descriptor can be setup in a line: pcl::GOODEstimation test_GOOD_descriptor (0.0015); + test_GOOD_descriptor.setInputCloud (object); // pass original point cloud + test_GOOD_descriptor.compute (object_description); // Actually compute the GOOD discriptor for the given object + + ///Printing GOOD_descriptor for the given point cloud, + ///NOTE: the descriptor is only the first point. + std::cout << "\n GOOD = " << object_description[0] << std::endl; + + /*__________________________________________________ + | | + | Usefull Functionalities for Object Manipulation | + |___________________________________________________| */ + + ///NOTE: The following functionalities of the GOOD descriptor are useful for manipulation tasks: + + /// Get objec point cloud in local reference frame + PointCloudInPtr transformed_object = test_GOOD_descriptor.getTransformedObject (); + + /// Get three orthographic projects and transformation matrix + std::vector vector_of_projected_views = test_GOOD_descriptor.getOrthographicProjections (); + Eigen::Matrix4f transformation = test_GOOD_descriptor.getTransformationMatrix (); + std::cout << "\n transofrmation matrix = \n" << transformation << std::endl; + + /// Get object bounding box information + pcl::PointXYZ center_of_bounding_box = test_GOOD_descriptor.getCenterOfObjectBoundingBox (); + Eigen::Vector4f bounding_box_dimensions = test_GOOD_descriptor.getObjectBoundingBoxDimensions (); + std::cout<<"\n center_of_bounding_box = " << center_of_bounding_box << std::endl; + std::cout<<"\n bounding_box_dimensions = " << bounding_box_dimensions << std::endl; + + /// Get the order of projection views programatically + const char *plane_name = test_GOOD_descriptor.getNameOfNthProjectedPlane (0); // input param can be {0, 1, 2} + std::cout << "\n the first projection view is " << plane_name << std::endl; + + /*_________________________________________ + | | + | Visualizing orthographic projections | + |__________________________________________| */ + + ///NOTE: Pass the following point cloud to a pcl::visualization::PCLVisualizer for Visualizing the + ///transformed object, local reference fram and three orthographic projections + + //pcl::PointCloud::Ptr transformed_object_and_projected_views (new pcl::PointCloud); + //*transformed_object_and_projected_views += *vector_of_projected_views.at(0); + //*transformed_object_and_projected_views += *vector_of_projected_views.at(1); + //*transformed_object_and_projected_views += *vector_of_projected_views.at(2); + //*transformed_object_and_projected_views += *transformed_object; + + return 0; +} diff --git a/features/CMakeLists.txt b/features/CMakeLists.txt index ad5d60bbd7e..1e8feefcaf6 100644 --- a/features/CMakeLists.txt +++ b/features/CMakeLists.txt @@ -1,6 +1,6 @@ set(SUBSYS_NAME features) set(SUBSYS_DESC "Point cloud features library") -set(SUBSYS_DEPS common search kdtree octree filters 2d) +set(SUBSYS_DEPS common sample_consensus search kdtree octree filters 2d) set(build TRUE) PCL_SUBSYS_OPTION(build "${SUBSYS_NAME}" "${SUBSYS_DESC}" ON) @@ -62,6 +62,7 @@ if(build) "include/pcl/${SUBSYS_NAME}/usc.h" "include/pcl/${SUBSYS_NAME}/boundary.h" "include/pcl/${SUBSYS_NAME}/range_image_border_extractor.h" + "include/pcl/${SUBSYS_NAME}/good.h" ) set(impl_incs @@ -110,6 +111,7 @@ if(build) "include/pcl/${SUBSYS_NAME}/impl/usc.hpp" "include/pcl/${SUBSYS_NAME}/impl/boundary.hpp" "include/pcl/${SUBSYS_NAME}/impl/range_image_border_extractor.hpp" + "include/pcl/${SUBSYS_NAME}/impl/good.hpp" ) set(srcs @@ -150,6 +152,7 @@ if(build) src/3dsc.cpp src/usc.cpp src/range_image_border_extractor.cpp + src/good.cpp ) if(MSVC) diff --git a/features/include/pcl/features/good.h b/features/include/pcl/features/good.h new file mode 100644 index 00000000000..f0eef725e36 --- /dev/null +++ b/features/include/pcl/features/good.h @@ -0,0 +1,283 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2017-, 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_FEATURES_GOOD_H_ +#define PCL_FEATURES_GOOD_H_ + +#include +#include +#include + + +namespace pcl +{ +/** \brief GOOD: a Global Orthographic Object Descriptor for 3D object recognition and manipulation. + * GOOD descriptor has been designed to be robust, descriptive and efficient to compute and use. + * It has two outstanding characteristics: + * + * (1) Providing a good trade-off among : + * - descriptiveness, + * - robustness, + * - computation time, + * - memory usage. + * + * (2) Allowing concurrent object recognition and pose estimation for manipulation. + * + * \note This is an implementation of the GOOD descriptor which has been presented in the following papers: + * + * [1] Kasaei, S. Hamidreza, Ana Maria Tome, Luis Seabra Lopes, Miguel Oliveira + * "GOOD: A global orthographic object descriptor for 3D object recognition and manipulation." + * Pattern Recognition Letters 83 (2016): 312-320.http://dx.doi.org/10.1016/j.patrec.2016.07.006 + * + * [2] Kasaei, S. Hamidreza, Luis Seabra Lopes, Ana Maria Tome, Miguel Oliveira + * "An orthographic descriptor for 3D object learning and recognition." + * 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Daejeon, 2016, + * pp. 4158-4163. doi: 10.1109/IROS.2016.7759612 + * + * Please adequately refer to this work any time this code is being used by citing above papers. + * If you do publish a paper where GOOD descriptor helped your research, we encourage you to cite the above papers in your publications. + * + * \author Hamidreza Kasaei (Seyed.Hamidreza[at]ua[dot]pt Kasaei.Hamidreza[at]gmail[dot]com ) + */ + + template + class PCL_EXPORTS GOODEstimation : public Feature > + { + public: + typedef pcl::Histogram<3*BinN*BinN> Descriptor; + using typename Feature::PointCloudOut; + using typename Feature::PointCloudIn; + using typename Feature::PointCloudInPtr; + using typename Feature::PointCloudInConstPtr; + using Feature::feature_name_; + using Feature::k_; + using PCLBase::input_; + + /** \brief Constructor. + * \param[in] threshold_ threshold parameter is used in constructing local reference frame + */ + GOODEstimation (const float threshold = 0.0015) + : threshold_ (threshold) + { + feature_name_ = "GOODEstimation"; + k_ = 1; + }; + + /** \brief Returns the number_of_bins_ parameter. */ + inline static int + getNumberOfBins () { return BinN; } + + /** \brief Set the threshold param which is used for local reference frame construction + * \param[in] threshold threshold_ parameter + */ + inline void + setThreshold (const float threshold) + { + threshold_ = threshold; + } + + /** \brief Returns the threshold_ parameter. */ + inline float + getThreshold () const { return threshold_; } + + /** \brief get three orthographic projections of a set of points given by setInputCloud() + * \return the resultant vector of point clouds that contains three orthographic projections of the query point cloud + */ + inline const std::vector& + getOrthographicProjections () const { return vector_of_projected_views_; } + + /** \brief get objec point cloud in local reference frame constructed by the GOOD descriptor + * \return the resultant point cloud of the object in local reference frame + */ + inline const PointCloudInPtr& + getTransformedObject () const { return transformed_point_cloud_; } + + /** \brief get center of boundingbox of a set of points given by setInputCloud() in camera reference frame + * \return the resultant center of boundingbox + */ + inline const pcl::PointXYZ& + getCenterOfObjectBoundingBox () const { return center_of_bbox_; } + + /** \brief get dimensions of bounding box of a set of points given by setInputCloud() + * \return the resultant boundingbox dimensions + */ + inline const Eigen::Vector4f& + getObjectBoundingBoxDimensions () const { return bbox_dimensions_; } + + /** \brief get the transformation matrix from camera reference frame to object local reference frame + * \return the resultant transformation matrix + */ + inline const Eigen::Matrix4f& + getTransformationMatrix () const {return transformation_;} + + /** \brief get the index of Nth protection plan in constructed GOOD descriptor; Index can be [0 - 2] which is related to {"YoZ", "XoZ", "XoY"} + * \return the resultant index [0 - 2] which is related to {"YoZ", "XoZ", "XoY"} + */ + inline const char* + getNameOfNthProjectedPlane (size_t n) const + { + const char *plane_name [3] = {"YoZ", "XoZ", "XoY"}; + return (plane_name [order_of_projected_plane_ [n]]); + } + + protected: + + /** \brief get the order of projection views programatically */ + enum Projection + { + YoZ, + XoZ, + XoY + }; + Projection order_of_projected_plane_[3]; + + /** \brief estimate the GOOD descriptor at a set of points given by setInputCloud() + * \param[out] output the resultant GOOD descriptor representing the feature at the query point cloud + */ + virtual void + computeFeature (PointCloudOut &output); + + private: + + /** \brief use the axis programatically */ + enum Axis { X, Y, Z}; + + /** \brief threshold parameter is used in constructing local reference frame. + * By default, the threshold_ is set to 0.0015. + */ + float threshold_; + + /** \brief resultant of sign disambiguation can be either 1 or -1 */ + int8_t sign_; + + /** \brief transformed point cloud in LRF */ + PointCloudInPtr transformed_point_cloud_; + + /** \brief get transformation matrix */ + Eigen::Matrix4f transformation_; + + /** \brief dimensions of boundingboxbox of given point cloud */ + Eigen::Vector4f bbox_dimensions_; + + /** \brief center of boundingboxbox of given point cloud */ + pcl::PointXYZ center_of_bbox_; + + /** \brief vector of three point clouds containing orthographic projection views */ + std::vector vector_of_projected_views_; + + /** \brief project point cloud to a plane + * \param[in] pc_in pointer to a point cloud. + * \param[in] coefficients pcl::ModelCoefficients + * \param[out] pc_out the resultant projected point cloud + */ + static void + projectPointCloudToPlane (const PointCloudInConstPtr &pc_in, const pcl::ModelCoefficients::Ptr &coefficients, PointCloudIn &pc_out); + + /** \brief sign disambiguation + * \param[in] projected_view pointer to a point cloud. + * \param[in] threshold used to deal with the special case when a point is close to the other planes + * \return the resultant sign (either 1 or -1) + */ + int8_t + signDisambiguation (const PointCloudIn &projected_view, const int8_t axis) const; + + /** \brief create a 2D histogram from a projection + * \param[in] projected_view pointer to a point cloud. + * \param[in] largest_side largest side of object bounding box. + * \param[in] number_of_bins number of bins along one dimension. + * \param[in] sign either 1 or -1. + * \return histogram i.e. eigen array (float). + */ + Eigen::Array + createNormalizedHistogramFromProjection (const PointCloudInPtr &projected_view, + const float largest_side, + const int8_t axis_a, + const int8_t axis_b) const; + + /** \brief compute viewpoint entropy used for concatenating projections + * \param[in] normalized_histogram normalized_histogram (float). + * \return the resultant view entropy. + */ + float + viewpointEntropy (const Eigen::Array &normalized_histogram); + + /** \brief find max view point entropy used for concatenating projections + * \param[in] view_point_entropy a vector of float contains three view entropies. + * \return the resultant projection index. + */ + size_t + findMaxViewPointEntropy (const std::vector &view_point_entropy); + + /** \brief compute variance of a given histogram + * \param[in] histogram normalized histogram of projection. + * \return variance the resultant variance. + */ + float + varianceOfHistogram (const Eigen::Array &histogram); + + /** \brief concatinating three orthographic projections + * three histograms are obtained for the projections; afterwards, two statistic features including entropy and variance have been calculated for each distribution vector; + * the histograms are consequently concatenated together using entropy and variance features, to form a single description for the given object. The ordering of the three + * histograms is first by decreasing values of entropy. Afterwards the second and third vectors are sorted again by increasing values of variance. + * \param[in] maximum_entropy_index index of orthographic projection that has maximum entropy. + * \param[in] normalized_projected_views a vector of eigen array of float contains three normalized histogram of projected views. + * \return the resultant GOOD description (i.e. eigen array of float). + */ + Eigen::Array + objectViewHistogram (const size_t maximum_entropy_index, + const std::vector > &normalized_projected_views); + + /** \brief compute largest side of the computed boundingbox i.e. bbox_dimensions_ + * \return the resultant largest side. + * It should be noted that a small value (i.e. 0.02) is added to the output to deal with the special cases, + * when a point is projected onto the upper bound of the projection area + */ + inline float + computeLargestSideOfBoundingBox () const { return (bbox_dimensions_.maxCoeff() + 0.02) ;} + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + }; +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif /* PCL_FEATURES_GOOD_H_ */ diff --git a/features/include/pcl/features/impl/good.hpp b/features/include/pcl/features/impl/good.hpp new file mode 100644 index 00000000000..c7ab51b48c4 --- /dev/null +++ b/features/include/pcl/features/impl/good.hpp @@ -0,0 +1,368 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2017-, 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_FEATURES_IMPL_GOOD_H_ +#define PCL_FEATURES_IMPL_GOOD_H_ +#include +#include +#include + + +#include ///std::accumulate + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GOODEstimation::projectPointCloudToPlane (const PointCloudInConstPtr &pc_in, const pcl::ModelCoefficients::Ptr &coefficients, PointCloudIn &pc_out) +{ + ///Create the projection object + pcl::ProjectInliers projection; + projection.setModelType (pcl::SACMODEL_NORMAL_PLANE); + projection.setInputCloud (pc_in); + projection.setModelCoefficients (coefficients); + projection.filter (pc_out); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template int8_t +pcl::GOODEstimation::signDisambiguation ( const PointCloudIn &projected_view, const int8_t axis) const +{ + unsigned int positive = 0; + unsigned int negative = 0; + const unsigned int threshold_overall = std::max (1u, (unsigned int) trunc (projected_view.size ()/10)); + + for (size_t i = 0; i < projected_view.size (); ++i) + { + if (projected_view.at (i).data[axis] > threshold_) + ++positive; + else if (projected_view.at (i).data[axis] < -threshold_) + ++negative; + } + const int8_t sign = ((int) (negative - positive) >= (int) threshold_overall)? -1 : 1; + return (sign); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template Eigen::Array +pcl::GOODEstimation::createNormalizedHistogramFromProjection (const PointCloudInPtr &projected_view, + const float largest_side, + const int8_t axis_a, + const int8_t axis_b) const +{ + Eigen::Array histogram = Eigen::Array::Zero(); + const float half = .5f * largest_side; + const float interval = largest_side / (float) BinN; + + for (size_t i = 0; i < projected_view->size (); ++i) + { + const float a = ((axis_a == Y)? sign_ : 1) * projected_view->at (i).data[axis_a] + half; + const float b = sign_ * projected_view->at (i).data[axis_b] + half; + + const int idx_i = (int) trunc (a / interval); //inner index + const int idx_o = (int) trunc (b / interval); //outer index + if ((idx_o < BinN) + && (idx_i < BinN) + && (idx_o >= 0) + && (idx_i >= 0)) + histogram(idx_o , idx_i) += 1; + } + + histogram /= histogram.sum(); // normalizing histogram + Eigen::Map normalized_1D_histogram (histogram.data (), histogram.size ()); // convert 2D Histogram to 1D Histogram + return (normalized_1D_histogram); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::GOODEstimation::viewpointEntropy (const Eigen::Array &normalized_histogram) +{ + ///NOTE: http://stats.stackexchange.com/questions/66108/why-is-entropy-maximised-when-the-probability-distribution-is-uniform + float entropy = 0; + for (size_t i = 0; i < normalized_histogram.rows (); ++i) + { + if (normalized_histogram [i] > 0) + { + entropy += normalized_histogram [i] * log2 (normalized_histogram [i]); + } + } + return (-entropy); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template size_t +pcl::GOODEstimation::findMaxViewPointEntropy (const std::vector &view_point_entropy) +{ + const std::vector::const_iterator it = std::max_element (view_point_entropy.begin (), view_point_entropy.end ()); + return (it - view_point_entropy.begin ()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::GOODEstimation::varianceOfHistogram (const Eigen::Array &histogram) +{ + ///NOTE: https://people.richland.edu/james/lecture/m170/ch06-prb.html + ///NOTE: http://www.stat.yale.edu/Courses/1997-98/101/rvmnvar.htm + float mean = histogram.mean (); + float variance = 0; + for (size_t i = 0; i < histogram.rows (); i++) + { + const float tmp = (i+1) - mean; + variance += tmp * tmp * histogram [i]; + } + return (variance); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template Eigen::Array +pcl::GOODEstimation::objectViewHistogram (const size_t maximum_entropy_index, + const std::vector > /*std::vector >*/ &normalized_projected_views) +{ + float variance1 = 0; + float variance2 = 0; + Eigen::Array sorted_normalized_projected_views = Eigen::Array::Zero(); + order_of_projected_plane_[0] = (Projection) maximum_entropy_index; + + switch (maximum_entropy_index) + { + case 0 : + variance1 = varianceOfHistogram (normalized_projected_views.at (1)); + variance2 = varianceOfHistogram (normalized_projected_views.at (2)); + + if (variance1 <= variance2) + { + order_of_projected_plane_[1] = XoZ; + order_of_projected_plane_[2] = XoY; + sorted_normalized_projected_views << normalized_projected_views.at (maximum_entropy_index), normalized_projected_views.at (1), normalized_projected_views.at (2); + } + else + { + order_of_projected_plane_[1] = XoY; + order_of_projected_plane_[2] = XoZ; + sorted_normalized_projected_views << normalized_projected_views.at (maximum_entropy_index), normalized_projected_views.at (2), normalized_projected_views.at (1); + } + break; + + case 1 : + variance1 = varianceOfHistogram (normalized_projected_views.at (0)); + variance2 = varianceOfHistogram (normalized_projected_views.at (2)); + + if (variance1 <= variance2) + { + order_of_projected_plane_[1] = YoZ; + order_of_projected_plane_[2] = XoY; + sorted_normalized_projected_views << normalized_projected_views.at (maximum_entropy_index), normalized_projected_views.at (0), normalized_projected_views.at (2); + } + else + { + order_of_projected_plane_[1] = XoY; + order_of_projected_plane_[2] = YoZ; + sorted_normalized_projected_views << normalized_projected_views.at (maximum_entropy_index), normalized_projected_views.at (2), normalized_projected_views.at (0); + } + break; + + case 2 : + variance1 = varianceOfHistogram (normalized_projected_views.at (0)); + variance2 = varianceOfHistogram (normalized_projected_views.at (1)); + + if (variance1 <= variance2) + { + order_of_projected_plane_[1] = YoZ; + order_of_projected_plane_[2] = XoZ; + sorted_normalized_projected_views << normalized_projected_views.at (maximum_entropy_index), normalized_projected_views.at (0), normalized_projected_views.at (1); + } + else + { + order_of_projected_plane_[1] = XoZ; + order_of_projected_plane_[2] = YoZ; + sorted_normalized_projected_views << normalized_projected_views.at (maximum_entropy_index), normalized_projected_views.at (1), normalized_projected_views.at (0); + } + break; + + default: + break; + } + return (sorted_normalized_projected_views); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::GOODEstimation::computeFeature (PointCloudOut &output ) +{ + PointCloudInPtr initial_cloud_projection_along_x_axis (new pcl::PointCloud);//Declare a boost share ptr to the pointCloud + PointCloudInPtr initial_cloud_projection_along_y_axis (new pcl::PointCloud);//Declare a boost share ptr to the pointCloud + PointCloudInPtr initial_cloud_projection_along_z_axis (new pcl::PointCloud);//Declare a boost share ptr to the pointCloud + pcl::PointXYZ pt; + + /* __________________________ + | | + | construct ORF based on PCA | + |____________________________| */ + + /// compute principal directions + Eigen::Vector4f centroid; + pcl::compute3DCentroid (*input_, centroid); + Eigen::Matrix3f covariance; + computeCovarianceMatrixNormalized (*input_, centroid, covariance); + Eigen::SelfAdjointEigenSolver eigen_solver (covariance, Eigen::ComputeEigenvectors); + Eigen::Matrix3f eigen_vectors = eigen_solver.eigenvectors(); + eigen_vectors.col(2) = eigen_vectors.col(0).cross(eigen_vectors.col(1)); + + /// sorting eigen vectors based on eigen values + eigen_vectors.col(0) = eigen_vectors.col(2); + eigen_vectors.col(2) = eigen_vectors.col(0).cross (eigen_vectors.col(1)); + + /// move the points to the PCA based reference frame + Eigen::Matrix4f p2w (Eigen::Matrix4f::Identity()); + p2w.block<3,3>(0,0) = eigen_vectors.transpose(); + p2w.block<3,1>(0,3) = -1.f * (p2w.block<3,3>(0,0) * centroid.head<3>()); + transformation_ = p2w; + + PointCloudInPtr transformed_point_cloud (new pcl::PointCloud); + pcl::transformPointCloud (*input_, *transformed_point_cloud, p2w); + transformed_point_cloud_ = transformed_point_cloud; + ///compute the max, the min and the center of the diagonal (mean_diag) + PointInT min_pt, max_pt; + pcl::getMinMax3D (*transformed_point_cloud_, min_pt, max_pt); + const Eigen::Vector3f mean_diag = 0.5f * (max_pt.getVector3fMap() + min_pt.getVector3fMap()); + + /// centroid transform + Eigen::Quaternionf qfinal (eigen_vectors); ///rotation matrix + Eigen::Vector3f center_of_bbox = eigen_vectors*mean_diag + centroid.head<3>(); // Translation = Rotation * center_diag + (c0, c1, c2) + center_of_bbox_.x = center_of_bbox(0,0); + center_of_bbox_.y = center_of_bbox(1,0); + center_of_bbox_.z = center_of_bbox(2,0); + + /* _______________________________ + | | + | construct three projection view | + |_________________________________| */ + + const static float OFFSET_TO_THE_ORIGIN = 0.3; + + /// ax+by+cz+d=0, where b=c=d=0, and a=1, or said differently, the YoZ plane. + pcl::ModelCoefficients::Ptr coefficients_x (new pcl::ModelCoefficients ()); + coefficients_x->values.resize (4); + coefficients_x->values[0] = 1.0; coefficients_x->values[1] = 0; coefficients_x->values[2] = 0; coefficients_x->values[3] = 0; + projectPointCloudToPlane (transformed_point_cloud_, coefficients_x, *initial_cloud_projection_along_x_axis); + for (size_t i = 0; i < initial_cloud_projection_along_x_axis->points.size (); i++) + { + initial_cloud_projection_along_x_axis->points.at(i).x = OFFSET_TO_THE_ORIGIN; + } + vector_of_projected_views_.push_back (initial_cloud_projection_along_x_axis); + + ///ax+by+cz+d=0, where a=c=d=0, and b=1, or said differently, the XoZ plane. + pcl::ModelCoefficients::Ptr coefficients_y (new pcl::ModelCoefficients ()); + coefficients_y->values.resize (4); + coefficients_y->values[0] = 0.0; coefficients_y->values[1] = 1.0; coefficients_y->values[2] = 0; coefficients_y->values[3] = 0; + + projectPointCloudToPlane (transformed_point_cloud_, coefficients_y, *initial_cloud_projection_along_y_axis); + for (size_t i = 0; i < initial_cloud_projection_along_y_axis->points.size (); i++) + { + initial_cloud_projection_along_y_axis->points.at(i).y = OFFSET_TO_THE_ORIGIN; + } + vector_of_projected_views_.push_back (initial_cloud_projection_along_y_axis); + + /// ax+by+cz+d=0, where a=b=d=0, and c=1, or said differently, the XoY plane. + pcl::ModelCoefficients::Ptr coefficients_z (new pcl::ModelCoefficients ()); + coefficients_z->values.resize (4); + coefficients_z->values[0] = 0; coefficients_z->values[1] = 0; coefficients_z->values[2] = 1.0; coefficients_z->values[3] = 0; + projectPointCloudToPlane (transformed_point_cloud_, coefficients_z, *initial_cloud_projection_along_z_axis); + for (size_t i = 0; i < initial_cloud_projection_along_z_axis->points.size (); i++) + { + initial_cloud_projection_along_z_axis->points.at(i).z = OFFSET_TO_THE_ORIGIN; + } + vector_of_projected_views_.push_back (initial_cloud_projection_along_z_axis); + + /// compute BoundingBox Dimensions + Eigen::Vector4f center; + Eigen::Vector4f bbox_dimensions; + getAxisAlignedBoundingBox(*transformed_point_cloud_, center, bbox_dimensions); + bbox_dimensions_ = bbox_dimensions; + float largest_side = computeLargestSideOfBoundingBox (); + + /* _________________________ + | | + | Axes sign disambiguation | + |___________________________| */ + + int8_t sx = 1, sy = 1; + sx = signDisambiguation (*initial_cloud_projection_along_y_axis, X); ///XoZ Plane + sy = signDisambiguation (*initial_cloud_projection_along_x_axis, Y); ///YoZ Plane + sign_= sx * sy; + + /* _______________________________________________________ + | | + | compute histograms of projection of the given object | + |_________________________________________________________| */ + + std::vector > normalized_histograms_of_all_projected_views (3, Eigen::Array::Zero()); + Eigen::Array normalized_histogram_tmp = Eigen::Array::Zero(); + std::vector view_points_entropy (3); + + /// YOZ Projection + normalized_histogram_tmp = createNormalizedHistogramFromProjection (initial_cloud_projection_along_x_axis, largest_side, Y, Z); + normalized_histograms_of_all_projected_views.at (0) = normalized_histogram_tmp; + view_points_entropy.at (0) = viewpointEntropy (normalized_histogram_tmp); // YOZ_entropy + + /// XOZ Projection + normalized_histogram_tmp = createNormalizedHistogramFromProjection (initial_cloud_projection_along_y_axis, largest_side, X, Z); + normalized_histograms_of_all_projected_views.at (1) = normalized_histogram_tmp; + view_points_entropy.at (1) = viewpointEntropy (normalized_histogram_tmp); // XOZ_entropy + + /// XOY Projection + normalized_histogram_tmp = createNormalizedHistogramFromProjection (initial_cloud_projection_along_z_axis, largest_side, X, Y); + normalized_histograms_of_all_projected_views.at (2) = normalized_histogram_tmp; + view_points_entropy.at (2) = viewpointEntropy (normalized_histogram_tmp); // XOY_entropy + + /* ____________________________ + | | + | producing GOOD description | + |______________________________| */ + + ///NOTE: The ordering of the three distribution vectors is first by decreasing values of entropy. + ///Afterwards the second and third vectors are sorted again by increasing values of variance. + + Eigen::Array object_description = Eigen::Array::Zero(); + size_t maximum_entropy_index = findMaxViewPointEntropy (view_points_entropy); + object_description = objectViewHistogram (maximum_entropy_index, normalized_histograms_of_all_projected_views); + + for(size_t i =0; i < object_description.rows (); i++) + output.points[0].histogram[i] = object_description[i]; +} + +#define PCL_INSTANTIATE_GOODEstimation(T, x) template class PCL_EXPORTS pcl::GOODEstimation; + +#endif /* PCL_FEATURES_IMPL_GOOD_H_ */ diff --git a/features/src/good.cpp b/features/src/good.cpp new file mode 100644 index 00000000000..8609523788a --- /dev/null +++ b/features/src/good.cpp @@ -0,0 +1,49 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2017-, 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 + +#ifndef PCL_NO_PRECOMPILE +#include +#include +// Instantiations of specific point types +#ifdef PCL_ONLY_CORE_POINT_TYPES +PCL_INSTANTIATE_PRODUCT(GOODEstimation, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))((15))) +#else +PCL_INSTANTIATE_PRODUCT(GOODEstimation, (PCL_XYZ_POINT_TYPES)((15))) +#endif +#endif // PCL_NO_PRECOMPILE diff --git a/test/features/CMakeLists.txt b/test/features/CMakeLists.txt index afa18a5f1f0..ecf3ed1fd13 100644 --- a/test/features/CMakeLists.txt +++ b/test/features/CMakeLists.txt @@ -20,6 +20,10 @@ if (build) LINK_WITH pcl_gtest pcl_features) if (BUILD_io) + PCL_ADD_TEST(feature_good_estimation test_good_estimation + FILES test_good_estimation.cpp + LINK_WITH pcl_gtest pcl_features pcl_io + ARGUMENTS "${PCL_SOURCE_DIR}/test/milk.pcd") PCL_ADD_TEST(feature_base test_base_feature FILES test_base_feature.cpp LINK_WITH pcl_gtest pcl_features pcl_io diff --git a/test/features/test_good_estimation.cpp b/test/features/test_good_estimation.cpp new file mode 100644 index 00000000000..0f50aeec264 --- /dev/null +++ b/test/features/test_good_estimation.cpp @@ -0,0 +1,138 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2017-, 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 + + +pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + +///////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, GOODEstimation) +{ + const int NUMBER_OF_BINS = 15; + typedef pcl::GOODEstimation::Descriptor Descriptor; + pcl::PointCloud object_description; + pcl::GOODEstimation test_GOOD_descriptor; + test_GOOD_descriptor.setThreshold (0.0015); + test_GOOD_descriptor.setInputCloud (cloud); // pass original point cloud + test_GOOD_descriptor.compute (object_description); // Actually compute the GOOD discriptor for the given object + + const float expected_values [675] = + { 0, 0, 0, 0, 0, 0, 0, 0.000875657, 0, 0.000218914, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00627554, 0.00861062, 0.0105079, 0.0110187, 0.00634851, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.000364857, + 0.0140835, 0.0164915, 0.015324, 0.0158348, 0.0134267, 0.00218914, 0, 0, 0, 0, 0, 0, 0, 0.00131349, 0.00890251, 0.0104349, 0.0172942, 0.0170753, 0.0165645, 0.0156888, 0.010289, + 0.000218914, 0, 0, 0, 0, 0, 0, 0.00496206, 0.0106538, 0.0137186, 0.0138646, 0.0140105, 0.0137186, 0.0116754, 0.0124051, 0.00211617, 0, 0, 0, 0, 0, 0, 0.00488908, 0.00992411, + 0.0106538, 0.0116025, 0.0132078, 0.0121133, 0.0107998, 0.0111646, 0.000729714, 0, 0, 0, 0, 0, 0, 0.00372154, 0.00882954, 0.0106538, 0.0119673, 0.0119673, 0.0119673, 0.0108727, + 0.0110187, 0.00167834, 0, 0, 0, 0, 0, 0, 0.00182428, 0.00904845, 0.0100701, 0.0108727, 0.0114565, 0.0110917, 0.0100701, 0.00941331, 0.00248103, 0, 0, 0, 0, 0, 0, 0.00313777, + 0.00846468, 0.00934034, 0.0103619, 0.0106538, 0.0113106, 0.0104349, 0.010143, 0.00211617, 0, 0, 0, 0, 0, 0, 0.00445125, 0.0081728, 0.00890251, 0.010289, 0.0109457, 0.010143, + 0.00941331, 0.00846468, 0.00240806, 0, 0, 0, 0, 0, 0, 0.00328371, 0.00839171, 0.00875657, 0.00941331, 0.0105809, 0.00934034, 0.00977817, 0.00904845, 0.00109457, 0, 0, 0, 0, 0, 0, + 0.00291886, 0.00685931, 0.00846468, 0.00926737, 0.00955925, 0.00955925, 0.00926737, 0.00831874, 0.00226211, 0, 0, 0, 0, 0, 0, 0.00350263, 0.00744308, 0.00795388, 0.00882954, + 0.0091944, 0.00912142, 0.00890251, 0.00795388, 0.00160537, 0, 0, 0, 0, 0, 0, 0.00131349, 0.00685931, 0.00751605, 0.00861062, 0.00912142, 0.00861062, 0.00751605, 0.00853765, + 0.0020432, 0, 0, 0, 0, 0, 0, 0.00109457, 0.00226211, 0.00328371, 0.0056188, 0.00539988, 0.00437828, 0.00350263, 0.00321074, 0.000729714, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.000218914, 0.00649445, 0.0267075, 0.00299183, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0.00175131, 0.0290426, 0.0513719, 0.0140105, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0139375, 0.0768389, 0.0391127, 0.000218914, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.0110917, 0.0218914, 0.071293, 0.0488179, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00277291, 0.0197023, 0.0111646, 0.086617, 0.0396235, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.017951, + 0.00875657, 0.0480881, 0.0799766, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.000437828, 0.015324, 0.0262697, 0.0934034, 0.00248103, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00328371, 0.0820198, + 0.0268535, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00218914, 0.0168564, 0.000437828, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00109457, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.000145943, 0.0186807, 0.015397, 0.00853765, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.000291886, 0.0172942, 0.0289696, 0.0289696, 0.00218914, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00350263, 0.0259048, 0.033275, 0.0311588, 0.00394046, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00248103, 0.0155429, 0.0278751, 0.0296264, 0.0215995, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00467017, 0.0115295, 0.0233508, 0.0261967, 0.0193374, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0.0061296, 0.00948628, 0.0244454, 0.0248103, 0.017805, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00299183, 0.0154699, 0.0213076, 0.0231319, 0.0134267, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.00554583, 0.0163456, 0.0206509, 0.0247373, 0.0086836, 0, 0, 0, 0, 0, 0, 0, 0, 0, 7.29714e-05, 0.00788091, 0.0161997, 0.0188266, 0.0264886, 0.00372154, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0.000437828, 0.00656743, 0.0173672, 0.0184618, 0.0267075, 0.000145943, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.000875657, 0.00576474, 0.017878, 0.0189726, 0.022986, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0.00335668, 0.00554583, 0.0168564, 0.020286, 0.0184618, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.00211617, 0.00569177, 0.0188266, 0.0183888, 0.0151051, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0.000218914, 0.00138646, 0.00342966, 0.00722417, 0.00934034, 0.00788091, 0, 0, 0, 0, 0, 0 + }; + + EXPECT_EQ (object_description.points[0].descriptorSize (), 675); + for (size_t i = 0; i < size_t (object_description.points[0].descriptorSize ()); ++i) + { + EXPECT_NEAR (object_description.points[0].histogram[i], expected_values[i], 1e-5); + } + + Eigen::Matrix4f transformation = test_GOOD_descriptor.getTransformationMatrix (); + Eigen::Matrix4f expected_transformation_values; + expected_transformation_values << + -0.0108418, 0.887046, 0.461555, -0.236646, + 0.998495, -0.0152106, 0.0526872, 0.0132545, + 0.0537565, 0.461432, -0.885546, 0.75173, + 0, 0, 0, 1; + + for (int i = 0; i < transformation.rows(); ++i) + { + for (int j = 0; j < transformation.cols (); ++j) + { + EXPECT_NEAR (transformation (i, j), expected_transformation_values (i, j), 1e-5); + } + } + + pcl::PointXYZ center_of_bounding_box = test_GOOD_descriptor.getCenterOfObjectBoundingBox (); + EXPECT_NEAR (center_of_bounding_box.x, -0.0624728, 0.01); + EXPECT_NEAR (center_of_bounding_box.y, -0.140744, 0.01); + EXPECT_NEAR (center_of_bounding_box.z, 0.790559, 0.01); + + Eigen::Vector4f bounding_box_dimensions = test_GOOD_descriptor.getObjectBoundingBoxDimensions (); + EXPECT_NEAR (bounding_box_dimensions (0), 0.257679, 0.01); + EXPECT_NEAR (bounding_box_dimensions (1), 0.151612, 0.01); + EXPECT_NEAR (bounding_box_dimensions (2), 0.111211, 0.01); + EXPECT_NEAR (bounding_box_dimensions (3), 0, 0.01); + +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +/* ---[ */ +int +main (int argc, char** argv) +{ + if (argc < 2) + { + std::cerr << "No test file given. Please download `milk.pcd` and pass its path to the test." << std::endl; + return (-1); + } + + if (pcl::io::loadPCDFile (argv[1], *cloud) == -1) + { + std::cerr << "Failed to read test file. Please download `milk.pcd` and pass its path to the test." << std::endl; + return (-1); + } + + testing::InitGoogleTest (&argc, argv); + return (RUN_ALL_TESTS ()); +} +/* ]--- */