37 #ifndef GEOMETRIC_SHAPES_CHECK_ISOMETRY_H
38 #define GEOMETRIC_SHAPES_CHECK_ISOMETRY_H
45 #include <Eigen/Geometry>
58 #ifndef CHECK_ISOMETRY_PRECISION
60 #define CHECK_ISOMETRY_PRECISION Eigen::NumTraits<double>::dummy_precision()
71 const bool printError =
true)
73 if (!
transform.matrix().row(3).isApprox(Eigen::Vector4d::UnitW().transpose(), precision))
77 std::cerr <<
"The given transform is not an isometry! Its last row deviates from [0 0 0 1] by ["
78 << (
transform.matrix().row(3) - Eigen::Vector4d::UnitW().transpose())
79 <<
"] but the required precision is " << precision <<
"." << std::endl;
84 Eigen::Isometry3d::LinearMatrixType scale;
85 transform.computeRotationScaling((Eigen::Isometry3d::LinearMatrixType*)
nullptr, &scale);
86 if (!scale.isApprox(Eigen::Matrix3d::Identity(), precision))
90 std::cerr <<
"The given transform is not an isometry! Its linear part involves non-unit scaling. The scaling "
91 "matrix diagonal differs from [1 1 1] by ["
92 << (scale.diagonal().transpose() - Eigen::Vector3d::Ones().transpose())
93 <<
"] but the required precision is " << precision <<
"." << std::endl;
106 #define ASSERT_ISOMETRY(transform) (void)sizeof(transform); // this is a no-op, but prevents unused variable warnings
109 #define ASSERT_ISOMETRY(transform) \
111 if (!::checkIsometry(transform, CHECK_ISOMETRY_PRECISION)) \
112 assert(!"Invalid isometry transform"); \
116 #endif // GEOMETRIC_SHAPES_CHECK_ISOMETRY_H