iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
tests/test-PointCloud2Search.C
/*! @file
@author Shane Grant
@copyright GNU Public License (GPL v3)
@section License
@verbatim
// ////////////////////////////////////////////////////////////////////////
// The iLab Neuromorphic Robotics Toolkit (NRT) //
// Copyright 2010-2012 by the University of Southern California (USC) //
// and the iLab at USC. //
// //
// iLab - University of Southern California //
// Hedco Neurociences Building, Room HNB-10 //
// Los Angeles, Ca 90089-2520 - USA //
// //
// See http://ilab.usc.edu for information about this project. //
// ////////////////////////////////////////////////////////////////////////
// This file is part of The iLab Neuromorphic Robotics Toolkit. //
// //
// The iLab Neuromorphic Robotics Toolkit is free software: you can //
// redistribute it and/or modify it under the terms of the GNU General //
// Public License as published by the Free Software Foundation, either //
// version 3 of the License, or (at your option) any later version. //
// //
// The iLab Neuromorphic Robotics Toolkit is distributed in the hope //
// that it will be useful, but WITHOUT ANY WARRANTY; without even the //
// implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR //
// PURPOSE. See the GNU General Public License for more details. //
// //
// You should have received a copy of the GNU General Public License //
// along with The iLab Neuromorphic Robotics Toolkit. If not, see //
// <http://www.gnu.org/licenses/>. //
// ////////////////////////////////////////////////////////////////////////
@endverbatim */
#include <nrt/config.h>
#ifdef NRT_HAVE_CLOUD
using namespace nrt;
int main( int argc, char ** argv )
{
PointCloud2 cloud;
cloud.resize( 10 );
for( size_t i = 0; i < 10; ++i )
cloud[i] = PointCloud2::Geometry(i);
Search search( cloud );
// search for 3 nearest neighbors, expect nearest = 1, 1, 1, 1
auto result = search.knn( PointCloud2::Geometry(0.75f), 3 );
NRT_INFO( "Found " << result.size() << " neighbors: " );
for( size_t i = 0; i < result.size(); ++i )
NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud[result[i].first] );
// Do another search with more neighbors
result = search.knn( PointCloud2::Geometry( 1, -2, 3.2, 1 ), 5 );
NRT_INFO( "Found " << result.size() << " neighbors: " );
for( size_t i = 0; i < result.size(); ++i )
NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud[result[i].first] );
// Try out radius, no sorting
result = search.radius( PointCloud2::Geometry(4), 2.0 );
NRT_INFO( "Found " << result.size() << " neighbors: " );
for( size_t i = 0; i < result.size(); ++i )
NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud[result[i].first] );
// Try radius with closest, sorted
result = search.radius( PointCloud2::Geometry(4), 5.0, 0, true );
NRT_INFO( "Found " << result.size() << " neighbors: " );
for( size_t i = 0; i < result.size(); ++i )
NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud[result[i].first] );
// Add some strange stuff to the cloud and sort that
// TODO: create makeBag function that works like make_tuple
cloud.insertSparse( 1, StreamableGenericBag<double>(3.0) );
cloud.insertSparse( 2, StreamableGenericBag<double>(-11.0) );
cloud.insertSparse( 5, StreamableGenericBag<double>(7.0) );
cloud.insertSparse( 3, StreamableGenericBag<double>(6.0) );
// update search
search.rebuild( cloud );
// knn should return all 3 results in order
result = search.knn( 3.7, 5 );
NRT_INFO( "Found " << result.size() << " neighbors: " );
for( size_t i = 0; i < result.size(); ++i )
NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud.at<double>(result[i].first) );
// Radius search on the sparse data
result = search.radius( 0.0, 10 );
NRT_INFO( "Found " << result.size() << " neighbors: " );
for( size_t i = 0; i < result.size(); ++i )
NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud.at<double>(result[i].first) );
// Test searching with a custom search functor
std::cout << "---------------" << std::endl;
result = search.knn( 3.7, 5, flann::L2<PointCloud2::BaseType>() );
NRT_INFO( "Found " << result.size() << " neighbors: " );
for( size_t i = 0; i < result.size(); ++i )
NRT_INFO( result[i].first << ": " << result[i].second << ": " << cloud.at<double>(result[i].first) );
}
#else
int main( int argc, char ** argv )
{
return 1;
}
#endif // NRT_HAVE_CLOUD