Point Cloud Library (PCL)  1.14.1-dev
pcl_tests.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  */
38 
39 #pragma once
40 
41 #include <Eigen/Core>
42 
43 /** \file pcl_tests.h
44  * Helper macros for testing equality of various data fields in PCL points */
45 
46 namespace pcl
47 {
48 
49  /** test_macros.h provide helper macros for testing vectors, matrices etc.
50  * We took some liberty with upcasing names to make them look like googletest
51  * macros names so that reader is not confused.
52  *
53  * This file also provides a family of googletest-style macros for asserting
54  * equality or nearness of xyz, normal, and rgba fields.
55  *
56  * \author Nizar Sallem, Sergey Alexandrov
57  */
58 
59  namespace test
60  {
61 
62  template <typename V1, typename V2>
63  void EXPECT_EQ_VECTORS (const V1& v1, const V2& v2)
64  {
65  SCOPED_TRACE("EXPECT_EQ_VECTORS failed");
66  EXPECT_EQ (v1.size (), v2.size ());
67  std::size_t length = std::min<std::size_t> (v1.size (), v2.size ());
68  for (std::size_t i = 0; i < length; ++i)
69  EXPECT_EQ (v1[i], v2[i]);
70  }
71 
72  template <typename V1, typename V2, typename Scalar>
73  void EXPECT_NEAR_VECTORS (const V1& v1, const V2& v2, const Scalar& epsilon)
74  {
75  SCOPED_TRACE("EXPECT_NEAR_VECTORS failed");
76  EXPECT_EQ (v1.size (), v2.size ());
77  std::size_t length = std::min<std::size_t> (v1.size (), v2.size());
78  for (std::size_t i = 0; i < length; ++i)
79  EXPECT_NEAR (v1[i], v2[i], epsilon);
80  }
81 
82  namespace internal
83  {
84 
85  template <typename Point1T, typename Point2T>
86  ::testing::AssertionResult XYZEQ (const char* expr1,
87  const char* expr2,
88  const Point1T& p1,
89  const Point2T& p2)
90  {
91  if ((p1).getVector3fMap ().cwiseEqual ((p2).getVector3fMap ()).all ())
92  return ::testing::AssertionSuccess ();
93  return ::testing::AssertionFailure ()
94  << "Value of: " << expr2 << ".getVector3fMap ()" << std::endl
95  << " Actual: " << p2.getVector3fMap ().transpose () << std::endl
96  << "Expected: " << expr1 << ".getVector3fMap ()" << std::endl
97  << "Which is: " << p1.getVector3fMap ().transpose ();
98  }
99 
100  template <typename Point1T, typename Point2T>
101  ::testing::AssertionResult XYZNear (const char* expr1,
102  const char* expr2,
103  const char* abs_error_expr,
104  const Point1T& p1,
105  const Point2T& p2,
106  double abs_error)
107  {
108  const Eigen::Vector3f diff = ((p1).getVector3fMap () -
109  (p2).getVector3fMap ()).cwiseAbs ();
110  if ((diff.array () < abs_error).all ())
111  return ::testing::AssertionSuccess ();
112  return ::testing::AssertionFailure ()
113  << "Some of the element-wise differences exceed " << abs_error_expr
114  << " (which evaluates to " << abs_error << ")" << std::endl
115  << "Difference: " << diff.transpose () << std::endl
116  << " Value of: " << expr2 << ".getVector3fMap ()" << std::endl
117  << " Actual: " << p2.getVector3fMap ().transpose () << std::endl
118  << " Expected: " << expr1 << ".getVector3fMap ()" << std::endl
119  << " Which is: " << p1.getVector3fMap ().transpose ();
120  }
121 
122  template <typename Point1T, typename Point2T>
123  ::testing::AssertionResult NormalEQ (const char* expr1,
124  const char* expr2,
125  const Point1T& p1,
126  const Point2T& p2)
127  {
128  if ((p1).getNormalVector3fMap ().cwiseEqual ((p2).getNormalVector3fMap ()).all ())
129  return ::testing::AssertionSuccess ();
130  return ::testing::AssertionFailure ()
131  << "Value of: " << expr2 << ".getNormalVector3fMap ()" << std::endl
132  << " Actual: " << p2.getNormalVector3fMap ().transpose () << std::endl
133  << "Expected: " << expr1 << ".getNormalVector3fMap ()" << std::endl
134  << "Which is: " << p1.getNormalVector3fMap ().transpose ();
135  }
136 
137  template <typename Point1T, typename Point2T>
138  ::testing::AssertionResult NormalNear (const char* expr1,
139  const char* expr2,
140  const char* abs_error_expr,
141  const Point1T& p1,
142  const Point2T& p2,
143  double abs_error)
144  {
145  const Eigen::Vector3f diff = ((p1).getNormalVector3fMap () -
146  (p2).getNormalVector3fMap ()).cwiseAbs ();
147  if ((diff.array () < abs_error).all ())
148  return ::testing::AssertionSuccess ();
149  return ::testing::AssertionFailure ()
150  << "Some of the element-wise differences exceed " << abs_error_expr
151  << " (which evaluates to " << abs_error << ")" << std::endl
152  << "Difference: " << diff.transpose () << std::endl
153  << " Value of: " << expr2 << ".getNormalVector3fMap ()" << std::endl
154  << " Actual: " << p2.getNormalVector3fMap ().transpose () << std::endl
155  << " Expected: " << expr1 << ".getNormalVector3fMap ()" << std::endl
156  << " Which is: " << p1.getNormalVector3fMap ().transpose ();
157  }
158 
159  template <typename Point1T, typename Point2T>
160  ::testing::AssertionResult RGBEQ (const char* expr1,
161  const char* expr2,
162  const Point1T& p1,
163  const Point2T& p2)
164  {
165  if ((p1).getRGBVector3i ().cwiseEqual ((p2).getRGBVector3i ()).all ())
166  return ::testing::AssertionSuccess ();
167  return ::testing::AssertionFailure ()
168  << "Value of: " << expr2 << ".getRGBVector3i ()" << std::endl
169  << " Actual: " << p2.getRGBVector3i ().transpose () << std::endl
170  << "Expected: " << expr1 << ".getRGBVector3i ()" << std::endl
171  << "Which is: " << p1.getRGBVector3i ().transpose ();
172  }
173 
174  template <typename Point1T, typename Point2T>
175  ::testing::AssertionResult RGBAEQ (const char* expr1,
176  const char* expr2,
177  const Point1T& p1,
178  const Point2T& p2)
179  {
180  if ((p1).getRGBAVector4i ().cwiseEqual ((p2).getRGBAVector4i ()).all ())
181  return ::testing::AssertionSuccess ();
182  return ::testing::AssertionFailure ()
183  << "Value of: " << expr2 << ".getRGBAVector4i ()" << std::endl
184  << " Actual: " << p2.getRGBAVector4i ().transpose () << std::endl
185  << "Expected: " << expr1 << ".getRGBAVector4i ()" << std::endl
186  << "Which is: " << p1.getRGBAVector4i ().transpose ();
187  }
188 
189  template <typename PointCloud1T, typename PointCloud2T>
190  ::testing::AssertionResult MetaDataEQ (const char* expr1,
191  const char* expr2,
192  const PointCloud1T& p1,
193  const PointCloud2T& p2)
194  {
195  if (!(p1.header == p2.header))
196  return ::testing::AssertionFailure () << "Headers are different";
197  if (p1.width != p2.width)
198  return ::testing::AssertionFailure ()
199  << "Value of: " << expr2 << ".width" << std::endl
200  << " Actual: " << p2.width << std::endl
201  << "Expected: " << expr1 << ".width" << std::endl
202  << "Which is: " << p1.width << std::endl;
203  if (p1.height != p2.height)
204  return ::testing::AssertionFailure ()
205  << "Value of: " << expr2 << ".height" << std::endl
206  << " Actual: " << p2.height << std::endl
207  << "Expected: " << expr1 << ".height" << std::endl
208  << "Which is: " << p1.height << std::endl;
209  if (p1.is_dense != p2.is_dense)
210  return ::testing::AssertionFailure ()
211  << "Value of: " << expr2 << ".is_dense" << std::endl
212  << " Actual: " << p2.is_dense << std::endl
213  << "Expected: " << expr1 << ".is_dense" << std::endl
214  << "Which is: " << p1.is_dense << std::endl;
215  if (p1.sensor_origin_ != p2.sensor_origin_)
216  return ::testing::AssertionFailure () << "Sensor origins are different";
217  if (p1.sensor_orientation_.coeffs () != p2.sensor_orientation_.coeffs ())
218  return ::testing::AssertionFailure () << "Sensor orientations are different";
219  return ::testing::AssertionSuccess ();
220  }
221 
222 
223  template<typename V>
224  ::testing::AssertionResult VectorContainsAll(const char* expr1, const char* expr2, const std::vector<V>& elements, const std::vector<V>& v) {
225  for(const V& item : elements) {
226  if(std::find(v.cbegin(), v.cend(), item)==v.cend()) {
227 
228  std::ostringstream vec_rep;
229  std::copy(v.cbegin(), v.cend()-1, std::ostream_iterator<V>(vec_rep, ", "));
230  vec_rep<<v.back();
231 
232  std::ostringstream elements_rep;
233  std::copy(elements.cbegin(), elements.cend()-1, std::ostream_iterator<V>(elements_rep, ", "));
234  elements_rep << elements.back();
235 
236  return ::testing::AssertionFailure ()
237  << "Actual : " << expr2 << std::endl
238  << "contains : " << vec_rep.str() << std::endl
239  << "Target set : " << expr1 << std::endl
240  << "contains : " << elements_rep.str() << std::endl;
241  }
242  }
243  return ::testing::AssertionSuccess ();
244  }
245 
246  template<typename V>
247  ::testing::AssertionResult VectorDoesNotContain(const char* expr1, const char* expr2, const std::vector<V>& elements, const std::vector<V>& v) {
248  for(const V& item : elements) {
249  if(std::find(v.cbegin(), v.cend(), item)!=v.cend()) {
250  std::ostringstream vec_rep;
251  std::copy(v.cbegin(), v.cend()-1, std::ostream_iterator<V>(vec_rep, ", "));
252  vec_rep<<v.back();
253 
254  std::ostringstream elements_rep;
255  std::copy(elements.cbegin(), elements.cend()-1, std::ostream_iterator<V>(elements_rep, ", "));
256  elements_rep << elements.back();
257 
258  return ::testing::AssertionFailure ()
259  << "Actual : " << expr2 << std::endl
260  << "contains : " << vec_rep.str() << std::endl
261  << "Forbidden set: " << expr1 << std::endl
262  << "contains : " << elements_rep.str() << std::endl;
263  }
264  }
265  return ::testing::AssertionSuccess ();
266  }
267 
268  }
269 
270  }
271 
272 }
273 
274 /// Expect that each of x, y, and z fields are equal in
275 /// two points.
276 #define EXPECT_XYZ_EQ(expected, actual) \
277  EXPECT_PRED_FORMAT2(::pcl::test::internal::XYZEQ, \
278  (expected), (actual))
279 
280 /// Assert that each of x, y, and z fields are equal in
281 /// two points.
282 #define ASSERT_XYZ_EQ(expected, actual) \
283  ASSERT_PRED_FORMAT2(::pcl::test::internal::XYZEQ, \
284  (expected), (actual))
285 
286 /// Expect that differences between x, y, and z fields in
287 /// two points are each within abs_error.
288 #define EXPECT_XYZ_NEAR(expected, actual, abs_error) \
289  EXPECT_PRED_FORMAT3(::pcl::test::internal::XYZNear, \
290  (expected), (actual), abs_error)
291 
292 /// Assert that differences between x, y, and z fields in
293 /// two points are each within abs_error.
294 #define ASSERT_XYZ_NEAR(expected, actual, abs_error) \
295  ASSERT_PRED_FORMAT3(::pcl::test::internal::XYZNear, \
296  (expected), (actual), abs_error)
297 
298 /// Expect that each of normal_x, normal_y, and normal_z
299 /// fields are equal in two points.
300 #define EXPECT_NORMAL_EQ(expected, actual) \
301  EXPECT_PRED_FORMAT2(::pcl::test::internal::NormalEQ, \
302  (expected), (actual))
303 
304 /// Assert that each of normal_x, normal_y, and normal_z
305 /// fields are equal in two points.
306 #define ASSERT_NORMAL_EQ(expected, actual) \
307  ASSERT_PRED_FORMAT2(::pcl::test::internal::NormalEQ, \
308  (expected), (actual))
309 
310 /// Expect that differences between normal_x, normal_y,
311 /// and normal_z fields in two points are each within
312 /// abs_error.
313 #define EXPECT_NORMAL_NEAR(expected, actual, abs_error) \
314  EXPECT_PRED_FORMAT3(::pcl::test::internal::NormalNear, \
315  (expected), (actual), abs_error)
316 
317 /// Assert that differences between normal_x, normal_y,
318 /// and normal_z fields in two points are each within
319 /// abs_error.
320 #define ASSERT_NORMAL_NEAR(expected, actual, abs_error) \
321  ASSERT_PRED_FORMAT3(::pcl::test::internal::NormalNear, \
322  (expected), (actual), abs_error)
323 
324 /// Expect that each of r, g, and b fields are equal in
325 /// two points.
326 #define EXPECT_RGB_EQ(expected, actual) \
327  EXPECT_PRED_FORMAT2(::pcl::test::internal::RGBEQ, \
328  (expected), (actual))
329 
330 /// Assert that each of r, g, and b fields are equal in
331 /// two points.
332 #define ASSERT_RGB_EQ(expected, actual) \
333  ASSERT_PRED_FORMAT2(::pcl::test::internal::RGBEQ, \
334  (expected), (actual))
335 
336 /// Expect that each of r, g, b, and a fields are equal
337 /// in two points.
338 #define EXPECT_RGBA_EQ(expected, actual) \
339  EXPECT_PRED_FORMAT2(::pcl::test::internal::RGBAEQ, \
340  (expected), (actual))
341 
342 /// Assert that each of r, g, b, and a fields are equal
343 /// in two points.
344 #define ASSERT_RGBA_EQ(expected, actual) \
345  ASSERT_PRED_FORMAT2(::pcl::test::internal::RGBAEQ, \
346  (expected), (actual))
347 
348 /// Assert that the metadata (header, width, height,
349 /// is_dense, sensor origin and orientation) are equal
350 /// in two point clouds.
351 #define ASSERT_METADATA_EQ(expected, actual) \
352  ASSERT_PRED_FORMAT2(::pcl::test::internal::MetaDataEQ, \
353  expected, actual)
354 
355 /// Expect that the metadata (header, width, height,
356 /// is_dense, sensor origin and orientation) are equal
357 /// in two point clouds.
358 #define EXPECT_METADATA_EQ(expected, actual) \
359  EXPECT_PRED_FORMAT2(::pcl::test::internal::MetaDataEQ, \
360  expected, actual)
361 
362 
363 /// Expect that the vector contains all elements
364 /// from the expected vector.
365 #define EXPECT_VECTOR_CONTAINS_ALL(expected, actual) \
366  EXPECT_PRED_FORMAT2(::pcl::test::internal::VectorContainsAll, \
367  expected, actual)
368 
369 
370 /// Expect that the vector does not contain any element
371 /// from the expected vector.
372 #define EXPECT_VECTOR_DOES_NOT_CONTAIN(expected, actual) \
373  EXPECT_PRED_FORMAT2(::pcl::test::internal::VectorDoesNotContain, \
374  expected, actual)
375 
376 
377 
378 /// Assert that the vector contains all elements
379 /// from the expected vector.
380 #define ASSERT_VECTOR_CONTAINS_ALL(expected, actual) \
381  ASSERT_PRED_FORMAT2(::pcl::test::internal::VectorContainsAll, \
382  expected, actual)
383 
384 
385 /// Assert that the vector does not contain any element
386 /// from the expected vector.
387 #define ASSERT_VECTOR_DOES_NOT_CONTAIN(expected, actual) \
388  ASSERT_PRED_FORMAT2(::pcl::test::internal::VectorDoesNotContain, \
389  expected, actual)
390 
391 
392 
::testing::AssertionResult RGBAEQ(const char *expr1, const char *expr2, const Point1T &p1, const Point2T &p2)
Definition: pcl_tests.h:175
::testing::AssertionResult MetaDataEQ(const char *expr1, const char *expr2, const PointCloud1T &p1, const PointCloud2T &p2)
Definition: pcl_tests.h:190
::testing::AssertionResult VectorContainsAll(const char *expr1, const char *expr2, const std::vector< V > &elements, const std::vector< V > &v)
Definition: pcl_tests.h:224
::testing::AssertionResult RGBEQ(const char *expr1, const char *expr2, const Point1T &p1, const Point2T &p2)
Definition: pcl_tests.h:160
::testing::AssertionResult XYZNear(const char *expr1, const char *expr2, const char *abs_error_expr, const Point1T &p1, const Point2T &p2, double abs_error)
Definition: pcl_tests.h:101
::testing::AssertionResult XYZEQ(const char *expr1, const char *expr2, const Point1T &p1, const Point2T &p2)
Definition: pcl_tests.h:86
::testing::AssertionResult VectorDoesNotContain(const char *expr1, const char *expr2, const std::vector< V > &elements, const std::vector< V > &v)
Definition: pcl_tests.h:247
::testing::AssertionResult NormalNear(const char *expr1, const char *expr2, const char *abs_error_expr, const Point1T &p1, const Point2T &p2, double abs_error)
Definition: pcl_tests.h:138
::testing::AssertionResult NormalEQ(const char *expr1, const char *expr2, const Point1T &p1, const Point2T &p2)
Definition: pcl_tests.h:123
void EXPECT_NEAR_VECTORS(const V1 &v1, const V2 &v2, const Scalar &epsilon)
Definition: pcl_tests.h:73
void EXPECT_EQ_VECTORS(const V1 &v1, const V2 &v2)
Definition: pcl_tests.h:63