GeneralTests.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 //---------------------------
42 // Test ICP with all existing filters.
43 
44 // Algorithm:
45 // 1. Iterate over all yaml files in
46 // libpointmatcher/examples/data/icp_data, each file tests ICP
47 // with one or more filters.
48 // 2. Run ICP with the given yaml file. The filters in the yaml
49 // file are applied along the way.
50 // 3. Write the obtained ICP transform to disk, to the same directory,
51 // with file extension .cur_trans (for easy future comparisons).
52 // 4. Load the reference (known as correct) ICP transform from disk,
53 // from the same directory, with file extension .ref_trans.
54 // 5. See if the current and reference transforms are equal.
55 
56 // To update an existing test or add a new test, simply add/modify
57 // the desired yaml file, run the tests (they may fail this time), then
58 // copy the (just written) desired current transform file on top of the
59 // corresponding reference transform file. Run the tests again. This
60 // time they will succeed.
61 //---------------------------
62 
63 // Find the median coefficient of a matrix
64 double median_coeff(Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic>& A){
65  Eigen::Map<Eigen::Matrix<NumericType, 1, Eigen::Dynamic>> v(A.data(),A.size());
66  std::sort(v.data(), v.data() + v.size());
67  return v[v.size()/2];
68 }
69 
70 TEST(icpTest, icpTest)
71 {
72  DP ref = DP::load(dataPath + "cloud.00000.vtk");
73  DP data = DP::load(dataPath + "cloud.00001.vtk");
74 
75  namespace fs = boost::filesystem;
76  fs::path config_dir(dataPath + "icp_data");
77  EXPECT_TRUE( fs::exists(config_dir) && fs::is_directory(config_dir) );
78 
79  fs::directory_iterator end_iter;
80  for( fs::directory_iterator d(config_dir); d != end_iter; ++d)
81  {
82  if (!fs::is_regular_file(d->status()) ) continue;
83 
84  std::cout << "Testing file " << d->path().string() << std::endl;
85  // Load config file, and form ICP object
86  PM::ICP icp;
87  std::string config_file = d->path().string();
88  if (fs::extension(config_file) != ".yaml") continue;
89  std::ifstream ifs(config_file.c_str());
90  EXPECT_NO_THROW(icp.loadFromYaml(ifs)) << "This error was caused by the test file:" << endl << " " << config_file;
91 
92  // Compute current ICP transform
94 
95  // Write current transform to disk (to easily compare it
96  // with reference transform offline)
97  fs::path cur_file = d->path();
98  cur_file.replace_extension(".cur_trans");
99  //std::cout << "Writing: " << cur_file << std::endl;
100  std::ofstream otfs(cur_file.c_str());
101  otfs.precision(16);
102  otfs << curT;
103  otfs.close();
104 
105  // Load reference transform
106  fs::path ref_file = d->path();
107  ref_file.replace_extension(".ref_trans");
108  PM::TransformationParameters refT = 0*curT;
109  //std::cout << "Reading: " << ref_file << std::endl;
110  std::ifstream itfs(ref_file.c_str());
111  EXPECT_TRUE(itfs.good()) << "Could not find " << ref_file
112  << ". If this is the first time this test is run, "
113  << "create it as a copy of " << cur_file;
114 
115  for (int row = 0; row < refT.cols(); row++)
116  {
117  for (int col = 0; col < refT.cols(); col++)
118  {
119  itfs >>refT(row, col);
120  }
121  }
122 
123  // Dump the reference transform and current one
124  //std::cout.precision(17);
125  //std::cout << "refT:\n" << refT << std::endl;
126  //std::cout << "curT:\n" << curT << std::endl;
127 
128  // We need to compare the stored icp transform vs the computed one.
129  // Since the icp solution is not unique, they may differ a lot.
130  // Yet, the point of icp is
131  // curT*data = ref, and refT*data = ref
132  // so no matter what, the difference curT*data - refT*data
133  // must be small, which is what we will test for.
134 
135  // Find the median absolute difference between curT*data and refT*data
136  Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic> AbsDiff = (curT*data.features - refT*data.features).array().abs();
137  double median_diff = median_coeff(AbsDiff);
138 
139  // Find the median absolute value of curT*data
140  Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic> Data = (curT*data.features).array().abs();
141  double median_data = median_coeff(Data);
142 
143  // Find the relative error
144  double rel_err = median_diff/median_data;
145 
146  // A relative error of 3% is probably acceptable.
147  // FIXME(ynava) Original value of 3% was replaced by 5% due to randomly failing unit test.
148  EXPECT_LT(rel_err, 0.05) << "This error was caused by the test file:" << endl << " " << config_file;
149  }
150 }
151 
152 TEST(icpTest, icpSingular)
153 {
154  // Here we test point-to-plane ICP where the point clouds underdetermine transformation
155  // This situation requires special treatment in the algorithm.
156 
157  // create a x-y- planar grid point cloud in points
158  const size_t nX = 10, nY = nX;
159  Eigen::Matrix<NumericType, Eigen::Dynamic, Eigen::Dynamic> points(4, nX * nY);
160  const NumericType d = 0.1;
161  const NumericType oX = -(nX * d / 2), oY = -(nY * d / 2);
162 
163  for(size_t x = 0; x < nX; x++){
164  for(size_t y = 0; y < nY; y++){
165  points.col( x * nY + y) << d * x + oX, d * y + oY, 0, 1;
166  }
167  }
168 
169  DP pts0;
170  pts0.features = points;
171  DP pts1;
172  points.row(2).setOnes();
173  pts1.features = points; // pts1 is pts0 shifted by one in z-direction
174 
175  PM::ICP icp;
176  std::string config_file = dataPath + "default-identity.yaml";
177  EXPECT_TRUE(boost::filesystem::exists(config_file));
178 
179  std::ifstream ifs(config_file.c_str());
180  EXPECT_NO_THROW(icp.loadFromYaml(ifs)) << "This error was caused by the test file:" << endl << " " << config_file;
181 
182  // Compute ICP transform
183  PM::TransformationParameters curT = icp(pts0, pts1);
184 
185  PM::Matrix expectedT = PM::Matrix::Identity(4,4);
186  expectedT(2,3) = 1;
187  EXPECT_TRUE(expectedT.isApprox(curT)) << "Expecting pure translation in z-direction of unit distance." << endl;
188 }
189 
190 TEST(icpTest, icpIdentity)
191 {
192  // Here we test point-to-plane ICP where we expect the output transform to be
193  // the identity. This situation requires special treatment in the algorithm.
194  const NumericType epsilon = 0.0001;
195 
196  DP pts0 = DP::load(dataPath + "cloud.00000.vtk");
197  DP pts1 = DP::load(dataPath + "cloud.00000.vtk");
198 
199  PM::ICP icp;
200  std::string config_file = dataPath + "default-identity.yaml";
201  EXPECT_TRUE(boost::filesystem::exists(config_file));
202 
203  std::ifstream ifs(config_file.c_str());
204  EXPECT_NO_THROW(icp.loadFromYaml(ifs)) << "This error was caused by the test file:" << endl << " " << config_file;
205 
206  // Compute current ICP transform
207  PM::TransformationParameters curT = icp(pts0, pts1);
208 
209  EXPECT_TRUE(curT.isApprox(PM::Matrix::Identity(4, 4), epsilon)) << "Expecting identity transform." << endl;
210 }
211 
212 TEST(icpTest, similarityTransform)
213 {
214  // Here we test similarity point-to-point ICP.
215 
216  DP pts0 = DP::load(dataPath + "car_cloud400.csv");
217  DP pts1 = DP::load(dataPath + "car_cloud400_scaled.csv");
218 
219  PM::ICP icp;
220  std::string config_file = dataPath + "icp_data/defaultSimilarityPointToPointMinDistDataPointsFilter.yaml";
221  EXPECT_TRUE(boost::filesystem::exists(config_file));
222 
223  std::ifstream ifs(config_file.c_str());
224  EXPECT_NO_THROW(icp.loadFromYaml(ifs)) << "This error was caused by the test file:" << endl << " " << config_file;
225 
226  // Compute current ICP transform
227  PM::TransformationParameters curT = icp(pts0, pts1);
228 
229  // We know the scale we're looking for is 1.04.
230  double scale = pow(curT.determinant(), 1.0/3.0);
231  EXPECT_LT( std::abs(scale - 1.04), 0.001)
232  << "Expecting the similarity transform scale to be 1.04.";
233 }
234 
235 TEST(icpTest, icpSequenceTest)
236 {
237  DP pts0 = DP::load(dataPath + "cloud.00000.vtk");
238  DP pts1 = DP::load(dataPath + "cloud.00001.vtk");
239  DP pts2 = DP::load(dataPath + "cloud.00002.vtk");
240 
241  PM::TransformationParameters Ticp = PM::Matrix::Identity(4,4);
242 
243  PM::ICPSequence icpSequence;
244 
245  std::ifstream ifs((dataPath + "default.yaml").c_str());
246  icpSequence.loadFromYaml(ifs);
247 
248  EXPECT_FALSE(icpSequence.hasMap());
249 
250  DP map = icpSequence.getPrefilteredInternalMap();
251  EXPECT_EQ(map.getNbPoints(), 0u);
252  EXPECT_EQ(map.getHomogeneousDim(), 0u);
253 
254  map = icpSequence.getPrefilteredMap();
255  EXPECT_EQ(map.getNbPoints(), 0u);
256  EXPECT_EQ(map.getHomogeneousDim(), 0u);
257 
258  icpSequence.setMap(pts0);
259  map = icpSequence.getPrefilteredInternalMap();
260  EXPECT_LE(map.getNbPoints(), pts0.getNbPoints());
261  EXPECT_GT(map.getNbPoints(), 0u);
263 
264  Ticp = icpSequence(pts1);
265  map = icpSequence.getPrefilteredMap();
266  EXPECT_LE(map.getNbPoints(), pts0.getNbPoints());
267  EXPECT_GT(map.getNbPoints(), 0u);
269 
270  Ticp = icpSequence(pts2);
271  map = icpSequence.getPrefilteredMap();
272  EXPECT_LE(map.getNbPoints(), pts0.getNbPoints());
273  EXPECT_GT(map.getNbPoints(), 0u);
275 
276  icpSequence.clearMap();
277  map = icpSequence.getPrefilteredInternalMap();
278  EXPECT_EQ(map.getNbPoints(), 0u);
279  EXPECT_EQ(map.getHomogeneousDim(), 0u);
280 }
281 
282 // Utility classes
283 class GenericTest: public IcpHelper
284 {
285 
286 public:
287 
288  // Will be called for every tests
289  virtual void SetUp()
290  {
291  icp.setDefault();
292  // Uncomment for consol outputs
293  //setLogger(PM::get().LoggerRegistrar.create("FileLogger"));
294  }
295 
296  // Will be called for every tests
297  virtual void TearDown()
298  {
299  }
300 };
301 
302 //---------------------------
303 // Generic tests
304 //---------------------------
305 
306 TEST_F(GenericTest, ICP_default)
307 {
308  validate2dTransformation();
309  validate3dTransformation();
310 }
EXPECT_LE
#define EXPECT_LE(val1, val2)
Definition: gtest.h:19753
ICPSequence
PM::ICPSequence ICPSequence
Definition: pypoint_matcher_helper.h:34
median_coeff
double median_coeff(Eigen::Matrix< NumericType, Eigen::Dynamic, Eigen::Dynamic > &A)
Definition: GeneralTests.cpp:64
TEST
TEST(icpTest, icpTest)
Definition: GeneralTests.cpp:70
align_sequence.icp
icp
Definition: align_sequence.py:40
TEST_F
TEST_F(GenericTest, ICP_default)
Definition: GeneralTests.cpp:306
PointMatcher::DataPoints
A point cloud.
Definition: PointMatcher.h:207
x
x
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:173
testing::internal::string
::std::string string
Definition: gtest.h:1979
IcpHelper
Definition: utest.h:32
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
GenericTest::TearDown
virtual void TearDown()
Definition: GeneralTests.cpp:297
GenericTest
Definition: GeneralTests.cpp:283
icp.data
data
Definition: icp.py:50
PointMatcher< float >::Matrix
Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic > Matrix
A dense matrix over ScalarType.
Definition: PointMatcher.h:169
ICP
PM::ICP ICP
Definition: pypoint_matcher_helper.h:33
GenericTest::SetUp
virtual void SetUp()
Definition: GeneralTests.cpp:289
align_sequence.config_file
string config_file
Definition: align_sequence.py:43
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
PointMatcher::DataPoints::features
Matrix features
features of points in the cloud
Definition: PointMatcher.h:331
icp.ref
ref
Definition: icp.py:49
NumericType
double NumericType
Definition: utest.h:15
std
icp
Definition: icp.py:1
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
PointMatcherSupport::pow
constexpr T pow(const T base, const std::size_t exponent)
Definition: utils.h:47
PointMatcher< float >::TransformationParameters
Matrix TransformationParameters
A matrix holding the parameters a transformation.
Definition: PointMatcher.h:182


libpointmatcher
Author(s):
autogenerated on Mon Sep 16 2024 02:24:08