utest.cpp
Go to the documentation of this file.
1 // kate: replace-tabs off; indent-width 4; indent-mode normal
2 // vim: ts=4:sw=4:noexpandtab
3 /*
4 
5 Copyright (c) 2010--2012,
6 François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
7 You can contact the authors at <f dot pomerleau at gmail dot com> and
8 <stephane at magnenat dot net>
9 
10 All rights reserved.
11 
12 Redistribution and use in source and binary forms, with or without
13 modification, are permitted provided that the following conditions are met:
14  * Redistributions of source code must retain the above copyright
15  notice, this list of conditions and the following disclaimer.
16  * Redistributions in binary form must reproduce the above copyright
17  notice, this list of conditions and the following disclaimer in the
18  documentation and/or other materials provided with the distribution.
19  * Neither the name of the <organization> nor the
20  names of its contributors may be used to endorse or promote products
21  derived from this software without specific prior written permission.
22 
23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
24 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
25 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26 DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
27 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
28 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
30 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
31 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
32 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33 
34 */
35 
36 #include "utest.h"
37 
38 using namespace std;
39 using namespace PointMatcherSupport;
40 
41 // TODO: avoid global by using testing::Environment
42 
44 
51 
52 //---------------------------
53 // Test ICP with all existing filters.
54 
55 // Algorithm:
56 // 1. Iterate over all yaml files in
57 // libpointmatcher/examples/data/icp_data, each file tests ICP
58 // with one or more filters.
59 // 2. Run ICP with the given yaml file. The filters in the yaml
60 // file are applied along the way.
61 // 3. Write the obtained ICP transform to disk, to the same directory,
62 // with file extension .cur_trans (for easy future comparisons).
63 // 4. Load the reference (known as correct) ICP transform from disk,
64 // from the same directory, with file extension .ref_trans.
65 // 5. See if the current and reference transforms are equal.
66 
67 // To update an existing test or add a new test, simply add/modify
68 // the desired yaml file, run the tests (they may fail this time), then
69 // copy the (just written) desired current transform file on top of the
70 // corresponding reference transform file. Run the tests again. This
71 // time they will succeed.
72 //---------------------------
73 
74 // Find the median coefficient of a matrix
75 double median_coeff(Eigen::MatrixXf& A){
76  Eigen::Map<Eigen::VectorXf> v(A.data(),A.size());
77  std::sort(v.data(), v.data() + v.size());
78  return v[v.size()/2];
79 }
80 
81 TEST(icpTest, icpTest)
82 {
83  DP ref = DP::load(dataPath + "cloud.00000.vtk");
84  DP data = DP::load(dataPath + "cloud.00001.vtk");
85 
86  namespace fs = boost::filesystem;
87  fs::path config_dir(dataPath + "icp_data");
88  EXPECT_TRUE( fs::exists(config_dir) && fs::is_directory(config_dir) );
89 
90  fs::directory_iterator end_iter;
91  for( fs::directory_iterator d(config_dir); d != end_iter; ++d)
92  {
93  if (!fs::is_regular_file(d->status()) ) continue;
94 
95  std::cout << "Testing file " << d->path().string() << std::endl;
96  // Load config file, and form ICP object
97  PM::ICP icp;
98  std::string config_file = d->path().string();
99  if (fs::extension(config_file) != ".yaml") continue;
100  std::ifstream ifs(config_file.c_str());
101  EXPECT_NO_THROW(icp.loadFromYaml(ifs)) << "This error was caused by the test file:" << endl << " " << config_file;
102 
103  // Compute current ICP transform
104  PM::TransformationParameters curT = icp(data, ref);
105 
106  // Write current transform to disk (to easily compare it
107  // with reference transform offline)
108  fs::path cur_file = d->path();
109  cur_file.replace_extension(".cur_trans");
110  //std::cout << "Writing: " << cur_file << std::endl;
111  std::ofstream otfs(cur_file.c_str());
112  otfs.precision(16);
113  otfs << curT;
114  otfs.close();
115 
116  // Load reference transform
117  fs::path ref_file = d->path();
118  ref_file.replace_extension(".ref_trans");
119  PM::TransformationParameters refT = 0*curT;
120  //std::cout << "Reading: " << ref_file << std::endl;
121  std::ifstream itfs(ref_file.c_str());
122  EXPECT_TRUE(itfs.good()) << "Could not find " << ref_file
123  << ". If this is the first time this test is run, "
124  << "create it as a copy of " << cur_file;
125 
126  for (int row = 0; row < refT.cols(); row++)
127  {
128  for (int col = 0; col < refT.cols(); col++)
129  {
130  itfs >>refT(row, col);
131  }
132  }
133 
134  // Dump the reference transform and current one
135  //std::cout.precision(17);
136  //std::cout << "refT:\n" << refT << std::endl;
137  //std::cout << "curT:\n" << curT << std::endl;
138 
139  // We need to compare the stored icp transform vs the computed one.
140  // Since the icp solution is not unique, they may differ a lot.
141  // Yet, the point of icp is
142  // curT*data = ref, and refT*data = ref
143  // so no matter what, the difference curT*data - refT*data
144  // must be small, which is what we will test for.
145 
146  // Find the median absolute difference between curT*data and refT*data
147  Eigen::MatrixXf AbsDiff = (curT*data.features - refT*data.features).array().abs();
148  double median_diff = median_coeff(AbsDiff);
149 
150  // Find the median absolute value of curT*data
151  Eigen::MatrixXf Data = (curT*data.features).array().abs();
152  double median_data = median_coeff(Data);
153 
154  // Find the relative error
155  double rel_err = median_diff/median_data;
156 
157  // A relative error of 3% is probably acceptable.
158  EXPECT_LT(rel_err, 0.03) << "This error was caused by the test file:" << endl << " " << config_file;
159  }
160 }
161 
162 TEST(icpTest, icpSingular)
163 {
164  // Here we test point-to-plane ICP where the point clouds underdetermine transformation
165  // This situation requires special treatment in the algorithm.
166 
167  // create a x-y- planar grid point cloud in points
168  const size_t nX = 10, nY = nX;
169  Eigen::MatrixXf points(4, nX * nY);
170  const float d = 0.1;
171  const float oX = -(nX * d / 2), oY = -(nY * d / 2);
172 
173  for(size_t x = 0; x < nX; x++){
174  for(size_t y = 0; y < nY; y++){
175  points.col( x * nY + y) << d * x + oX, d * y + oY, 0, 1;
176  }
177  }
178 
179  DP pts0;
180  pts0.features = points;
181  DP pts1;
182  points.row(2).setOnes();
183  pts1.features = points; // pts1 is pts0 shifted by one in z-direction
184 
185  PM::ICP icp;
186  std::string config_file = dataPath + "default-identity.yaml";
187  EXPECT_TRUE(boost::filesystem::exists(config_file));
188 
189  std::ifstream ifs(config_file.c_str());
190  EXPECT_NO_THROW(icp.loadFromYaml(ifs)) << "This error was caused by the test file:" << endl << " " << config_file;
191 
192  // Compute ICP transform
193  PM::TransformationParameters curT = icp(pts0, pts1);
194 
195  PM::Matrix expectedT = PM::Matrix::Identity(4,4);
196  expectedT(2,3) = 1;
197  EXPECT_TRUE(expectedT.isApprox(curT)) << "Expecting pure translation in z-direction of unit distance." << endl;
198 }
199 
200 TEST(icpTest, icpIdentity)
201 {
202  // Here we test point-to-plane ICP where we expect the output transform to be
203  // the identity. This situation requires special treatment in the algorithm.
204  const float epsilon = 0.0001;
205 
206  DP pts0 = DP::load(dataPath + "cloud.00000.vtk");
207  DP pts1 = DP::load(dataPath + "cloud.00000.vtk");
208 
209  PM::ICP icp;
210  std::string config_file = dataPath + "default-identity.yaml";
211  EXPECT_TRUE(boost::filesystem::exists(config_file));
212 
213  std::ifstream ifs(config_file.c_str());
214  EXPECT_NO_THROW(icp.loadFromYaml(ifs)) << "This error was caused by the test file:" << endl << " " << config_file;
215 
216  // Compute current ICP transform
217  PM::TransformationParameters curT = icp(pts0, pts1);
218 
219  EXPECT_TRUE(curT.isApprox(PM::Matrix::Identity(4, 4), epsilon)) << "Expecting identity transform." << endl;
220 }
221 
222 TEST(icpTest, similarityTransform)
223 {
224  // Here we test similarity point-to-point ICP.
225 
226  DP pts0 = DP::load(dataPath + "car_cloud400.csv");
227  DP pts1 = DP::load(dataPath + "car_cloud400_scaled.csv");
228 
229  PM::ICP icp;
230  std::string config_file = dataPath + "icp_data/defaultSimilarityPointToPointMinDistDataPointsFilter.yaml";
231  EXPECT_TRUE(boost::filesystem::exists(config_file));
232 
233  std::ifstream ifs(config_file.c_str());
234  EXPECT_NO_THROW(icp.loadFromYaml(ifs)) << "This error was caused by the test file:" << endl << " " << config_file;
235 
236  // Compute current ICP transform
237  PM::TransformationParameters curT = icp(pts0, pts1);
238 
239  // We know the scale we're looking for is 1.04.
240  double scale = pow(curT.determinant(), 1.0/3.0);
241  EXPECT_LT( std::abs(scale - 1.04), 0.001)
242  << "Expecting the similarity transform scale to be 1.04.";
243 }
244 
245 TEST(icpTest, icpSequenceTest)
246 {
247  DP pts0 = DP::load(dataPath + "cloud.00000.vtk");
248  DP pts1 = DP::load(dataPath + "cloud.00001.vtk");
249  DP pts2 = DP::load(dataPath + "cloud.00002.vtk");
250 
251  PM::TransformationParameters Ticp = PM::Matrix::Identity(4,4);
252 
253  PM::ICPSequence icpSequence;
254 
255  std::ifstream ifs((dataPath + "default.yaml").c_str());
256  icpSequence.loadFromYaml(ifs);
257 
258  EXPECT_FALSE(icpSequence.hasMap());
259 
260  DP map = icpSequence.getPrefilteredInternalMap();
261  EXPECT_EQ(map.getNbPoints(), 0u);
262  EXPECT_EQ(map.getHomogeneousDim(), 0u);
263 
264  map = icpSequence.getPrefilteredMap();
265  EXPECT_EQ(map.getNbPoints(), 0u);
266  EXPECT_EQ(map.getHomogeneousDim(), 0u);
267 
268  icpSequence.setMap(pts0);
269  map = icpSequence.getPrefilteredInternalMap();
270  EXPECT_LE(map.getNbPoints(), pts0.getNbPoints());
271  EXPECT_GT(map.getNbPoints(), 0u);
273 
274  Ticp = icpSequence(pts1);
275  map = icpSequence.getPrefilteredMap();
276  EXPECT_LE(map.getNbPoints(), pts0.getNbPoints());
277  EXPECT_GT(map.getNbPoints(), 0u);
279 
280  Ticp = icpSequence(pts2);
281  map = icpSequence.getPrefilteredMap();
282  EXPECT_LE(map.getNbPoints(), pts0.getNbPoints());
283  EXPECT_GT(map.getNbPoints(), 0u);
285 
286  icpSequence.clearMap();
287  map = icpSequence.getPrefilteredInternalMap();
288  EXPECT_EQ(map.getNbPoints(), 0u);
289  EXPECT_EQ(map.getHomogeneousDim(), 0u);
290 }
291 
292 // Utility classes
293 class GenericTest: public IcpHelper
294 {
295 
296 public:
297 
298  // Will be called for every tests
299  virtual void SetUp()
300  {
301  icp.setDefault();
302  // Uncomment for consol outputs
303  //setLogger(PM::get().LoggerRegistrar.create("FileLogger"));
304  }
305 
306  // Will be called for every tests
307  virtual void TearDown()
308  {
309  }
310 };
311 
312 //---------------------------
313 // Generic tests
314 //---------------------------
315 
316 TEST_F(GenericTest, ICP_default)
317 {
318  validate2dTransformation();
319  validate3dTransformation();
320 }
321 
322 //---------------------------
323 // Main
324 //---------------------------
325 int main(int argc, char **argv)
326 {
327  dataPath = "";
328  for(int i=1; i < argc; i++)
329  {
330  if (strcmp(argv[i], "--path") == 0 && i+1 < argc)
331  dataPath = argv[i+1];
332  }
333 
334  if(dataPath == "")
335  {
336  cerr << "Missing the flag --path ./path/to/examples/data\n Please give the path to the test data folder which should be included with the source code. The folder is named 'examples/data'." << endl;
337  return -1;
338  }
339 
340  // Load point cloud for all test
341  ref2D = DP::load(dataPath + "2D_oneBox.csv");
342  data2D = DP::load(dataPath + "2D_twoBoxes.csv");
343  ref3D = DP::load(dataPath + "car_cloud400.csv");
344  data3D = DP::load(dataPath + "car_cloud401.csv");
345 
346  // Result of data express in ref (from visual inspection)
348  validT2d << 0.987498, 0.157629, 0.0859918,
349  -0.157629, 0.987498, 0.203247,
350  0, 0, 1;
351 
353  validT3d << 0.982304, 0.166685, -0.0854066, 0.0446816,
354  -0.150189, 0.973488, 0.172524, 0.191998,
355  0.111899, -0.156644, 0.981296, -0.0356313,
356  0, 0, 0, 1;
357 
358  testing::GTEST_FLAG(print_time) = true;
359  testing::InitGoogleTest(&argc, argv);
360  return RUN_ALL_TESTS();
361 }
362 
363 
364 
main
int main(int argc, char **argv)
Definition: utest.cpp:325
EXPECT_LE
#define EXPECT_LE(val1, val2)
Definition: gtest.h:19753
epsilon
double epsilon
GTEST_FLAG
#define GTEST_FLAG(name)
Definition: gtest.h:3016
TEST
TEST(icpTest, icpTest)
Definition: utest.cpp:81
ref3D
DP ref3D
Definition: utest.cpp:47
utest.h
validT2d
PM::TransformationParameters validT2d
Definition: utest.cpp:49
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
test.x
x
Definition: test.py:4
EXPECT_TRUE
#define EXPECT_TRUE(condition)
Definition: gtest.h:19327
PointMatcher::DataPoints::getHomogeneousDim
unsigned getHomogeneousDim() const
Return the dimension of the point cloud in homogeneous coordinates (one more than Euclidean dimension...
Definition: pointmatcher/DataPoints.cpp:172
testing::internal::string
::std::string string
Definition: gtest.h:1979
IcpHelper
Definition: utest.h:31
EXPECT_GT
#define EXPECT_GT(val1, val2)
Definition: gtest.h:19759
EXPECT_LT
#define EXPECT_LT(val1, val2)
Definition: gtest.h:19755
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
Definition: gtest.h:19747
EXPECT_NO_THROW
#define EXPECT_NO_THROW(statement)
Definition: gtest.h:19313
EXPECT_FALSE
#define EXPECT_FALSE(condition)
Definition: gtest.h:19330
data2D
DP data2D
Definition: utest.cpp:46
GenericTest::TearDown
virtual void TearDown()
Definition: utest.cpp:307
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
Definition: gtest.h:20057
testing::InitGoogleTest
void InitGoogleTest(int *argc, char **argv)
Definition: gtest-all.cc:6489
GenericTest
Definition: utest.cpp:293
d
d
PointMatcher< float >::Matrix
Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dense matrix over ScalarType.
Definition: PointMatcher.h:169
GenericTest::SetUp
virtual void SetUp()
Definition: utest.cpp:299
ref2D
DP ref2D
Definition: utest.cpp:45
PointMatcher::DataPoints::getNbPoints
unsigned getNbPoints() const
Return the number of points contained in the point cloud.
Definition: pointmatcher/DataPoints.cpp:158
dataPath
std::string dataPath
Definition: utest.cpp:43
validT3d
PM::TransformationParameters validT3d
Definition: utest.cpp:50
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
data3D
DP data3D
Definition: utest.cpp:48
std
PointMatcherSupport
Functions and classes that are not dependant on scalar type are defined in this namespace.
Definition: Bibliography.cpp:45
PointMatcher::DataPoints::load
static DataPoints load(const std::string &fileName)
Load a point cloud from a file, determine format from extension.
Definition: pointmatcher/IO.cpp:375
TEST_F
TEST_F(GenericTest, ICP_default)
Definition: utest.cpp:316
PointMatcherSupport::pow
constexpr T pow(const T base, const std::size_t exponent)
Definition: libpointmatcher/pointmatcher/DataPointsFilters/utils/utils.h:47
median_coeff
double median_coeff(Eigen::MatrixXf &A)
Definition: utest.cpp:75
PointMatcher< float >::TransformationParameters
Matrix TransformationParameters
A matrix holding the parameters a transformation.
Definition: PointMatcher.h:182


mp2p_icp
Author(s):
autogenerated on Wed Oct 23 2024 02:45:41