iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
test-PointCloud2Common.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 #include <nrt/config.h>
35 #ifdef NRT_HAVE_CLOUD
36 
42 #include <array>
43 
44 using namespace nrt;
45 
46 int main( int argc, char ** argv )
47 {
48  typedef std::array<int,3> Arr;
50 
51  PointCloud2 cloud2 = PointCloud2::create<int, bool, Arr>();
52  typedef PointCloud2::Geometry Geometry;
53 
54  //TODO: allow this to work without the wrap up in pointcloud2data
55  // also add emplace
56  // also allow this to work with initializer lists
57  Arr xx = {{1,2,3}};
58  Arr yy = {{0,0,0}};
59  cloud2.insert( PT(Geometry(0), 3, true, xx ) );
60  cloud2.insert( PT(Geometry(1), 2, false, yy ) );
61  cloud2.insert( PT(Geometry(7), 7, false, yy ) );
62 
63  // Test centroid and minmax
64  auto centroid = computeCentroid( cloud2 );
65  Geometry min, max;
66  computeMinMax( cloud2, min, max );
67 
68  NRT_INFO( "centroid " << centroid );
69  NRT_INFO( "min " << min );
70  NRT_INFO( "max " << max );
71 
72  // Test covariance
73  NRT_INFO( computeCovariance( cloud2 ) );
74  NRT_INFO( computeCovariance( cloud2, true ) );
75 
76  // Test centroid on specific fields
77  auto centroidInt = computeCentroid<int>( cloud2 );
78  NRT_INFO( centroidInt );
79  //NRT_INFO( computeCentroid<Arr>( cloud2 ) ); // Should not compile since array does not support arithmetic ops
80  // TODO: throw some type traits magic in here to make this a nice compile time error
81 
82  // Test minmax on specific fields
83  bool minB, maxB;
84  int minI, maxI;
85  computeMinMax<bool>( cloud2, minB, maxB );
86  computeMinMax<int>( cloud2, minI, maxI );
87 
88  NRT_INFO( minB << " " << maxB << " " << minI << " " << maxI);
89 
90  // Test transform
91  auto demeaned = demeanPointCloud( cloud2, centroid );
92  auto demeanedInt = demeanPointCloud<int>( demeaned, centroidInt );
93 
94  auto iter1 = cloud2.begin<int>();
95  auto iter2 = demeanedInt.begin<int>();
96 
97  for( ; iter1 != cloud2.end<int>(); ++iter1, ++iter2 )
98  {
99  NRT_INFO( "original " << *iter1 );
100  NRT_INFO( "demeaned " << *iter2 );
101  }
102 
103  auto cloudIdent = transformPointCloud( cloud2, PointCloud2::AffineTransform::Identity() );
104 
105  auto iter3 = cloud2.geometry_begin();
106  auto iter4 = cloudIdent.geometry_begin();
107 
108  for( ; iter3 != cloud2.geometry_end(); ++iter3, ++iter4 )
109  NRT_INFO( "expect zeros " << *iter3 - *iter4 );
110 
111  transformPointCloudInPlace( cloud2, PointCloud2::AffineTransform::Identity() );
112  for( auto i = cloud2.geometry_const_begin(); i != cloud2.geometry_const_end(); ++i )
113  NRT_INFO( *i );
114 
115  return 0;
116 }
117 #else
118 int main( int argc, char ** argv )
119 {
120  return 1;
121 }
122 #endif // NRT_HAVE_CLOUD