check_isometry.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020, Martin Pecka
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Martin Pecka, Robert Haschke */
36 
37 #ifndef GEOMETRIC_SHAPES_CHECK_ISOMETRY_H
38 #define GEOMETRIC_SHAPES_CHECK_ISOMETRY_H
39 
40 #include <algorithm>
41 #include <assert.h>
42 #include <iostream>
43 #include <math.h>
44 #include <Eigen/Core>
45 #include <Eigen/Geometry>
46 
58 #ifndef CHECK_ISOMETRY_PRECISION
59 
60 #define CHECK_ISOMETRY_PRECISION Eigen::NumTraits<double>::dummy_precision()
61 #endif
62 
70 inline bool checkIsometry(const Eigen::Isometry3d& transform, const double precision = CHECK_ISOMETRY_PRECISION,
71  const bool printError = true)
72 {
73  if (!transform.matrix().row(3).isApprox(Eigen::Vector4d::UnitW().transpose(), precision))
74  {
75  if (printError)
76  {
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;
80  }
81  return false;
82  }
83 
84  Eigen::Isometry3d::LinearMatrixType scale;
85  transform.computeRotationScaling((Eigen::Isometry3d::LinearMatrixType*)nullptr, &scale);
86  if (!scale.isApprox(Eigen::Matrix3d::Identity(), precision))
87  {
88  if (printError)
89  {
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;
94  }
95  return false;
96  }
97 
98  return true;
99 }
100 
101 // To be able to use the more efficient Eigen::Transform::linear() instead of rotation(),
102 // we need to check the user has really passed an isometry. To avoid runtime costs,
103 // this check is only done as assert, which gets compiled-out in release builds
104 #ifdef NDEBUG
105 
106 #define ASSERT_ISOMETRY(transform) (void)sizeof(transform); // this is a no-op, but prevents unused variable warnings
107 #else
108 
109 #define ASSERT_ISOMETRY(transform) \
110  { \
111  if (!::checkIsometry(transform, CHECK_ISOMETRY_PRECISION)) \
112  assert(!"Invalid isometry transform"); \
113  }
114 #endif
115 
116 #endif // GEOMETRIC_SHAPES_CHECK_ISOMETRY_H
CHECK_ISOMETRY_PRECISION
#define CHECK_ISOMETRY_PRECISION
This file provides functions and macros that can be used to verify that an Eigen::Isometry3d is reall...
Definition: check_isometry.h:60
checkIsometry
bool checkIsometry(const Eigen::Isometry3d &transform, const double precision=CHECK_ISOMETRY_PRECISION, const bool printError=true)
Check whether the given transform is really (mathematically) an isometry.
Definition: check_isometry.h:70


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Sun Aug 22 2021 02:42:25