diff --git a/recognition/include/pcl/recognition/ransac_based/auxiliary.h b/recognition/include/pcl/recognition/ransac_based/auxiliary.h index 3934482399e..91e775c47ce 100644 --- a/recognition/include/pcl/recognition/ransac_based/auxiliary.h +++ b/recognition/include/pcl/recognition/ransac_based/auxiliary.h @@ -139,7 +139,14 @@ namespace pcl a[1] = -a[1]; a[2] = -a[2]; } - + + /** \brief a = b */ + template bool + equal3 (const T a[3], const T b[3]) + { + return (a[0] == b[0] && a[1] == b[1] && a[2] == b[2]); + } + /** \brief a += b */ template void add3 (T a[3], const T b[3]) diff --git a/recognition/src/ransac_based/orr_octree.cpp b/recognition/src/ransac_based/orr_octree.cpp index a9f0db480c6..d922bb2fbca 100644 --- a/recognition/src/ransac_based/orr_octree.cpp +++ b/recognition/src/ransac_based/orr_octree.cpp @@ -315,12 +315,16 @@ pcl::recognition::ORROctree::getFullLeavesIntersectedBySphere (const float* p, f for ( i = 0 ; i < 8 ; ++i ) { child = node->getChild (i); - // We do not want to push all children -> only leaves or children with children - if ( child->hasData () || child->hasChildren () ) + // We do not want to push all children -> only children with children or leaves + if (child->hasChildren ()) + nodes.push_back(child); + // only push back the child if it is not the leaf of p + else if (child->hasData () && !aux::equal3 (p, child->getData ()->getPoint ())) nodes.push_back (child); } } - else if ( node->hasData () ) + // only push back the node if it is not the leaf of p + else if (node->hasData () && !aux::equal3 (p, node->getData ()->getPoint ())) out.push_back (node); // We got a full leaf } } diff --git a/test/test_recognition_ransac_based_ORROctree.cpp b/test/test_recognition_ransac_based_ORROctree.cpp new file mode 100644 index 00000000000..e9c624862a1 --- /dev/null +++ b/test/test_recognition_ransac_based_ORROctree.cpp @@ -0,0 +1,147 @@ + +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2011, Willow Garage, 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. + * + * $Id: $ + * + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace pcl; +using namespace pcl::io; +using namespace pcl::recognition; + +typedef pcl::PointXYZ PointT; +typedef pcl::PointCloud PointCloudT; +typedef pcl::PointCloud PointCloudTN; +typedef pcl::PointCloud::Ptr PointCloudTPtr; +typedef pcl::PointCloud::Ptr PointCloudTNPtr; + +PointCloud::Ptr cloud_; +PointCloudTPtr model_cloud(new pcl::PointCloud); +PointCloudTNPtr model_cloud_normals (new pcl::PointCloud); + +////////////////////////////////////////////////////////////////////////////////////////////// + +int +estimateNormals(pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr cloud_normals) { + // Create the normal estimation class, and pass the input dataset to it + pcl::NormalEstimation ne; + ne.setInputCloud (cloud); + + // Create an empty kdtree representation, and pass it to the normal estimation object. + // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given). + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree ()); + ne.setSearchMethod (tree); + + // Use all neighbors in a sphere of radius 1m + // experiments with tensors dataset show that the points are as far apart as 1m from each other + ne.setRadiusSearch (0.03); + + // Compute the features + ne.compute (*cloud_normals); + + // cloud_normals->points.size () should have the same size as the input cloud->points.size ()* + return cloud_normals->points.size(); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// + +TEST (ORROctreeTest, OctreeSphereIntersection) +{ + float voxel_size = 0.02f; + float pair_width = 0.05f; + float frac_of_points_for_registration = 0.3f; + std::string object_name = "test_object"; + + ModelLibrary::Model* new_model = new ModelLibrary::Model (*model_cloud, *model_cloud_normals, voxel_size, object_name, frac_of_points_for_registration); + + const ORROctree& octree = new_model->getOctree (); + const vector &full_leaves = octree.getFullLeaves (); + list inter_leaves; + + // Run through all full leaves + for ( vector::const_iterator leaf1 = full_leaves.begin () ; leaf1 != full_leaves.end () ; ++leaf1 ) + { + const ORROctree::Node::Data* node_data1 = (*leaf1)->getData (); + // Get all full leaves at the right distance to the current leaf + inter_leaves.clear (); + octree.getFullLeavesIntersectedBySphere (node_data1->getPoint (), pair_width, inter_leaves); + // Ensure that inter_leaves does not contain leaf1 + for ( list::iterator leaf2 = inter_leaves.begin () ; leaf2 != inter_leaves.end () ; ++leaf2 ) + { + EXPECT_NE(*leaf1, *leaf2); + } + } + +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +//* ---[ */ +int + main (int argc, char** argv) +{ + if (argc < 2) + { + std::cerr << "No test file given. Please download `bunny.pcd` and pass its path to the test." << std::endl; + return (-1); + } + + // Load a standard PCD file from disk + if (pcl::io::loadPCDFile (argv[1], *model_cloud) < 0) + { + std::cerr << "Failed to read test file. Please download `bunny.pcd` and pass its path to the test." << std::endl; + return (-1); + } + + if (!estimateNormals(model_cloud, model_cloud_normals) == model_cloud->points.size()) + { + std::cerr << "Failed to estimate normals" << std::endl; + return (-1); + } + + testing::InitGoogleTest (&argc, argv); + return (RUN_ALL_TESTS ()); +} +/* ]--- */