voxels.hpp

Go to the documentation of this file.

// Copyright 2017-2019 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
 
 
#ifndef VOXEL_GRID__VOXELS_HPP_
#define VOXEL_GRID__VOXELS_HPP_
 
#include "common/types.hpp"
#include "voxel_grid/config.hpp"
#include "voxel_grid/visibility_control.hpp"
#include "voxel_grid/voxel.hpp"
 
namespace autoware
{
namespace perception
{
namespace filters
{
namespace voxel_grid
{
 
using autoware::common::types::PointXYZIF;
using autoware::common::types::float32_t;
 
template<typename PointT>
VOXEL_GRID_PUBLIC PointT operator+(const PointT & lhs, const PointT & rhs)
{
  PointT ret = lhs;
  ret.x += rhs.x;
  ret.y += rhs.y;
  ret.z += rhs.z;
  return ret;
}
 
template<typename PointT>
VOXEL_GRID_PUBLIC PointT operator*(const PointT & lhs, const float32_t rhs)
{
  PointT ret = lhs;
  ret.x *= rhs;
  ret.y *= rhs;
  ret.z *= rhs;
  return ret;
}
 
template<>
inline VOXEL_GRID_PUBLIC PointXYZIF operator+(
  //lint -e{9073} NOLINT This is a template specialization, not a token mismatch
  const PointXYZIF & lhs,
  //lint -e{9073} NOLINT This is a template specialization, not a token mismatch
  const PointXYZIF & rhs)
{
  PointXYZIF ret = lhs;
  ret.x += rhs.x;
  ret.y += rhs.y;
  ret.z += rhs.z;
  ret.intensity += rhs.intensity;
  return ret;
}
template<>
inline VOXEL_GRID_PUBLIC PointXYZIF operator*(
  //lint -e{9073} NOLINT This is a template specialization, not a token mismatch
  const PointXYZIF & lhs,
  const float32_t rhs)
{
  PointXYZIF ret = lhs;
  ret.x *= rhs;
  ret.y *= rhs;
  ret.z *= rhs;
  ret.intensity *= rhs;
  return ret;
}
 
template<typename PointT>
class VOXEL_GRID_PUBLIC CentroidVoxel : public Voxel<PointT>
{
public:
  using Voxel<PointT>::Voxel;
  void add_observation(const PointT & pt)
  {
    const float32_t last = static_cast<float32_t>(Voxel<PointT>::count());
    Voxel<PointT>::set_count(Voxel<PointT>::count() + 1U);
    const float32_t count_inv = 1.0F / static_cast<float32_t>(Voxel<PointT>::count());
    // Incremental update: u' = ((u * n) + x) / (n + 1), u = mean, x = obs, n = count
    const PointT tmp = ((Voxel<PointT>::get() * last) + pt) * count_inv;
    Voxel<PointT>::set_centroid(tmp);
  }
  //lint -e{9175} NOLINT this is to match a parent API
  void configure(const Config & cfg, const uint64_t idx)
  {
    (void)cfg;
    (void)idx;
  }
};  // class CentroidVoxel
 
template<typename PointT>
class VOXEL_GRID_PUBLIC ApproximateVoxel : public Voxel<PointT>
{
public:
  using Voxel<PointT>::Voxel;
  void add_observation(const PointT & pt)
  {
    Voxel<PointT>::set_count(Voxel<PointT>::count() + 1U);
    (void)pt;
  }
  void configure(const Config & cfg, const uint64_t idx)
  {
    Voxel<PointT>::set_centroid(cfg.centroid<PointT>(idx));
  }
};  // class ApproximateVoxel
 
 
}  // namespace voxel_grid
}  // namespace filters
}  // namespace perception
}  // namespace autoware
 
#endif  // VOXEL_GRID__VOXELS_HPP_