point_cloud_map.hpp

Go to the documentation of this file.

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

#include <point_cloud_mapping/visibility_control.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <voxel_grid/voxel_grid.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <helper_functions/crtp.hpp>
#include <helper_functions/template_utils.hpp>
#include <common/types.hpp>
#include <time_utils/time_utils.hpp>
#include <pcl/io/pcd_io.h>
#include <vector>
#include <string>
#include <unordered_map>
#include <utility>

namespace autoware
{
namespace mapping
{
namespace point_cloud_mapping
{

enum class MapUpdateType
{
  NEW,
  UPDATE,
  PARTIAL_UPDATE,
  NO_CHANGE
};

struct POINT_CLOUD_MAPPING_PUBLIC MapUpdateSummary
{
  MapUpdateType update_type;
  std::size_t num_added_pts;
};
using common::types::float32_t;

enum class Requires {};

template<typename LocalizerMapT>
struct LocalizationMapConstraint
{
  using Cloud = sensor_msgs::msg::PointCloud2;

  template<typename Map>
  using call_insert = decltype(std::declval<Map>().insert(std::declval<const Cloud &>()));

  template<typename Map>
  using call_clear = decltype(std::declval<Map>().clear());

  static_assert(
    ::autoware::common::helper_functions::expression_valid<call_insert, LocalizerMapT>::value,
    "The map should provide a `insert(msg)` method to be used for mapping");
  static_assert(
    ::autoware::common::helper_functions::expression_valid<call_clear, LocalizerMapT>::value,
    "The map should provide a `clear()` method to be used for mapping");

  static constexpr Requires value{};
};

template<typename LocalizerMapT, Requires = LocalizationMapConstraint<LocalizerMapT>::value>
class POINT_CLOUD_MAPPING_PUBLIC DualVoxelMap
{
public:
  static constexpr auto NUM_FIELDS{4U};
  using Cloud = sensor_msgs::msg::PointCloud2;
  using CloudConstIt = sensor_msgs::PointCloud2ConstIterator<float32_t>;

  explicit DualVoxelMap(
    const perception::filters::voxel_grid::Config & grid_config,
    const std::string & frame_id,
    LocalizerMapT && localizer_map
  )
  : m_grid_config{grid_config}, m_frame_id{frame_id},
    m_localizer_map{std::forward<LocalizerMapT>(localizer_map)} {}

  MapUpdateSummary update(const Cloud & observation)
  {
    if (observation.header.frame_id != m_frame_id) {
      throw std::runtime_error("pointcloud map and the updates should be on the same frame");
    }

    const auto check_not_end = [](const auto & its) {
        return std::all_of(its.cbegin(), its.cend(), [](const auto & it) {return it != it.end();});
      };

    MapUpdateSummary ret{MapUpdateType::NO_CHANGE, 0U};

    std::array<CloudConstIt, NUM_FIELDS> obs_its = {
      CloudConstIt(observation, "x"),
      CloudConstIt(observation, "y"),
      CloudConstIt(observation, "z"),
      CloudConstIt(observation, "intensity")
    };

    ret.update_type = m_grid.empty() ? MapUpdateType::NEW : MapUpdateType::UPDATE;
    auto obs_idx = 0U;
    for (; check_not_end(obs_its); ++obs_idx) {
      common::types::PointXYZIF pt{*obs_its[0], *obs_its[1], *obs_its[2], *obs_its[3]};
      const auto pt_key = m_grid_config.index(pt);
      if (m_grid.size() >= capacity() &&
        m_grid.find(pt_key) == m_grid.end())
      {
        if (obs_idx == 0U) {
          ret.update_type = MapUpdateType::NO_CHANGE;
        } else {
          ret.update_type = MapUpdateType::PARTIAL_UPDATE;
        }
        break;
      }

      m_grid[pt_key].add_observation(pt);

      // Advance the iterators.
      std::for_each(obs_its.begin(), obs_its.end(), [](auto & it) {++it;});
    }
    m_localizer_map.insert(observation);
    ret.num_added_pts = obs_idx;
    return ret;
  }

  void write(const std::string & file_name_prefix) const
  {
    // TODO(yunus.caliskan) Remove dynamic allocations.
    // pcl cloud is constructed here and the map is copied to it for sake
    // of simplicity to be able to use the pcl library's pcd writer.
    pcl::PointCloud<pcl::PointXYZI> cloud;
    cloud.reserve(m_grid.size());
    for (const auto & vx : m_grid) {
      pcl::PointXYZI pt;
      const auto & vx_pt = vx.second.get();
      pt.x = vx_pt.x;
      pt.y = vx_pt.y;
      pt.z = vx_pt.z;
      pt.intensity = vx_pt.intensity;
      cloud.push_back(pt);
    }
    pcl::io::savePCDFile(file_name_prefix + ".pcd", cloud);
  }

  std::size_t size() const noexcept
  {
    return m_grid.size();
  }
  std::size_t capacity() const noexcept
  {
    return m_grid_config.get_capacity();
  }
  void clear()
  {
    m_grid.clear();
    m_localizer_map.clear();
  }
  const LocalizerMapT & localizer_map() const noexcept
  {
    return m_localizer_map;
  }
  const std::string & frame_id() const noexcept
  {
    return m_frame_id;
  }

  bool empty()
  {
    return m_grid.empty();
  }

private:
  perception::filters::voxel_grid::Config m_grid_config;
  std::unordered_map<uint64_t,
    perception::filters::voxel_grid::CentroidVoxel<common::types::PointXYZIF>> m_grid;
  std::string m_frame_id;
  LocalizerMapT m_localizer_map;
};
}  // namespace point_cloud_mapping
}  // namespace mapping
}  // namespace autoware

#endif  // POINT_CLOUD_MAPPING__POINT_CLOUD_MAP_HPP_