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__NDT_VOXEL_VIEW_HPP_
      18                 :            : #define NDT__NDT_VOXEL_VIEW_HPP_
      19                 :            : 
      20                 :            : #include <ndt/ndt_voxel.hpp>
      21                 :            : 
      22                 :            : namespace autoware
      23                 :            : {
      24                 :            : namespace localization
      25                 :            : {
      26                 :            : namespace ndt
      27                 :            : {
      28                 :            : 
      29                 :            : template<typename VoxelT>
      30                 :            : class NDT_PUBLIC VoxelView;
      31                 :            : 
      32                 :            : /// Base CRTP interface for the voxel view. It assumes a cached
      33                 :            : /// centroid and inverse covariance are provided by the implementation. This interface
      34                 :            : /// does not contain a covariance access function as it's not directly needed for the
      35                 :            : /// gaussian term shared in ndt optimization problems.
      36                 :            : /// It should be noted that, as this is purely a view of an existing voxel,
      37                 :            : /// it should not outlive the voxel it is referring to.
      38                 :            : /// \tparam VoxelT Type of voxel to view.
      39                 :            : /// \tparam Derived Implementation. Expected to be of `type VoxelView<VoxelT>`.
      40                 :            : template<typename VoxelT, typename Derived>
      41                 :            : class NDT_PUBLIC VoxelViewBase : public common::helper_functions::crtp<Derived>
      42                 :            : {
      43                 :            : public:
      44                 :            :   using Point = Eigen::Vector3d;
      45                 :            :   using Cov = Eigen::Matrix3d;
      46                 :      71179 :   explicit VoxelViewBase(const VoxelT & vx)
      47                 :      71179 :   : m_data_ref{vx} {}
      48                 :            : 
      49                 :      70929 :   const Point & centroid() const
      50                 :            :   {
      51                 :      70929 :     return this->impl().centroid_();
      52                 :            :   }
      53                 :      70704 :   const Cov & inverse_covariance() const
      54                 :            :   {
      55                 :      70704 :     return this->impl().inverse_covariance_();
      56                 :            :   }
      57                 :      70704 :   bool8_t usable() const noexcept
      58                 :            :   {
      59                 :      70704 :     return this->impl().usable_();
      60                 :            :   }
      61                 :         50 :   const VoxelT & get() const noexcept
      62                 :            :   {
      63                 :         50 :     return m_data_ref;
      64                 :            :   }
      65                 :            : 
      66                 :            : private:
      67                 :            :   const VoxelT & m_data_ref;
      68                 :            : };
      69                 :            : 
      70                 :            : /// VoxelViewBase implementation for `StaticNDTVoxel`. It's just a pure wrapper.
      71                 :            : template<>
      72                 :            : class NDT_PUBLIC VoxelView<StaticNDTVoxel>
      73                 :            :   : public VoxelViewBase<StaticNDTVoxel, VoxelView<StaticNDTVoxel>>
      74                 :            : {
      75                 :            : public:
      76                 :            :   using Point = Eigen::Vector3d;
      77                 :            :   using Cov = Eigen::Matrix3d;
      78                 :            :   using Base = VoxelViewBase<StaticNDTVoxel, VoxelView<StaticNDTVoxel>>;
      79                 :            :   explicit VoxelView(const StaticNDTVoxel & voxel);
      80                 :            : 
      81                 :            :   const Cov & inverse_covariance_() const;
      82                 :            :   const Point & centroid_() const;
      83                 :            :   bool8_t usable_() const noexcept;
      84                 :            : 
      85                 :            : private:
      86                 :            :   bool8_t m_usable;
      87                 :            : };
      88                 :            : 
      89                 :            : 
      90                 :            : /// VoxelViewBase implementation for `DynamicNDTVoxel`. On construction, it will compute the
      91                 :            : /// covariance. If the cell is not usable, the value returned by `inverse_covariance_` will
      92                 :            : /// be have an invalid value, thus the user should always first check the usability of a dynamic
      93                 :            : /// voxel view before querying its inverse covariance.
      94                 :            : template<>
      95                 :          0 : class NDT_PUBLIC VoxelView<DynamicNDTVoxel>
      96                 :            :   : public VoxelViewBase<DynamicNDTVoxel, VoxelView<DynamicNDTVoxel>>
      97                 :            : {
      98                 :            : public:
      99                 :            :   using Base = VoxelViewBase<DynamicNDTVoxel, VoxelView<DynamicNDTVoxel>>;
     100                 :            :   using Point = Eigen::Vector3d;
     101                 :            :   using Cov = Eigen::Matrix3d;
     102                 :            :   explicit VoxelView(const DynamicNDTVoxel & voxel);
     103                 :            : 
     104                 :            :   const Cov & inverse_covariance_() const;
     105                 :            :   const Point & centroid_() const;
     106                 :            :   bool8_t usable_() const noexcept;
     107                 :            : 
     108                 :            : private:
     109                 :            :   Cov m_inverse_covariance;
     110                 :            :   bool8_t m_usable{true};
     111                 :            : };
     112                 :            : 
     113                 :            : 
     114                 :            : }  // namespace ndt
     115                 :            : }  // namespace localization
     116                 :            : }  // namespace autoware
     117                 :            : 
     118                 :            : #endif  // NDT__NDT_VOXEL_VIEW_HPP_