test_keypoints.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  */
00035 
00038 #include <gtest/gtest.h>
00039 
00040 #include <pcl/point_types.h>
00041 #include <pcl/io/pcd_io.h>
00042 #include <pcl/filters/approximate_voxel_grid.h>
00043 
00044 #include <pcl/keypoints/sift_keypoint.h>
00045 
00046 #include <set>
00047 
00048 using namespace pcl;
00049 using namespace pcl::io;
00050 using namespace std;
00051 
00052 struct KeypointT
00053 {
00054   float x, y, z, scale;
00055 };
00056 
00057 POINT_CLOUD_REGISTER_POINT_STRUCT (KeypointT,
00058     (float, x, x)
00059     (float, y, y)
00060     (float, z, z)
00061     (float, scale, scale)
00062 )
00063 
00064 PointCloud<PointXYZI>::Ptr cloud_xyzi (new PointCloud<PointXYZI>);
00065 
00067 TEST (PCL, SIFTKeypoint)
00068 {
00069   PointCloud<KeypointT> keypoints;
00070 
00071   // Compute the SIFT keypoints
00072   SIFTKeypoint<PointXYZI, KeypointT> sift_detector;
00073         search::KdTree<PointXYZI>::Ptr tree (new search::KdTree<PointXYZI>);
00074   sift_detector.setSearchMethod (tree);
00075   sift_detector.setScales (0.02f, 5, 3);
00076   sift_detector.setMinimumContrast (0.03f);
00077 
00078   sift_detector.setInputCloud (cloud_xyzi);
00079   sift_detector.compute (keypoints);
00080 
00081   ASSERT_EQ (keypoints.width, keypoints.points.size ());
00082   ASSERT_EQ (keypoints.height, 1);
00083   EXPECT_EQ (keypoints.points.size (), static_cast<size_t> (169));
00084 
00085   // Change the values and re-compute
00086   sift_detector.setScales (0.05f, 5, 3);
00087   sift_detector.setMinimumContrast (0.06f);
00088   sift_detector.compute (keypoints);
00089 
00090   ASSERT_EQ (keypoints.width, keypoints.points.size ());
00091   ASSERT_EQ (keypoints.height, 1);
00092 
00093   // Compare to previously validated output
00094   const size_t correct_nr_keypoints = 5;
00095   const float correct_keypoints[correct_nr_keypoints][4] = 
00096     { 
00097       // { x,  y,  z,  scale }
00098       {-0.9425f, -0.6381f,  1.6445f,  0.0794f},
00099       {-0.5083f, -0.5587f,  1.8519f,  0.0500f},
00100       { 1.0265f,  0.0500f,  1.7154f,  0.1000f},
00101       { 0.3005f, -0.3007f,  1.9526f,  0.2000f},
00102       {-0.1002f, -0.1002f,  1.9933f,  0.3175f}
00103     };
00104 
00105   ASSERT_EQ (keypoints.points.size (), correct_nr_keypoints);
00106   for (size_t i = 0; i < correct_nr_keypoints; ++i)
00107   {
00108     EXPECT_NEAR (keypoints.points[i].x, correct_keypoints[i][0], 1e-4);
00109     EXPECT_NEAR (keypoints.points[i].y, correct_keypoints[i][1], 1e-4);
00110     EXPECT_NEAR (keypoints.points[i].z, correct_keypoints[i][2], 1e-4);
00111     EXPECT_NEAR (keypoints.points[i].scale, correct_keypoints[i][3], 1e-4);
00112   }
00113 
00114 }
00115 
00116 TEST (PCL, SIFTKeypoint_radiusSearch)
00117 {
00118   const int nr_scales_per_octave = 3;
00119   const float scale = 0.02f;
00120 
00121   KdTreeFLANN<PointXYZI>::Ptr tree_ (new KdTreeFLANN<PointXYZI>);
00122   boost::shared_ptr<pcl::PointCloud<PointXYZI> > cloud = cloud_xyzi->makeShared ();
00123 
00124   ApproximateVoxelGrid<PointXYZI> voxel_grid;
00125   const float s = 1.0 * scale;
00126   voxel_grid.setLeafSize (s, s, s);
00127   voxel_grid.setInputCloud (cloud);
00128   voxel_grid.filter (*cloud);
00129   tree_->setInputCloud (cloud);
00130   
00131   const PointCloud<PointXYZI> & input = *cloud;
00132   KdTreeFLANN<PointXYZI> & tree = *tree_;
00133   const float base_scale = scale;
00134 
00135   std::vector<float> scales (nr_scales_per_octave + 3);
00136   for (int i_scale = 0; i_scale <= nr_scales_per_octave + 2; ++i_scale)
00137   {
00138     scales[i_scale] = base_scale * pow (2.0f, static_cast<float> (i_scale-1) / nr_scales_per_octave);
00139   }
00140   Eigen::MatrixXf diff_of_gauss;
00141 
00142   std::vector<int> nn_indices;
00143   std::vector<float> nn_dist;
00144   diff_of_gauss.resize (input.size (), scales.size () - 1);
00145 
00146   const float max_radius = 0.10f;
00147 
00148   const size_t i_point = 500;
00149   tree.radiusSearch (i_point, max_radius, nn_indices, nn_dist);
00150 
00151   // Are they all unique?
00152   set<int> unique_indices;
00153   for (size_t i_neighbor = 0; i_neighbor < nn_indices.size (); ++i_neighbor)
00154   {
00155     unique_indices.insert (nn_indices[i_neighbor]);
00156   }
00157 
00158   EXPECT_EQ (nn_indices.size (), unique_indices.size ());
00159 }
00160 
00161 /* ---[ */
00162 int
00163   main (int argc, char** argv)
00164 {
00165   if (argc < 2)
00166   {
00167     std::cerr << "No test file given. Please download `cturtle.pcd` and pass its path to the test." << std::endl;
00168     return (-1);
00169   }
00170 
00171   // Load a sample point cloud
00172   if (io::loadPCDFile (argv[1], *cloud_xyzi) < 0)
00173   {
00174     std::cerr << "Failed to read test file. Please download `cturtle.pcd` and pass its path to the test." << std::endl;
00175     return (-1);
00176   }
00177 
00178   testing::InitGoogleTest (&argc, argv);
00179   return (RUN_ALL_TESTS ());
00180 }
00181 /* ]--- */


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:18:20