iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
test-PointCloud2Search.C
Go to the documentation of this file.
1 /*! @file
2  @author Shane Grant
3  @copyright GNU Public License (GPL v3)
4  @section License
5  @verbatim
6  // ////////////////////////////////////////////////////////////////////////
7  // The iLab Neuromorphic Robotics Toolkit (NRT) //
8  // Copyright 2010-2012 by the University of Southern California (USC) //
9  // and the iLab at USC. //
10  // //
11  // iLab - University of Southern California //
12  // Hedco Neurociences Building, Room HNB-10 //
13  // Los Angeles, Ca 90089-2520 - USA //
14  // //
15  // See http://ilab.usc.edu for information about this project. //
16  // ////////////////////////////////////////////////////////////////////////
17  // This file is part of The iLab Neuromorphic Robotics Toolkit. //
18  // //
19  // The iLab Neuromorphic Robotics Toolkit is free software: you can //
20  // redistribute it and/or modify it under the terms of the GNU General //
21  // Public License as published by the Free Software Foundation, either //
22  // version 3 of the License, or (at your option) any later version. //
23  // //
24  // The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
25  // that it will be useful, but WITHOUT ANY WARRANTY; without even the //
26  // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
27  // PURPOSE. See the GNU General Public License for more details. //
28  // //
29  // You should have received a copy of the GNU General Public License //
30  // along with The iLab Neuromorphic Robotics Toolkit. If not, see //
31  // <http://www.gnu.org/licenses/>. //
32  // ////////////////////////////////////////////////////////////////////////
33  @endverbatim */
34 
35 #include <nrt/config.h>
36 #ifdef NRT_HAVE_CLOUD
37 
39 
40 using namespace nrt;
41 
42 int main( int argc, char ** argv )
43 {
44  PointCloud2 cloud;
45 
46  cloud.resize( 10 );
47 
48  for( size_t i = 0; i < 10; ++i )
49  cloud[i] = PointCloud2::Geometry(i);
50 
51  Search search( cloud );
52 
53  // search for 3 nearest neighbors, expect nearest = 1, 1, 1, 1
54  auto result = search.knn( PointCloud2::Geometry(0.75f), 3 );
55 
56  NRT_INFO( "Found " << result.size() << " neighbors: " );
57  for( size_t i = 0; i < result.size(); ++i )
58  NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud[result[i].first] );
59 
60  // Do another search with more neighbors
61  result = search.knn( PointCloud2::Geometry( 1, -2, 3.2, 1 ), 5 );
62  NRT_INFO( "Found " << result.size() << " neighbors: " );
63  for( size_t i = 0; i < result.size(); ++i )
64  NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud[result[i].first] );
65 
66  // Try out radius, no sorting
67  result = search.radius( PointCloud2::Geometry(4), 2.0 );
68  NRT_INFO( "Found " << result.size() << " neighbors: " );
69  for( size_t i = 0; i < result.size(); ++i )
70  NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud[result[i].first] );
71 
72  // Try radius with closest, sorted
73  result = search.radius( PointCloud2::Geometry(4), 5.0, 0, true );
74  NRT_INFO( "Found " << result.size() << " neighbors: " );
75  for( size_t i = 0; i < result.size(); ++i )
76  NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud[result[i].first] );
77 
78  // Add some strange stuff to the cloud and sort that
80 
81  // TODO: create makeBag function that works like make_tuple
82  cloud.insertSparse( 1, StreamableGenericBag<double>(3.0) );
83  cloud.insertSparse( 2, StreamableGenericBag<double>(-11.0) );
84  cloud.insertSparse( 5, StreamableGenericBag<double>(7.0) );
85  cloud.insertSparse( 3, StreamableGenericBag<double>(6.0) );
86 
87  // update search
88  search.rebuild( cloud );
89 
90  // knn should return all 3 results in order
91  result = search.knn( 3.7, 5 );
92 
93  NRT_INFO( "Found " << result.size() << " neighbors: " );
94  for( size_t i = 0; i < result.size(); ++i )
95  NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud.at<double>(result[i].first) );
96 
97  // Radius search on the sparse data
98  result = search.radius( 0.0, 10 );
99  NRT_INFO( "Found " << result.size() << " neighbors: " );
100  for( size_t i = 0; i < result.size(); ++i )
101  NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud.at<double>(result[i].first) );
102 
103  // Test searching with a custom search functor
104  std::cout << "---------------" << std::endl;
105  result = search.knn( 3.7, 5, flann::L2<PointCloud2::BaseType>() );
106  NRT_INFO( "Found " << result.size() << " neighbors: " );
107  for( size_t i = 0; i < result.size(); ++i )
108  NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud.at<double>(result[i].first) );
109 }
110 #else
111 int main( int argc, char ** argv )
112 {
113  return 1;
114 }
115 #endif // NRT_HAVE_CLOUD