ndt_localizer.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_LOCALIZER_HPP_
#define NDT__NDT_LOCALIZER_HPP_

#include <helper_functions/template_utils.hpp>
#include <localization_common/optimized_registration_summary.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <ndt/ndt_common.hpp>
#include <ndt/ndt_optimization_problem.hpp>
#include <ndt/constraints.hpp>
#include <optimization/optimizer_options.hpp>
#include <experimental/optional>
#include <utility>
#include <string>

namespace autoware
{
namespace localization
{
namespace ndt
{
using CloudT = sensor_msgs::msg::PointCloud2;

template<
  typename ScanT,
  typename NDTOptimizationProblemT,
  typename OptimizationProblemConfigT,
  typename OptimizerT>
class NDT_PUBLIC NDTLocalizerBase
{
public:
  using Transform = geometry_msgs::msg::TransformStamped;
  using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
  using Summary = localization_common::OptimizedRegistrationSummary;

  explicit NDTLocalizerBase(
    const NDTLocalizerConfigBase & config,
    const OptimizationProblemConfigT & optimization_problem_config,
    const OptimizerT & optimizer,
    ScanT && scan)
  : m_config{config},
    m_optimization_problem_config{optimization_problem_config},
    m_optimizer{optimizer},
    m_scan{std::forward<ScanT>(scan)} {}

  template<typename MapT,
    Requires = traits::LocalizationMapConstraint<MapT>::value>
  PoseWithCovarianceStamped register_measurement(
    const CloudT & msg,
    const Transform & transform_initial,
    const MapT & map,
    Summary * const summary = nullptr)
  {
    PoseWithCovarianceStamped pose_out{};
    validate_msg(msg, map);
    validate_guess(msg, transform_initial);
    // Initial checks passed, proceed with initialization
    // Eigen representations to be used for internal computations.
    EigenPose<Real> eig_pose_initial, eig_pose_result;
    eig_pose_initial.setZero();
    eig_pose_result.setZero();
    // Convert the ros transform/pose to eigen pose vector
    transform_adapters::transform_to_pose(transform_initial.transform, eig_pose_initial);

    // Set the scan
    m_scan.clear();
    m_scan.insert(msg);

    // Define and solve the problem.
    NDTOptimizationProblemT problem(m_scan, map, m_optimization_problem_config);
    const auto opt_summary = m_optimizer.solve(problem, eig_pose_initial, eig_pose_result);

    if (opt_summary.termination_type() == common::optimization::TerminationType::FAILURE) {
      throw std::runtime_error(
              "NDT localizer has likely encountered a numerical "
              "error during optimization.");
    }

    // Convert eigen pose back to ros pose/transform
    transform_adapters::pose_to_transform(
      eig_pose_result,
      pose_out.pose.pose);

    pose_out.header.stamp = msg.header.stamp;
    pose_out.header.frame_id = map.frame_id();

    // Populate covariance information. It is implementation defined.
    set_covariance(problem, eig_pose_initial, eig_pose_result, pose_out);
    if (summary != nullptr) {
      *summary = localization_common::OptimizedRegistrationSummary{opt_summary};
    }
    return pose_out;
  }


  const ScanT & scan() const noexcept
  {
    return m_scan;
  }
  const OptimizerT & optimizer() const noexcept
  {
    return m_optimizer;
  }
  const NDTLocalizerConfigBase & config() const noexcept
  {
    return m_config;
  }
  const OptimizationProblemConfigT & optimization_problem_config() const noexcept
  {
    return m_optimization_problem_config;
  }

protected:
  virtual void set_covariance(
    const NDTOptimizationProblemT & problem,
    const EigenPose<Real> & initial_guess,
    const EigenPose<Real> & pose_result,
    PoseWithCovarianceStamped & solution) const
  {
    (void) problem;
    (void) initial_guess;
    (void) pose_result;
    (void) solution;
    // For now, do nothing.
  }

  template<typename MapT>
  void validate_msg(const CloudT & msg, const MapT & map) const
  {
    const auto message_time = ::time_utils::from_message(msg.header.stamp);
    // Map shouldn't be newer than a measurement.
    if (message_time < map.stamp()) {
      throw std::logic_error(
              "Lidar scan should not have a timestamp older than the timestamp of"
              "the current map.");
    }
  }

  virtual void validate_guess(const CloudT & msg, const Transform & transform_initial) const
  {
    const auto message_time = ::time_utils::from_message(msg.header.stamp);

    const auto guess_scan_diff = ::time_utils::from_message(transform_initial.header.stamp) -
      message_time;
    const auto stamp_tol = m_config.guess_time_tolerance();
    // An initial estimate should be comparable in time to the measurement's time stamp
    if (std::abs(
        std::chrono::duration_cast<std::decay_t<decltype(stamp_tol)>>(guess_scan_diff).
        count()) >
      std::abs(stamp_tol.count()))
    {
      throw std::domain_error(
              "Initial guess is not within: " + std::to_string(stamp_tol.count()) +
              "ns range of the scan's time stamp. Either increase the tolerance range or"
              "make sure the localizer takes in timely initial pose guesses.");
    }
  }

private:
  NDTLocalizerConfigBase m_config;
  OptimizationProblemConfigT m_optimization_problem_config;
  OptimizerT m_optimizer;
  ScanT m_scan;
};

template<typename OptimizerT, typename MapT = StaticNDTMap>
class NDT_PUBLIC P2DNDTLocalizer : public NDTLocalizerBase<
    P2DNDTScan, P2DNDTOptimizationProblem<MapT>, P2DNDTOptimizationConfig, OptimizerT>
{
public:
  using CloudT = sensor_msgs::msg::PointCloud2;
  using ParentT = NDTLocalizerBase<
    P2DNDTScan, P2DNDTOptimizationProblem<MapT>, P2DNDTOptimizationConfig, OptimizerT>;
  using Transform = typename ParentT::Transform;
  using PoseWithCovarianceStamped = typename ParentT::PoseWithCovarianceStamped;
  using ScanT = P2DNDTScan;

  explicit P2DNDTLocalizer(
    const P2DNDTLocalizerConfig & config,
    const OptimizerT & optimizer,
    const Real outlier_ratio)
  : ParentT{
      config,
      P2DNDTOptimizationConfig{outlier_ratio},
      optimizer,
      ScanT{config.scan_capacity()}} {}

protected:
  void set_covariance(
    const P2DNDTOptimizationProblem<MapT> &,
    const EigenPose<Real> &,
    const EigenPose<Real> &,
    PoseWithCovarianceStamped &) const override
  {
    // For now, do nothing.
  }
};

}  // namespace ndt
}  // namespace localization
}  // namespace autoware

#endif  // NDT__NDT_LOCALIZER_HPP_