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;
 
  static constexpr uint32_t kNumConfigPoints = 3U;
 
  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;
 
  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;
 
  std::size_t size() const;
 
  typename VoxelGrid::const_iterator begin() const;
 
  typename VoxelGrid::const_iterator end() const;
 
  void clear();
 
private:
  void deserialize_from(const sensor_msgs::msg::PointCloud2 & msg);
  std::experimental::optional<NDTGrid<StaticNDTVoxel>> m_grid{};
  TimePoint m_stamp{};
  std::string m_frame_id{};
};
 
template<typename PCMessageT>
using PC2DoubleIterator = typename std::conditional<std::is_const<PCMessageT>::value,
    sensor_msgs::PointCloud2ConstIterator<ndt::Real>,
    sensor_msgs::PointCloud2Iterator<ndt::Real>>::type;
 
template<typename PointCloud2T>
std::array<PC2DoubleIterator<PointCloud2T>, 9U> get_iterators(PointCloud2T & msg)
{
  return std::array<PC2DoubleIterator<PointCloud2T>, 9U>{
    PC2DoubleIterator<PointCloud2T>{msg, "x"},
    PC2DoubleIterator<PointCloud2T>{msg, "y"},
    PC2DoubleIterator<PointCloud2T>{msg, "z"},
    PC2DoubleIterator<PointCloud2T>{msg, "icov_xx"},
    PC2DoubleIterator<PointCloud2T>{msg, "icov_xy"},
    PC2DoubleIterator<PointCloud2T>{msg, "icov_xz"},
    PC2DoubleIterator<PointCloud2T>{msg, "icov_yy"},
    PC2DoubleIterator<PointCloud2T>{msg, "icov_yz"},
    PC2DoubleIterator<PointCloud2T>{msg, "icov_zz"}
  };
}
 
struct SerializedNDTMapPoint
{
  Real x{0.0};
  Real y{0.0};
  Real z{0.0};
  Real icov_xx{0.0};
  Real icov_xy{0.0};
  Real icov_xz{0.0};
  Real icov_yy{0.0};
  Real icov_yz{0.0};
  Real icov_zz{0.0};
};
 
void push_back(
  std::array<sensor_msgs::PointCloud2Iterator<ndt::Real>, 9U> & pc_its,
  const SerializedNDTMapPoint & point);
 
template<typename T>
SerializedNDTMapPoint next(std::array<T, 9U> & pc_its)
{
  const auto ret = SerializedNDTMapPoint{
    *pc_its[0U], *pc_its[1U], *pc_its[2U],    // x, y, z
    *pc_its[3U], *pc_its[4U], *pc_its[5U],    // xx, yy, zz
    *pc_its[6U], *pc_its[7U],    // yy, yz
    *pc_its[8U]    // zz
  };
  std::for_each(pc_its.begin(), pc_its.end(), [](auto & it) {++it;});
  return ret;
}
 
}  // namespace ndt
}  // namespace localization
}  // namespace autoware
#endif  // NDT__NDT_MAP_HPP_