Branch data     Line data    Source code

       1                 :            : // Copyright 2017-2019 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                 :            : /// \file
      18                 :            : /// \brief This file defines the voxel grid data structure for downsampling point clouds
      19                 :            : 
      20                 :            : #ifndef VOXEL_GRID__VOXEL_GRID_HPP_
      21                 :            : #define VOXEL_GRID__VOXEL_GRID_HPP_
      22                 :            : 
      23                 :            : #include <voxel_grid/config.hpp>
      24                 :            : #include <voxel_grid/voxels.hpp>
      25                 :            : #include <common/types.hpp>
      26                 :            : #include <forward_list>
      27                 :            : #include <unordered_map>
      28                 :            : 
      29                 :            : using autoware::common::types::bool8_t;
      30                 :            : 
      31                 :            : namespace autoware
      32                 :            : {
      33                 :            : namespace perception
      34                 :            : {
      35                 :            : namespace filters
      36                 :            : {
      37                 :            : namespace voxel_grid
      38                 :            : {
      39                 :            : 
      40                 :            : /// \brief A voxel grid data structure for downsampling point clouds
      41                 :            : /// \tparam VoxelT The underlying voxel type, assumed to be a child class of Voxel with the
      42                 :            : ///                addition of the add_observation(PointT) method
      43                 :            : template<typename VoxelT>
      44                 :            : class VOXEL_GRID_PUBLIC VoxelGrid
      45                 :            : {
      46                 :            :   using Grid = std::unordered_map<uint64_t, VoxelT>;
      47                 :            :   using IT = typename Grid::const_iterator;
      48                 :            :   using OutputQueue = std::forward_list<IT>;
      49                 :            :   // TODO(c.ho) static assert for better error messages
      50                 :            : 
      51                 :            : public:
      52                 :            :   /// \brief Constructor
      53                 :            :   /// \param[in] cfg The configuration class
      54                 :          2 :   explicit VoxelGrid(const Config & cfg)
      55                 :            :   : m_config(cfg),
      56                 :            :     m_map(m_config.get_capacity()),
      57                 :            :     m_output_pool(m_config.get_capacity()),
      58                 :            :     m_output(m_output_pool.get_allocator()),
      59                 :            :     m_new_voxels_called(false),
      60   [ +  -  +  -  :          4 :     m_last_output_begin(m_output.end())
                   +  - ]
      61                 :            :   {
      62                 :          2 :   }
      63                 :            : 
      64                 :            :   using point_t = typename VoxelT::point_t;
      65                 :            :   /// \brief Inserts a point into the voxel grid, may result in new voxels being activated
      66                 :            :   /// \param[in] pt The point to insert
      67                 :    5088620 :   void insert(const point_t & pt)
      68                 :            :   {
      69                 :            :     // Get voxel
      70                 :    5088620 :     const uint64_t idx = m_config.index(pt);
      71                 :            :     // Check capacity if it would be a new voxel
      72         [ +  + ]:    5088620 :     if (m_map.end() == m_map.find(idx)) {
      73         [ +  + ]:     657464 :       if (capacity() <= size()) {
      74         [ +  - ]:          4 :         throw std::length_error{"VoxelGrid: insertion would overrun capacity"};
      75                 :            :       }
      76                 :            :     }
      77                 :            :     // TODO(c.ho) #2023 adding a new node allocates memory...
      78                 :            :     VoxelT & vx = m_map[idx];
      79                 :            :     // Add to new queue if newly activated, set up any stateful information
      80         [ +  + ]:    5088616 :     if (!vx.occupied()) {
      81                 :            :       // Set hint here, rationale:
      82                 :            :       // Using some conditional constructor would require C++17 (my preferred solution)
      83                 :            :       // In addition, doing the voxel centroid calculation for every point is wasted work
      84                 :            :       // I in general agree that an `init` or `configure` method is poor style, but in this case
      85                 :            :       // The details of the voxel should be mostly hidden from the user
      86                 :            :       //lint -e{523} NOLINT This is to support multiple voxel implementations, see above
      87                 :     657460 :       vx.configure(m_config, idx);
      88                 :            :       // TODO(c.ho) clean up logic
      89                 :            :       // Add to output queue
      90                 :     657460 :       const auto it = m_output_pool.begin();
      91         [ +  - ]:     657460 :       *it = m_map.find(idx);
      92                 :            :       m_output.splice_after(m_output.cbefore_begin(), m_output_pool, m_output_pool.before_begin());
      93         [ +  + ]:     657460 :       if (m_new_voxels_called) {
      94                 :          2 :         m_last_output_begin = m_output.begin();
      95                 :          2 :         m_new_voxels_called = false;
      96                 :            :       }
      97                 :            :     }
      98                 :            :     // Add observation to voxel
      99                 :    5088598 :     vx.add_observation(pt);
     100                 :    5088616 :   }
     101                 :            :   /// \brief Inserts many points into the voxel grid, dispatches to the core insert method.
     102                 :            :   /// \tparam IT The iterator type
     103                 :            :   /// \param[in] begin The starting iterator
     104                 :            :   /// \param[in] end An iterator pointing one past the last element to be inserted.
     105                 :            :   template<typename IT>
     106                 :            :   void insert(const IT begin, const IT end)
     107                 :            :   {
     108   [ +  +  +  + ]:         10 :     for (IT it = begin; it != end; ++it) {
     109   [ +  -  +  - ]:          8 :       insert(*it);
     110                 :            :     }
     111                 :            :   }
     112                 :            :   // TODO(c.ho) operator[]? centroid(uint64_t) out here?
     113                 :            : 
     114                 :            :   /// \brief Return a list of iterators pointing to newly activated voxels. Voxels will be removed
     115                 :            :   ///        from queue during a subsequent call to this method.
     116                 :            :   /// \return A list of newly activated iterators pointing to index-voxel pairs, from most recently
     117                 :            :   ///         activated to least recently activated voxels.
     118                 :            :   ///
     119                 :            :   /// An example of queueing behavior:
     120                 :            :   /// insert 0, 1, 2, 3, 4
     121                 :            :   /// new_voxels: 4->3->2->1->0
     122                 :            :   /// insert 0, 3, 5
     123                 :            :   /// new_voxels: 5
     124                 :          5 :   const OutputQueue & new_voxels()
     125                 :            :   {
     126                 :            :     // Remove old outputs and put it into the pool
     127                 :          5 :     m_output_pool.splice_after(
     128                 :            :       m_output_pool.cbefore_begin(),
     129                 :            :       m_output,
     130                 :            :       m_last_output_begin,
     131                 :            :       m_output.end());
     132                 :          5 :     m_new_voxels_called = true;
     133                 :          5 :     m_last_output_begin = m_output.before_begin();
     134                 :          5 :     return m_output;
     135                 :            :   }
     136                 :            : 
     137                 :            :   /// \brief Returns an iterator to the first element of the voxel grid
     138                 :            :   /// \return Iterator
     139                 :          0 :   IT begin() const
     140                 :            :   {
     141                 :          0 :     return cbegin();
     142                 :            :   }
     143                 :            :   /// \brief Returns an iterator to the first element of the voxel grid
     144                 :            :   /// \return Iterator
     145                 :          0 :   IT cbegin() const
     146                 :            :   {
     147                 :          0 :     return m_map.cbegin();
     148                 :            :   }
     149                 :            :   /// \brief Returns an iterator to one past the last element of the voxel grid
     150                 :            :   /// \return Iterator
     151                 :          0 :   IT end() const
     152                 :            :   {
     153                 :          0 :     return cend();
     154                 :            :   }
     155                 :            :   /// \brief Returns an iterator to one past the last element of the voxel grid
     156                 :            :   /// \return Iterator
     157                 :          0 :   IT cend() const
     158                 :            :   {
     159                 :          0 :     return m_map.cend();
     160                 :            :   }
     161                 :            :   /// \brief Resets the state of the voxel grid
     162         [ +  - ]:          2 :   void clear()
     163                 :            :   {
     164                 :            :     // TODO(c.ho) clear deallocates nodes #2023
     165                 :            :     m_output_pool.splice_after(m_output_pool.before_begin(), m_output);
     166                 :          2 :     m_new_voxels_called = false;
     167                 :          2 :     m_last_output_begin = m_output.end();
     168                 :            :     m_map.clear();
     169                 :          2 :   }
     170                 :            :   /// \brief Returns the current size of the voxel grid
     171                 :          0 :   std::size_t size() const
     172                 :            :   {
     173                 :          0 :     return m_map.size();
     174                 :            :   }
     175                 :            :   /// \brief Returns the preallocated capacity of the voxel grid
     176                 :            :   /// \return The preallocated capacity
     177                 :          0 :   std::size_t capacity() const
     178                 :            :   {
     179   [ +  -  +  -  :     657474 :     return m_config.get_capacity();
          +  -  +  -  +  
          -  +  -  +  -  
          +  -  +  -  +  
                      - ]
     180                 :            :   }
     181                 :            :   /// \brief Whether the voxel grid is empty
     182                 :            :   /// \return True or false
     183                 :          0 :   bool8_t empty() const
     184                 :            :   {
     185                 :          0 :     return m_map.empty();
     186                 :            :   }
     187                 :            : 
     188                 :            : private:
     189                 :            :   const Config m_config;
     190                 :            :   Grid m_map;
     191                 :            :   // Mechanisms to support output queueing in static memory
     192                 :            :   OutputQueue m_output_pool;
     193                 :            :   OutputQueue m_output;
     194                 :            :   bool8_t m_new_voxels_called;
     195                 :            :   typename OutputQueue::iterator m_last_output_begin;
     196                 :            : };  // class VoxelGrid
     197                 :            : 
     198                 :            : }  // namespace voxel_grid
     199                 :            : }  // namespace filters
     200                 :            : }  // namespace perception
     201                 :            : }  // namespace autoware
     202                 :            : 
     203                 :            : #endif  // VOXEL_GRID__VOXEL_GRID_HPP_