7 const Eigen::Rotation2D<NumericType>&
rotation,
10 const Eigen::Transform<NumericType, 2, Eigen::Affine>
transformation =
16 const Eigen::Quaternion<NumericType>&
rotation,
19 const Eigen::Transform<NumericType, 3, Eigen::Affine>
transformation =
20 Eigen::Translation<NumericType, 3>(
translation) * Eigen::AngleAxis<NumericType>(
rotation.normalized()) * Eigen::Scaling(scale);
25 std::shared_ptr<PM::Transformation>& transformator,
29 auto transformedCloud = cloud;
35 for (
size_t i = 0; i < cloud.getNbPoints(); ++i)
37 const auto transformedFeature =
transformation * cloud.features.col(i);
38 ASSERT_TRUE(transformedFeature.isApprox(transformedCloud.features.col(i), kEpsilonNumericalError));
43 const int descCols(cloud.descriptors.cols());
47 for (
size_t i = 0; i < cloud.descriptorLabels.size(); ++i)
49 const int span(cloud.descriptorLabels[i].span);
51 if (
name ==
"normals" ||
name ==
"observationDirections")
53 const auto transformedDescriptor = R * cloud.descriptors.block(row, 0, span, descCols);
54 ASSERT_TRUE(transformedDescriptor.isApprox(transformedCloud.descriptors.block(row, 0, span, descCols), kEpsilonNumericalError));
58 EXPECT_EQ(cloud.featureLabels, transformedCloud.featureLabels);
59 EXPECT_EQ(cloud.descriptorLabels, transformedCloud.descriptorLabels);
60 EXPECT_EQ(cloud.timeLabels, transformedCloud.timeLabels);
61 EXPECT_TRUE(cloud.times.isApprox(transformedCloud.times, 0));
81 icp.transformationCheckers.clear();
91 icp.transformationCheckers.push_back(transformCheck);
98 addFilter(
"CounterTransformationChecker", { {
"maxIterationCount",
toParam(20) } });
99 validate2dTransformation();
104 addFilter(
"DifferentialTransformationChecker",
105 { {
"minDiffRotErr",
toParam(0.001) }, {
"minDiffTransErr",
toParam(0.001) }, {
"smoothLength",
toParam(4) } });
106 validate2dTransformation();
114 std::shared_ptr<PM::TransformationChecker> extraTransformCheck;
116 extraTransformCheck =
PM::get().TransformationCheckerRegistrar.create(
"CounterTransformationChecker");
117 icp.transformationCheckers.push_back(extraTransformCheck);
119 addFilter(
"BoundTransformationChecker", { {
"maxRotationNorm",
toParam(1.0) }, {
"maxTranslationNorm",
toParam(1.0) } });
120 validate2dTransformation();
128 std::shared_ptr<PM::Transformation> rigidTrans;
136 T_3D << 0.99935116, 0.13669771, 0.03436585, 1.71138524, -0.02633967, 0.99326295, -0.04907545, -0.10860933, -0.03615132, 0.04400287,
137 0.99820427, -0.04454497, 0., 0., 0., 1.;
144 for (
int i = 0; i < 10; i++)
146 T_3D = rigidTrans->correctParameters(T_3D);
152 PM::Matrix T_2D_non_ortho = PM::Matrix::Identity(3, 3);
153 T_2D_non_ortho(0, 0) = 0.8;
154 T_2D_non_ortho(0, 1) = -0.5;
155 T_2D_non_ortho(1, 0) = 0.5;
156 T_2D_non_ortho(1, 1) = 0.8;
158 EXPECT_FALSE(rigidTrans->checkParameters(T_2D_non_ortho));
164 for (
int i = 0; i < 10; i++)
166 T_2D_non_ortho = rigidTrans->correctParameters(T_2D_non_ortho);
167 EXPECT_TRUE(rigidTrans->checkParameters(T_2D_non_ortho));
172 PM::Matrix T_2D_reflection = PM::Matrix::Identity(3, 3);
173 T_2D_reflection(1, 1) = -1;
180 std::shared_ptr<PM::Transformation> transformator =
PM::get().REG(
Transformation).create(
"PureTranslation");
184 const Eigen::Matrix<NumericType, 2, 1>
translation{ 0, 0 };
185 const Eigen::Rotation2D<NumericType>
rotation{ 0 };
192 const Eigen::Matrix<NumericType, 2, 1>
translation{ -1.0001, 34.5 };
193 const Eigen::Rotation2D<NumericType>
rotation{ 0 };
202 std::shared_ptr<PM::Transformation> transformator =
PM::get().REG(
Transformation).create(
"PureTranslation");
206 const Eigen::Matrix<NumericType, 3, 1>
translation{ 0, 0, 0 };
207 const Eigen::Quaternion<NumericType>
rotation{ 1, 0, 0, 0 };
215 const Eigen::Matrix<NumericType, 3, 1>
translation{ -1.0001, 5, -12321.234 };
216 const Eigen::Quaternion<NumericType>
rotation{ 1, 0, 0, 0 };
225 std::shared_ptr<PM::Transformation> transformator =
PM::get().REG(
Transformation).create(
"RigidTransformation");
229 const Eigen::Matrix<NumericType, 2, 1>
translation{ 0, 0 };
230 const Eigen::Rotation2D<NumericType>
rotation{ 0 };
237 const Eigen::Matrix<NumericType, 2, 1>
translation{ -1.0001, 34.5 };
238 const Eigen::Rotation2D<NumericType>
rotation{ 0 };
245 const Eigen::Matrix<NumericType, 2, 1>
translation{ 0, 0 };
246 const Eigen::Rotation2D<NumericType>
rotation{ 3.53453 };
253 const Eigen::Matrix<NumericType, 2, 1>
translation{ -3.11, 100.222 };
254 const Eigen::Rotation2D<NumericType>
rotation{ -123.3 };
304 std::shared_ptr<PM::Transformation> transformator =
PM::get().REG(
Transformation).create(
"SimilarityTransformation");
308 const Eigen::Matrix<NumericType, 2, 1>
translation{ 0, 0 };
309 const Eigen::Rotation2D<NumericType>
rotation{ 0 };
317 const Eigen::Matrix<NumericType, 2, 1>
translation{ 0, 0 };
318 const Eigen::Rotation2D<NumericType>
rotation{ 0 };
326 const Eigen::Matrix<NumericType, 2, 1>
translation{ 0, 0 };
327 const Eigen::Rotation2D<NumericType>
rotation{ 0 };
335 const Eigen::Matrix<NumericType, 2, 1>
translation{ -1.0001, 34.5 };
336 const Eigen::Rotation2D<NumericType>
rotation{ 0 };
344 const Eigen::Matrix<NumericType, 2, 1>
translation{ 0, 0 };
345 const Eigen::Rotation2D<NumericType>
rotation{ 3.53453 };
353 const Eigen::Matrix<NumericType, 2, 1>
translation{ -3.11, 100.222 };
354 const Eigen::Rotation2D<NumericType>
rotation{ -123.3 };