ndt_map.hpp

Go to the documentation of this file.

// Copyright 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 NDT__NDT_MAP_HPP_
#define NDT__NDT_MAP_HPP_

#include <ndt/ndt_common.hpp>
#include <ndt/ndt_voxel.hpp>
#include <ndt/ndt_voxel_view.hpp>
#include <ndt/ndt_grid.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <time_utils/time_utils.hpp>
#include <vector>
#include <limits>
#include <unordered_map>
#include <utility>
#include <string>

using autoware::common::types::float32_t;

namespace autoware
{
namespace localization
{
namespace ndt
{
uint32_t NDT_PUBLIC validate_pcl_map(const sensor_msgs::msg::PointCloud2 & msg);

class NDT_PUBLIC DynamicNDTMap
{
public:
  using Voxel = DynamicNDTVoxel;
  using Config = autoware::perception::filters::voxel_grid::Config;
  using Point = Eigen::Vector3d;
  using TimePoint = std::chrono::system_clock::time_point;
  using VoxelViewVector = std::vector<VoxelView<Voxel>>;
  using VoxelGrid = NDTGrid<Voxel>::Grid;
  using ConfigPoint = NDTGrid<Voxel>::ConfigPoint;

  explicit DynamicNDTMap(const Config & voxel_grid_config);

  void set(const sensor_msgs::msg::PointCloud2 & msg);

  void insert(const sensor_msgs::msg::PointCloud2 & msg);

  template<typename DeserializingMapT>
  void serialize_as(sensor_msgs::msg::PointCloud2 & msg_out) const;

  const VoxelViewVector & cell(const Point & pt) const;

  const VoxelViewVector & cell(float32_t x, float32_t y, float32_t z) const;

  const std::string & frame_id() const noexcept;

  TimePoint stamp() const noexcept;

  bool valid() const noexcept;

  const ConfigPoint & cell_size() const noexcept;

  std::size_t size() const noexcept;

  typename VoxelGrid::const_iterator begin() const noexcept;

  typename VoxelGrid::const_iterator end() const noexcept;

  void clear() noexcept;

private:
  NDTGrid<DynamicNDTVoxel> m_grid;
  TimePoint m_stamp{};
  std::string m_frame_id{};
};

class NDT_PUBLIC StaticNDTMap
{
public:
  using Voxel = StaticNDTVoxel;
  using Config = autoware::perception::filters::voxel_grid::Config;
  using TimePoint = std::chrono::system_clock::time_point;
  using Point = Eigen::Vector3d;
  using VoxelViewVector = std::vector<VoxelView<Voxel>>;
  using VoxelGrid = NDTGrid<Voxel>::Grid;
  using ConfigPoint = NDTGrid<Voxel>::ConfigPoint;

  explicit StaticNDTMap(const Config & voxel_grid_config);

  void set(const sensor_msgs::msg::PointCloud2 & msg);

  const VoxelViewVector & cell(const Point & pt) const;

  const VoxelViewVector & cell(float32_t x, float32_t y, float32_t z) const;

  const std::string & frame_id() const noexcept;

  TimePoint stamp() const noexcept;

  bool valid() const noexcept;

  const ConfigPoint & cell_size() const noexcept;

  std::size_t size() const noexcept;

  typename VoxelGrid::const_iterator begin() const noexcept;

  typename VoxelGrid::const_iterator end() const noexcept;

  void clear() noexcept;

private:
  void deserialize_from(const sensor_msgs::msg::PointCloud2 & msg);
  NDTGrid<StaticNDTVoxel> m_grid;
  TimePoint m_stamp{};
  std::string m_frame_id{};
};

}  // namespace ndt
}  // namespace localization
}  // namespace autoware
#endif  // NDT__NDT_MAP_HPP_