iLab Neuromorphic Robotics Toolkit  0.1
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
test-PointCloud2Registration.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 
38 #include <random>
39 
41 
43 
53 
54 using namespace nrt;
55 
56 void createRandomCloud( PointCloud2 & cloud, int size )
57 {
58  std::mt19937 rng(12345u);
59  std::uniform_real_distribution<float> dist( -3.0f, 3.0f );
60 
61  cloud.resize( size );
62  for( auto i = cloud.geometry_begin(); i != cloud.geometry_end(); ++i )
63  *i = {dist(rng), dist(rng), dist(rng)};
64 }
65 
66 int main( int argc, char ** argv )
67 {
68  PointCloud2 cloud1;
69 
70  createRandomCloud( cloud1, 10 );
71 
72  PointCloud2 cloud2 = cloud1;
73 
74  for( auto i = cloud1.geometry_begin(); i != cloud1.geometry_end(); ++i )
75  {
76  i->x() += 0.7;
77  }
78 
79  // Create the parameters for our registration methods
80  ConvergenceCriteriaBase::SharedPtr convergeCritera( new StandardConvergenceCriteria(10) );
81 
82  CorrespondenceEstimationBase::SharedPtr correspondenceEstimator( new CorrespondenceEstimationNearestNeighbor() );
83 
85 
88 
90 
92 
93  // Construct the registration objects
94  Registration myRegistrationSVD( correspondenceEstimator, {correspondencePruner},
95  estimationSVD,
96  convergeCritera );
97 
98  Registration myRegistrationLM( correspondenceEstimator, {correspondencePruner},
99  estimationLM,
100  convergeCritera );
101 
102  // Perform registration - given the sample data created above we should
103  // expect a registration that gives no rotation and an X displacement of -0.7
104  auto lmResult = myRegistrationLM.align( cloud1, cloud2 );
105  NRT_INFO( "LM Results (expect -0.7 x translation): " << lmResult.matrix() );
106 
107  auto svdResult = myRegistrationSVD.align( cloud1, cloud2 );
108  NRT_INFO( "SVD Results (expect -0.7 x translation): " << svdResult.matrix() );
109 
110  // Basic test that was in PCL example
111  PointCloud2 cloud3(5);
112  cloud3[0] = {0.352222, -0.151883, -0.106395};
113  cloud3[1] = {-0.397406, -0.473106, 0.292602};
114  cloud3[2] = {-0.731898, 0.667105, 0.441304};
115  cloud3[3] = {-0.734766, 0.854581, -0.0361733};
116  cloud3[4] = {-0.4607, -0.277468, -0.916762};
117 
118  PointCloud2 cloud4 = cloud3;
119  for( auto i = cloud3.geometry_begin(); i != cloud3.geometry_end(); ++i )
120  i->x() += 0.7;
121 
122  svdResult = myRegistrationSVD.align( cloud3, cloud4 );
123  NRT_INFO( "SVD PCL example Results (expect -0.7 x translation): " << svdResult.matrix() );
124 
125  // Testing again with RANSAC based rejection
127 
128  auto myRegistrationSVDRANSAC = Registration( correspondenceEstimator, {ransacPruner},
129  estimationSVD,
130  convergeCritera );
131 
132  auto myRegistrationLMRANSAC = Registration( correspondenceEstimator, {ransacPruner},
133  estimationLM,
134  convergeCritera );
135 
136  // Perform registration - given the sample data created above we should
137  // expect a registration that gives no rotation and an X displacement of -0.7
138  lmResult = myRegistrationLMRANSAC.align( cloud1, cloud2 );
139  NRT_INFO( "LM RANSAC Results (expect -0.7 x translation): " << lmResult.matrix() );
140 
141  svdResult = myRegistrationSVDRANSAC.align( cloud1, cloud2 );
142  NRT_INFO( "SVD RANSAC Results (expect -0.7 x translation): " << svdResult.matrix() );
143 
144  svdResult = myRegistrationSVDRANSAC.align( cloud3, cloud4 );
145  NRT_INFO( "SVD RANSAC PCL example Results (expect -0.7 x translation): " << svdResult.matrix() );
146 
147  // Testing with rotation based transforms
148  PointCloud2 cloud5 = cloud1;
149  PointCloud2::AffineTransform transform = PointCloud2::AffineTransform::Identity();
150  transform.rotate( Eigen::AngleAxisf( 0.261799388, Eigen::Vector3f::UnitZ() ) ); // 15 degrees about Z
151  transform.rotate( Eigen::AngleAxisf( 0.261799388, Eigen::Vector3f::UnitX() ) ); // 15 degrees about X
152  transformPointCloudInPlace( cloud5, transform );
153 
154  svdResult = myRegistrationSVD.align( cloud1, cloud5 );
155  NRT_INFO( "SVD rotation Results: " << svdResult.rotation() );
156  NRT_INFO( "does that look like this? " << transform.rotation() );
157 
158  lmResult = myRegistrationLM.align( cloud1, cloud5 );
159  NRT_INFO( "LM rotation Results: " << svdResult.rotation() );
160  NRT_INFO( "does that look like this? " << transform.rotation() );
161 
162  // rotation and translation
163  transform.translate( Eigen::Vector3f::UnitZ()*2 );
164 
165  cloud5 = cloud1;
166  transformPointCloudInPlace( cloud5, transform );
167 
168  svdResult = myRegistrationSVD.align( cloud1, cloud5 );
169  NRT_INFO( "SVD rotation translation Results: " << svdResult.matrix() );
170  NRT_INFO( "does that look like this? " << transform.matrix() );
171  NRT_INFO( "does this look like zero:");
172  auto temp = transformPointCloud( cloud1, svdResult );
173  for( size_t i = 0; i < cloud1.size(); ++i )
174  NRT_INFO( (temp.at(i).geometry() - cloud5.at(i).geometry() ) );
175 
176  lmResult = myRegistrationLM.align( cloud1, cloud5 );
177  NRT_INFO( "LM rotation translation Results: " << svdResult.matrix() );
178  NRT_INFO( "does that look like this? " << transform.matrix() );
179 
180  // with ransac
181  svdResult = myRegistrationSVDRANSAC.align( cloud1, cloud5 );
182  NRT_INFO( "SVD RANSAC rotation translation Results: " << svdResult.matrix() );
183  NRT_INFO( "does that look like this? " << transform.matrix() );
184 
185  lmResult = myRegistrationLMRANSAC.align( cloud1, cloud5 );
186  NRT_INFO( "LM RANSAC rotation translation Results: " << svdResult.matrix() );
187  NRT_INFO( "does that look like this? " << transform.matrix() );
188 
189  // with normals
190  Normals normalWorker(PointCloudSearchMethod::knn, 5 );
191 
192  auto normalCloud = normalWorker.computeFeatures( cloud1 );
193  cloud5 = normalCloud;
194  transformPointCloudWithNormalsInPlace( cloud5, transform );
195 
196  auto myRegistrationP2PRANSAC = Registration( correspondenceEstimator, {ransacPruner},
197  estimationP2P,
198  convergeCritera );
199 
200  auto p2pResult = myRegistrationP2PRANSAC.align( normalCloud, cloud5 );
201  NRT_INFO( "PointToPlane rotation translation Results: " << p2pResult.matrix() );
202  NRT_INFO( "does that look like this? " << transform.matrix() );
203 
204  // with normals part deux
205  auto myRegistrationP2PLLSRANSAC = Registration( correspondenceEstimator, {ransacPruner},
206  estimationP2PLLS,
207  convergeCritera );
208 
209  auto p2pllsResult = myRegistrationP2PLLSRANSAC.align( normalCloud, cloud5 );
210  NRT_INFO( "PointToPlane LLS rotation translation Results: " << p2pllsResult.matrix() );
211  NRT_INFO( "does that look like this? " << transform.matrix() );
212 }
213 #else
214 int main( int argc, char ** argv )
215 {
216  return 1;
217 }
218 #endif // NRT_HAVE_CLOUD