utest/ui/PointCloudGenerator.cpp
Go to the documentation of this file.
1 
2 #include "../utest.h"
3 
5 {
6 public:
7  PointCloudGeneratorTest() = default;
8 
9  /* Setup methods */
11  {
13  orientation_ = PM::Quaternion(0.0f, 0.2f, 5.0f, 1.0f);
14  orientation_.normalize();
15 
16  numberOfPoints_ = 10000;
17  }
18 
19  // Parameters.
20  PM::PointCloudGenerator::StaticCoordVector translation_{ PM::PointCloudGenerator::StaticCoordVector::Zero() };
21  PM::Quaternion orientation_{ PM::Quaternion::Identity() };
22  PM::DataPoints::Index numberOfPoints_{ 0 };
23 
24  // Error tolerance.
26 };
27 
28 // This test validates that the function that builds up transformations to point clouds is correct. Considers pure translation
29 TEST_F(PointCloudGeneratorTest, BuildUpTransformationTranslationOnly)
30 { // NOLINT
31  const PM::PointCloudGenerator::StaticCoordVector translation{ 1.0f, 0.5f, -50.212312f };
32  const PM::Quaternion orientation{ 0.0f, 0.0f, 0.0f, 1.0f };
33 
34  // Build up transformation.
35  const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation, orientation) };
36 
37  // Assertions on results.
38  ASSERT_EQ(transformation.translation(), translation);
39  ASSERT_TRUE(transformation.linear().isApprox(orientation.normalized().toRotationMatrix()));
40 }
41 
42 // This test validates that the function that builds up transformations to point clouds is correct. Considers pure rotation.
43 TEST_F(PointCloudGeneratorTest, BuildUpTransformationRotationOnly)
44 { // NOLINT
46  const PM::Quaternion orientation{ 0.123123f, 0.9576f, -42.232193f, 0.00001f };
47 
48  // Build up transformation.
49  const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation, orientation) };
50 
51  // Assertions on results.
52  ASSERT_EQ(transformation.translation(), translation);
53  ASSERT_TRUE(transformation.linear().isApprox(orientation.normalized().toRotationMatrix()));
54 }
55 
56 // This test validates that the function that builds up transformations to point clouds is correct. Considers translation+rotation.
57 TEST_F(PointCloudGeneratorTest, BuildUpTransformationTranslationRotation)
58 { // NOLINT
59  const PM::PointCloudGenerator::StaticCoordVector translation{ 1.0f, 0.5f, -50.212312f };
60  const PM::Quaternion orientation{ 0.123123f, 0.9576f, -42.232193f, 0.00001f };
61 
62  // Build up transformation.
63  const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation, orientation) };
64 
65  // Assertions on results.
66  ASSERT_EQ(transformation.translation(), translation);
67  ASSERT_TRUE(transformation.linear().isApprox(orientation.normalized().toRotationMatrix()));
68 }
69 
70 // This test validates that the function that creates empty 3D point clouds is correct.
71 TEST_F(PointCloudGeneratorTest, AddEmpty3dPointFields)
72 { // NOLINT
73  PM::DataPoints pointCloud;
74  PM::PointCloudGenerator::addEmpty3dPointFields(numberOfPoints_, pointCloud);
75 
76  // Assertions on results.
77  // Number of points.
78  ASSERT_EQ(pointCloud.getNbPoints(), numberOfPoints_);
79  // Feature labels.
80  ASSERT_TRUE(pointCloud.featureExists(std::string("x")));
81  ASSERT_TRUE(pointCloud.featureExists(std::string("y")));
82  ASSERT_TRUE(pointCloud.featureExists(std::string("z")));
83  ASSERT_TRUE(pointCloud.featureLabels.size());
84 }
85 
86 // This test validates that the function that applies transformations to point clouds is correct.
87 TEST_F(PointCloudGeneratorTest, ApplyTransformation)
88 { // NOLINT
89  // Test points.
90  const PM::DataPoints::Index numberOfPoints{ 2 };
91  const PM::PointCloudGenerator::StaticCoordVector point1{ 0.0f, 0.0f, 0.0f };
92  const PM::PointCloudGenerator::StaticCoordVector point2{ 2.1213f, -100000.0f, -23459999.2342312370987978687f };
93  // First point is at the origin, second is somewhere else.
94 
95  // Point cloud.
96  PM::DataPoints pointCloud;
97  PM::PointCloudGenerator::addEmpty3dPointFields(numberOfPoints, pointCloud);
98  pointCloud.features(0, 0) = point1(0);
99  pointCloud.features(1, 0) = point1(1);
100  pointCloud.features(2, 0) = point1(2);
101  pointCloud.features(0, 1) = point2(0);
102  pointCloud.features(1, 1) = point2(1);
103  pointCloud.features(2, 1) = point2(2);
104 
105  // Build up transformation.
106  const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) };
107 
108  // Transform point cloud and points.
109  PM::PointCloudGenerator::applyTransformation(translation_, orientation_, pointCloud);
110  const PM::PointCloudGenerator::StaticCoordVector transformedPoint1{ transformation * point1 };
111  const PM::PointCloudGenerator::StaticCoordVector transformedPoint2{ transformation * point2 };
112 
113  // Assertions on results.
114  // Number of points.
115  ASSERT_EQ(pointCloud.getNbPoints(), numberOfPoints);
116  // Value of transformed points.
117  ASSERT_EQ(pointCloud.features(0, 0), transformedPoint1(0));
118  ASSERT_EQ(pointCloud.features(1, 0), transformedPoint1(1));
119  ASSERT_EQ(pointCloud.features(2, 0), transformedPoint1(2));
120  ASSERT_EQ(pointCloud.features(0, 1), transformedPoint2(0));
121  ASSERT_EQ(pointCloud.features(1, 1), transformedPoint2(1));
122  ASSERT_EQ(pointCloud.features(2, 1), transformedPoint2(2));
123 }
124 
125 // This test validates the construction of base shape attributes through a derived class.
127 { // NOLINT
128  setDefaultParameters();
129 
130  // Dimensions of the sphere.
131  const PM::ScalarType radius{ 1 };
132 
133  // Generate point cloud.
135  radius, numberOfPoints_, translation_, orientation_) };
136 
137  // Assertions on results.
138  // Number of points.
139  ASSERT_EQ(pointCloud.getNbPoints(), numberOfPoints_);
140  // Points correspond to volume.
141  const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) };
142  bool isSphere{ true };
143  const PM::ScalarType expectedRadiusSquared{ radius * radius };
144  for (PM::DataPoints::Index i{ 0 }; i < pointCloud.features.cols() && isSphere; ++i)
145  {
146  // Fetch point and remove transformation offset.
147  const PM::PointCloudGenerator::StaticCoordVector point(pointCloud.features(0), pointCloud.features(1), pointCloud.features(2));
148  const PM::PointCloudGenerator::StaticCoordVector centeredPoint{ transformation.inverse() * point };
149 
150  // Check whether the point lies inside the volume.
151  const PM::ScalarType computedRadiusSquared{ centeredPoint(0) * centeredPoint(0) + centeredPoint(1) * centeredPoint(1)
152  + centeredPoint(2) * centeredPoint(2) };
153  // For the point to belong to a sphere, its radius from the center must be within the expected margin.
154  if (computedRadiusSquared > expectedRadiusSquared + kEpsilonError_)
155  {
156  isSphere = false;
157  }
158  }
159  ASSERT_TRUE(isSphere);
160 }
161 
163 { // NOLINT
164  setDefaultParameters();
165 
166  // Dimensions of the cylinder.
167  const PM::ScalarType radius{ 1 };
168  const PM::ScalarType height{ 2 };
169 
170  // Generate point cloud.
172  radius, height, numberOfPoints_, translation_, orientation_) };
173 
174  // Assertions on results.
175  // Number of points.
176  ASSERT_EQ(pointCloud.getNbPoints(), numberOfPoints_);
177  // Points correspond to volume.
178  const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) };
179  bool isCylinder{ true };
180  const PM::ScalarType expectedRadiusSquared = radius * radius;
181  for (PM::DataPoints::Index i{ 0 }; i < pointCloud.features.cols() && isCylinder; ++i)
182  {
183  // Fetch point and remove transformation offset.
184  const PM::PointCloudGenerator::StaticCoordVector point(pointCloud.features(0), pointCloud.features(1), pointCloud.features(2));
185  const PM::PointCloudGenerator::StaticCoordVector centeredPoint{ transformation.inverse() * point };
186 
187  // Check whether the point lies inside the volume.
188  const PM::ScalarType computedRadiusSquared{ centeredPoint(0) * centeredPoint(0) + centeredPoint(1) * centeredPoint(1) };
189  const PM::ScalarType computedHeight{ std::abs(centeredPoint(2)) };
190  // For the point to belong to a cylinder, its 2D section (circle) must have the right radius, and its height must be within the
191  // expected margin.
192  if (computedRadiusSquared > expectedRadiusSquared + kEpsilonError_ || computedHeight > height + kEpsilonError_)
193  {
194  isCylinder = false;
195  }
196  }
197  ASSERT_TRUE(isCylinder);
198 }
199 
201 { // NOLINT
202  setDefaultParameters();
203 
204  // Dimensions of the box.
205  const PM::ScalarType length{ 1 };
206  const PM::ScalarType width{ 2 };
207  const PM::ScalarType height{ 5 };
208 
209  // Generate point cloud.
210  const PM::DataPoints pointCloud{ PM::PointCloudGenerator::generateUniformlySampledBox(
211  length, width, height, numberOfPoints_, translation_, orientation_) };
212 
213  // Assertions on results.
214  // Number of points.
215  ASSERT_EQ(pointCloud.getNbPoints(), numberOfPoints_);
216  // Points correspond to volume.
217  const PM::PointCloudGenerator::AffineTransform transformation{ PM::PointCloudGenerator::buildUpTransformation(translation_, orientation_) };
218  bool isCube{ true };
219  for (PM::DataPoints::Index i{ 0 }; i < pointCloud.features.cols() && isCube; ++i)
220  {
221  // Fetch point and remove transformation offset.
222  const PM::PointCloudGenerator::StaticCoordVector point(pointCloud.features(0), pointCloud.features(1), pointCloud.features(2));
223  const PM::PointCloudGenerator::StaticCoordVector centeredPoint{ transformation.inverse() * point };
224 
225  // Check whether the point lies inside the volume.
226  if (std::abs(centeredPoint(0)) > 0.5f * length + kEpsilonError_ || std::abs(centeredPoint(1)) > 0.5f * width + kEpsilonError_
227  || std::abs(centeredPoint(2)) > 0.5f * height + kEpsilonError_)
228  {
229  isCube = false;
230  }
231  }
232  ASSERT_TRUE(isCube);
233 }
python::pointmatcher::applyTransformation
void applyTransformation(const StaticCoordVector &translation, const Eigen::Matrix< ScalarType, 4, 1 > &rotation, DataPoints &pointCloud)
Wrapper class that converts general Eigen::Matrix, convertible from numpy array, to Eigen::Quaternion...
Definition: point_cloud_generator.cpp:19
python::pointmatcher::generateUniformlySampledSphere
DataPoints generateUniformlySampledSphere(const ScalarType radius, const DataPoints::Index numberOfPoints, const StaticCoordVector &translation, const Eigen::Matrix< ScalarType, 4, 1 > &rotation)
Wrapper class that converts general Eigen::Matrix, convertible from numpy array, to Eigen::Quaternion...
Definition: point_cloud_generator.cpp:96
DataPoints
PM::DataPoints DataPoints
Definition: pypoint_matcher_helper.h:16
PointCloudGeneratorTest::setDefaultParameters
void setDefaultParameters()
Definition: utest/ui/PointCloudGenerator.cpp:10
PointMatcher::ScalarType
T ScalarType
The scalar type.
Definition: PointMatcher.h:159
PointCloudGeneratorTest::numberOfPoints_
PM::DataPoints::Index numberOfPoints_
Definition: utest/ui/PointCloudGenerator.cpp:22
AffineTransform
Generator::AffineTransform AffineTransform
Definition: pypoint_matcher_helper.h:68
testing::Test
Definition: gtest.h:17760
ASSERT_EQ
#define ASSERT_EQ(val1, val2)
Definition: gtest.h:19781
testing::internal::string
::std::string string
Definition: gtest.h:1979
icp.translation
translation
Definition: icp.py:73
PointCloudGeneratorTest::PointCloudGeneratorTest
PointCloudGeneratorTest()=default
PointCloudGeneratorTest::translation_
PM::PointCloudGenerator::StaticCoordVector translation_
Definition: utest/ui/PointCloudGenerator.cpp:20
PointCloudGeneratorTest::kEpsilonError_
const PM::ScalarType kEpsilonError_
Definition: utest/ui/PointCloudGenerator.cpp:25
ASSERT_TRUE
#define ASSERT_TRUE(condition)
Definition: gtest.h:19333
PointCloudGeneratorTest
Definition: utest/ui/PointCloudGenerator.cpp:4
TEST_F
TEST_F(PointCloudGeneratorTest, BuildUpTransformationTranslationOnly)
Definition: utest/ui/PointCloudGenerator.cpp:29
build_map.transformation
transformation
Definition: build_map.py:37
python::pointmatcher::generateUniformlySampledCylinder
DataPoints generateUniformlySampledCylinder(const ScalarType radius, const ScalarType height, const DataPoints::Index numberOfPoints, const StaticCoordVector &translation, const Eigen::Matrix< ScalarType, 4, 1 > &rotation)
Wrapper class that converts general Eigen::Matrix, convertible from numpy array, to Eigen::Quaternion...
Definition: point_cloud_generator.cpp:67
PointCloudGeneratorTest::orientation_
PM::Quaternion orientation_
Definition: utest/ui/PointCloudGenerator.cpp:21
StaticCoordVector
Generator::StaticCoordVector StaticCoordVector
Definition: pypoint_matcher_helper.h:69
PointMatcher< float >::Quaternion
Eigen::Quaternion< float > Quaternion
A quaternion over ScalarType.
Definition: PointMatcher.h:165


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