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 leaf 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 leaf 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 ());
+}
+/* ]--- */