iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
CorrespondenceEstimationNearestNeighborImpl.H
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 #ifndef NRT_POINTCLOUD2_REGISTRATION_CORRESPONDENCE_DETAILS_CORRESPONDENCEESTIMATIONNEARESTNEIGHBORIMPL_H_
36 #define NRT_POINTCLOUD2_REGISTRATION_CORRESPONDENCE_DETAILS_CORRESPONDENCEESTIMATIONNEARESTNEIGHBORIMPL_H_
37 
38 template <class Field> inline
40  PointCloud2 const target ) -> Correspondences
41 {
42  Correspondences retVal( source.size() );
43  rebuildSearch( target );
44 
45  size_t index = 0;
46  auto corrIter = retVal.begin();
47  for( auto iter = source.template const_begin<Field>(), end = source.template const_end<Field>(); iter != end; ++iter, ++corrIter, ++index )
48  {
49  auto dist = itsSearch.template knn<Field>( iter->template get<Field>(), 1 )[0];
50  corrIter->sourceIndex = index;
51  corrIter->targetIndex = dist.first;
52  corrIter->score = dist.second;
53  }
54 
55  return retVal;
56 }
57 
58 template <class Field> inline
60  PointCloud2 const target ) -> Correspondences
61 {
62  Correspondences retVal( source.size() );
63  rebuildSearch( target );
64 
65  Search reverseSearch( source, itsSearchEpsilon );
66 
67  size_t index = 0;
68  size_t validCorrs = 0;
69  auto corrIter = retVal.begin();
70  auto targetIter = target.const_begin<Field>();
71  for( auto iter = source.const_begin<Field>(), end = source.const_end<Field>(); iter != end; ++iter, ++index )
72  {
73  // Go the first way
74  auto dist = itsSearch.knn<Field>( iter->template get<Field>(), 1 )[0];
75 
76  // Check if we get the same result going backwards
77  auto distReciprocal = reverseSearch.knn<Field>( (targetIter + dist.first)->template get<Field>(), 1 )[0];
78 
79  if( distReciprocal.first == index )
80  {
81  corrIter->sourceIndex = index;
82  corrIter->targetIndex = dist.first;
83  corrIter->score = dist.second;
84  ++validCorrs;
85  ++corrIter;
86  }
87  }
88 
89  retVal.resize( validCorrs );
90 
91  return retVal;
92 }
93 
94 #endif // NRT_POINTCLOUD2_REGISTRATION_CORRESPONDENCE_DETAILS_CORRESPONDENCEESTIMATIONNEARESTNEIGHBORIMPL_H_