Merge pull request #269 from musi00/ObjRecRANSAC_addModel_error

ObjRecRANSAC::addModel() error

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 <typename T> 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 <typename T> void
       add3 (T a[3], const T b[3])

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<float> (p, node->getData ()->getPoint ()))
         out.push_back (node); // We got a full leaf
     }
   }

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 <gtest/gtest.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/recognition/ransac_based/model_library.h>
+#include <pcl/features/normal_3d.h>
+
+using namespace std;
+using namespace pcl;
+using namespace pcl::io;
+using namespace pcl::recognition;
+
+typedef pcl::PointXYZ PointT;
+typedef pcl::PointCloud<PointT> PointCloudT;
+typedef pcl::PointCloud<pcl::Normal> PointCloudTN;
+typedef pcl::PointCloud<PointT>::Ptr PointCloudTPtr;
+typedef pcl::PointCloud<pcl::Normal>::Ptr PointCloudTNPtr;
+
+PointCloud<PointXYZ>::Ptr cloud_;
+PointCloudTPtr model_cloud(new pcl::PointCloud<PointT>);
+PointCloudTNPtr model_cloud_normals (new pcl::PointCloud<pcl::Normal>);
+
+//////////////////////////////////////////////////////////////////////////////////////////////
+
+int
+estimateNormals(pcl::PointCloud<PointT>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr cloud_normals) {
+    // Create the normal estimation class, and pass the input dataset to it
+      pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> 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<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
+      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<ORROctree::Node*> &full_leaves = octree.getFullLeaves ();
+  list<ORROctree::Node*> inter_leaves;
+
+  // Run through all full leaves
+  for ( vector<ORROctree::Node*>::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<ORROctree::Node*>::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 ());
+}
+/* ]--- */