test-PointCloud2Search.C

Go to the documentation of this file.

/*! @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

#include <nrt/PointCloud2/Search/Search.H>

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
  cloud.addField<SparseField<double>>();

  // 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