LCOV - code coverage report
Current view: top level - src/localization/ndt/include/ndt - utils.hpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 50 60 83.3 %
Date: 2023-03-03 05:44:19 Functions: 12 13 92.3 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 19 68 27.9 %

           Branch data     Line data    Source code
       1                 :            : // Copyright 2020 the Autoware Foundation
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : //
      15                 :            : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
      16                 :            : 
      17                 :            : #ifndef NDT__UTILS_HPP_
      18                 :            : #define NDT__UTILS_HPP_
      19                 :            : 
      20                 :            : #include <Eigen/Core>
      21                 :            : #include <Eigen/Geometry>
      22                 :            : #include <geometry_msgs/msg/transform.hpp>
      23                 :            : #include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
      24                 :            : #include <helper_functions/float_comparisons.hpp>
      25                 :            : #include <geometry_msgs/msg/pose.hpp>
      26                 :            : #include <Eigen/Eigenvalues>
      27                 :            : #include <algorithm>
      28                 :            : #include <limits>
      29                 :            : #include <tuple>
      30                 :            : 
      31                 :            : namespace autoware
      32                 :            : {
      33                 :            : namespace localization
      34                 :            : {
      35                 :            : namespace ndt
      36                 :            : {
      37                 :            : 
      38                 :            : struct PointWithCovariances
      39                 :            : {
      40                 :            :   double x;
      41                 :            :   double y;
      42                 :            :   double z;
      43                 :            :   double icov_xx;
      44                 :            :   double icov_xy;
      45                 :            :   double icov_xz;
      46                 :            :   double icov_yy;
      47                 :            :   double icov_yz;
      48                 :            :   double icov_zz;
      49                 :          0 :   friend bool operator==(const PointWithCovariances & p1, const PointWithCovariances & p2)
      50                 :            :   {
      51                 :            :     constexpr auto eps = std::numeric_limits<double>::epsilon();
      52   [ #  #  #  # ]:          0 :     return common::helper_functions::comparisons::rel_eq(p1.x, p2.x, eps) &&
      53   [ #  #  #  # ]:          0 :            common::helper_functions::comparisons::rel_eq(p1.y, p2.y, eps) &&
      54   [ #  #  #  # ]:          0 :            common::helper_functions::comparisons::rel_eq(p1.z, p2.z, eps) &&
      55   [ #  #  #  # ]:          0 :            common::helper_functions::comparisons::rel_eq(p1.icov_xx, p2.icov_xx, eps) &&
      56   [ #  #  #  # ]:          0 :            common::helper_functions::comparisons::rel_eq(p1.icov_xy, p2.icov_xy, eps) &&
      57   [ #  #  #  # ]:          0 :            common::helper_functions::comparisons::rel_eq(p1.icov_xz, p2.icov_xz, eps) &&
      58   [ #  #  #  # ]:          0 :            common::helper_functions::comparisons::rel_eq(p1.icov_yy, p2.icov_yy, eps) &&
      59   [ #  #  #  #  :          0 :            common::helper_functions::comparisons::rel_eq(p1.icov_yz, p2.icov_yz, eps) &&
                   #  # ]
      60         [ #  # ]:          0 :            common::helper_functions::comparisons::rel_eq(p1.icov_zz, p2.icov_zz, eps);
      61                 :            :   }
      62                 :            : };
      63   [ +  -  +  - ]:        118 : LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_xx);
      64   [ +  -  +  - ]:        118 : LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_xy);
      65   [ +  -  +  - ]:        118 : LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_xz);
      66   [ +  -  +  - ]:        118 : LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_yy);
      67   [ +  -  +  - ]:        118 : LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_yz);
      68   [ +  -  +  - ]:        118 : LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(icov_zz);
      69                 :            : 
      70                 :            : using PointWithCovariancesFieldGenerators = std::tuple<
      71                 :            :   point_cloud_msg_wrapper::field_x_generator,
      72                 :            :   point_cloud_msg_wrapper::field_y_generator,
      73                 :            :   point_cloud_msg_wrapper::field_z_generator,
      74                 :            :   field_icov_xx_generator,
      75                 :            :   field_icov_xy_generator,
      76                 :            :   field_icov_xz_generator,
      77                 :            :   field_icov_yy_generator,
      78                 :            :   field_icov_yz_generator,
      79                 :            :   field_icov_zz_generator>;
      80                 :            : 
      81                 :            : using NdtMapCloudModifier =
      82                 :            :   point_cloud_msg_wrapper::PointCloud2Modifier<PointWithCovariances,
      83                 :            :     PointWithCovariancesFieldGenerators>;
      84                 :            : 
      85                 :            : using NdtMapCloudView =
      86                 :            :   point_cloud_msg_wrapper::PointCloud2View<PointWithCovariances,
      87                 :            :     PointWithCovariancesFieldGenerators>;
      88                 :            : 
      89                 :            : /// This function will check if the covariance is valid based on its eigenvalues. If the covariance
      90                 :            : /// is valid, eigen values smaller than a fraction of the biggest eigen value will be capped to the
      91                 :            : /// threshold. Covariance then will be reconstructed from the modified set of eigen values and
      92                 :            : /// vectors. This should result in increased numerical stability as stated in [Magnusson 2009].
      93                 :            : /// If the covariance is invalid, it will not be updated and false will be returned.
      94                 :            : /// \tparam Derived Deduced Eigen Matrix type.
      95                 :            : /// \param covariance [in, out] Covariance matrix to get stabilized.
      96                 :            : /// \param scaling_factor [in] The ratio between the max. eigen value and the minimum
      97                 :            : /// allowed eigenvalue. Default value is 0.01 as suggested in [Magnusson 2009], page 60.
      98                 :            : /// \return True if the covariance matrix is valid.
      99                 :            : template<typename Derived>
     100                 :      19959 : bool try_stabilize_covariance(
     101                 :            :   Eigen::MatrixBase<Derived> & covariance,
     102                 :            :   typename Derived::PlainMatrix::Scalar scaling_factor = 0.1)
     103                 :            : {
     104                 :            :   using CovMatrixT = typename Derived::PlainMatrix;
     105                 :            :   using ScalarT = typename CovMatrixT::Scalar;
     106                 :            :   using IndexT = typename CovMatrixT::Index;
     107                 :            :   using SolverT = Eigen::SelfAdjointEigenSolver<CovMatrixT>;
     108                 :            :   using VectorT = typename SolverT::RealVectorType;
     109                 :            : 
     110                 :            :   Eigen::SelfAdjointEigenSolver<CovMatrixT> solver(covariance);
     111         [ +  - ]:      19959 :   if (solver.info() != Eigen::Success) {return false;}
     112                 :            : 
     113                 :            :   VectorT eigen_values = solver.eigenvalues();  // Sorted in increasing order.
     114                 :      19959 :   const auto min_eigen_value = eigen_values[0];
     115                 :      19959 :   const auto max_eigen_value = eigen_values[eigen_values.size() - IndexT{1U}];
     116                 :      19959 :   const auto stabilized_min_eigen_value = scaling_factor * max_eigen_value;
     117         [ +  + ]:      19959 :   if (stabilized_min_eigen_value < std::numeric_limits<ScalarT>::epsilon()) {return false;}
     118         [ +  + ]:      16690 :   if (min_eigen_value > scaling_factor * max_eigen_value) {return true;}  // Already stable.
     119         [ +  + ]:      48044 :   for (auto i = IndexT{0U}; i < eigen_values.size(); ++i) {
     120                 :      36033 :     eigen_values[i] = std::max(eigen_values[i], stabilized_min_eigen_value);
     121                 :            :   }
     122                 :            :   covariance =
     123                 :      12011 :     solver.eigenvectors() * eigen_values.asDiagonal() * solver.eigenvectors().transpose();
     124                 :      12011 :   return true;
     125                 :            : }
     126                 :            : 
     127                 :            : template<typename T>
     128                 :            : using EigenPose = Eigen::Matrix<T, 6U, 1U>;
     129                 :            : template<typename T>
     130                 :            : using EigenTransform = Eigen::Transform<T, 3, Eigen::Affine, Eigen::ColMajor>;
     131                 :            : using RosTransform = geometry_msgs::msg::Transform;
     132                 :            : using RosPose = geometry_msgs::msg::Pose;
     133                 :            : namespace transform_adapters
     134                 :            : {
     135                 :            : /// Template function to convert a 6D pose to a transformation matrix.
     136                 :            : /// This function should be specialized and implemented for the supported types.
     137                 :            : /// \tparam PoseT Pose type.
     138                 :            : /// \tparam TransformT Transform type.
     139                 :            : /// \param[in] pose pose to convert
     140                 :            : /// \param[out] transform resulting transform
     141                 :            : template<typename PoseT, typename TransformT>
     142                 :            : void pose_to_transform(const PoseT & pose, TransformT & transform);
     143                 :            : 
     144                 :            : /// Template function to convert a 6D pose to a transformation matrix.
     145                 :            : /// This function should be specialized and implemented for the supported types.
     146                 :            : /// \tparam PoseT Pose type.
     147                 :            : /// \tparam TransformT Transform type.
     148                 :            : /// \param[in] transform resulting transform
     149                 :            : /// \param[out] pose pose to convert
     150                 :            : template<typename PoseT, typename TransformT>
     151                 :            : void transform_to_pose(const TransformT & transform, PoseT & pose);
     152                 :            : 
     153                 :            : 
     154                 :            : template<typename T>
     155                 :        692 : void pose_to_transform(
     156                 :            :   const EigenPose<T> & pose,
     157                 :            :   EigenTransform<T> & transform)
     158                 :            : {
     159                 :            :   static_assert(std::is_floating_point<T>::value, "Eigen transform should use floating points");
     160                 :            :   transform.setIdentity();
     161                 :            :   transform.translation() = pose.head(3);
     162                 :        692 :   transform.rotate(Eigen::AngleAxis<T>(pose(3), Eigen::Vector3d::UnitX()));
     163                 :        692 :   transform.rotate(Eigen::AngleAxis<T>(pose(4), Eigen::Vector3d::UnitY()));
     164                 :        692 :   transform.rotate(Eigen::AngleAxis<T>(pose(5), Eigen::Vector3d::UnitZ()));
     165                 :        692 : }
     166                 :            : 
     167                 :            : /// Specialization to convert from the eigen pose to the ros transform type.
     168                 :            : /// \tparam T Eigen scalar type
     169                 :            : template<typename T>
     170                 :        424 : void pose_to_transform(
     171                 :            :   const EigenPose<T> & pose,
     172                 :            :   RosTransform & transform)
     173                 :            : {
     174                 :            :   static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
     175                 :        424 :   Eigen::Quaternion<T> eig_rot{Eigen::Quaternion<T>{}.setIdentity()};
     176                 :            :   eig_rot.setIdentity();
     177                 :        424 :   eig_rot =
     178                 :            :     Eigen::AngleAxis<T>(pose(3), Eigen::Matrix<T, 3, 1>::UnitX()) *
     179                 :            :     Eigen::AngleAxis<T>(pose(4), Eigen::Matrix<T, 3, 1>::UnitY()) *
     180                 :            :     Eigen::AngleAxis<T>(pose(5), Eigen::Matrix<T, 3, 1>::UnitZ());
     181                 :            : 
     182                 :            :   decltype(RosTransform::translation) trans;
     183                 :            :   decltype(RosTransform::rotation) rot;
     184                 :            : 
     185                 :        424 :   trans.set__x(pose(0)).set__y(pose(1)).set__z(pose(2));
     186                 :            :   transform.set__translation(trans);
     187                 :            : 
     188                 :            :   rot.set__x(eig_rot.x()).
     189                 :            :   set__y(eig_rot.y()).
     190                 :            :   set__z(eig_rot.z()).
     191                 :            :   set__w(eig_rot.w());
     192                 :            :   transform.set__rotation(rot);
     193                 :        424 : }
     194                 :            : 
     195                 :            : /// Specialization to convert from the eigen pose to the ros pose type.
     196                 :            : /// `pose_to_transform` template is used as conversion to `RosPose`
     197                 :            : /// is identical to conversion to `RosTransform`
     198                 :            : /// \tparam T Eigen scalar type
     199                 :            : template<typename T>
     200                 :         25 : void pose_to_transform(
     201                 :            :   const EigenPose<T> & pose,
     202                 :            :   RosPose & ros_pose)
     203                 :            : {
     204                 :            :   static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
     205                 :         25 :   Eigen::Quaternion<T> eig_rot{Eigen::Quaternion<T>{}.setIdentity()};
     206                 :         25 :   eig_rot =
     207                 :            :     Eigen::AngleAxis<T>(pose(3), Eigen::Matrix<T, 3, 1>::UnitX()) *
     208                 :            :     Eigen::AngleAxis<T>(pose(4), Eigen::Matrix<T, 3, 1>::UnitY()) *
     209                 :            :     Eigen::AngleAxis<T>(pose(5), Eigen::Matrix<T, 3, 1>::UnitZ());
     210                 :            : 
     211                 :            :   decltype(RosPose::position) trans;
     212                 :            :   decltype(RosTransform::rotation) rot;
     213                 :            : 
     214                 :         25 :   trans.set__x(pose(0)).set__y(pose(1)).set__z(pose(2));
     215                 :            :   ros_pose.set__position(trans);
     216                 :            : 
     217                 :            :   rot.set__x(eig_rot.x()).
     218                 :            :   set__y(eig_rot.y()).
     219                 :            :   set__z(eig_rot.z()).
     220                 :            :   set__w(eig_rot.w());
     221                 :            :   ros_pose.set__orientation(rot);
     222                 :         25 : }
     223                 :            : 
     224                 :            : /// Specialization to convert from the ros pose type to an eigen one.
     225                 :            : /// \tparam T Eigen scalar type
     226                 :            : template<typename T>
     227                 :         25 : void transform_to_pose(const RosTransform & transform, EigenPose<T> & pose)
     228                 :            : {
     229                 :            :   static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
     230                 :            :   const auto & ros_rot = transform.rotation;
     231                 :            :   const auto & ros_trans = transform.translation;
     232                 :            :   Eigen::Quaternion<T> eig_rot{ros_rot.w, ros_rot.x, ros_rot.y, ros_rot.z};
     233                 :         25 :   pose(0) = ros_trans.x;
     234                 :         25 :   pose(1) = ros_trans.y;
     235                 :         25 :   pose(2) = ros_trans.z;
     236                 :            : 
     237                 :         25 :   const auto rot = eig_rot.matrix().eulerAngles(0, 1, 2);
     238                 :         25 :   pose(3) = rot(0);
     239                 :         25 :   pose(4) = rot(1);
     240                 :         25 :   pose(5) = rot(2);
     241                 :         25 : }
     242                 :            : 
     243                 :            : /// Specialization to convert from the ros pose type to an eigen one.
     244                 :            : /// `transform_to_pose` template is used as conversion from `RosPose`
     245                 :            : /// is identical to conversion from `RosTransform`
     246                 :            : /// \tparam T Eigen scalar type
     247                 :            : template<typename T>
     248                 :         20 : void transform_to_pose(const RosPose & ros_pose, EigenPose<T> & pose)
     249                 :            : {
     250                 :            :   static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
     251                 :            :   const auto & ros_rot = ros_pose.orientation;
     252                 :            :   const auto & ros_trans = ros_pose.position;
     253                 :            :   Eigen::Quaternion<T> eig_rot{ros_rot.w, ros_rot.x, ros_rot.y, ros_rot.z};
     254                 :         20 :   pose(0) = ros_trans.x;
     255                 :         20 :   pose(1) = ros_trans.y;
     256                 :         20 :   pose(2) = ros_trans.z;
     257                 :            : 
     258                 :         20 :   const auto rot = eig_rot.matrix().eulerAngles(0, 1, 2);
     259                 :         20 :   pose(3) = rot(0);
     260                 :         20 :   pose(4) = rot(1);
     261                 :         20 :   pose(5) = rot(2);
     262                 :         20 : }
     263                 :            : 
     264                 :            : }  // namespace transform_adapters
     265                 :            : }  // namespace ndt
     266                 :            : }  // namespace localization
     267                 :            : }  // namespace autoware
     268                 :            : 
     269                 :            : #endif  // NDT__UTILS_HPP_

Generated by: LCOV version 1.14