m-explore-ros2/map_merge/include/combine_grids/merging_pipeline.h

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015-2016, Jiri Horner.
 *  Copyright (c) 2021, Carlos Alvarez. 
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Jiri Horner nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 *********************************************************************/

#ifndef MERGING_PIPELINE_H_
#define MERGING_PIPELINE_H_

#include <vector>

#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>

#include <opencv2/core/utility.hpp>

namespace combine_grids
{
enum class FeatureType { AKAZE, ORB, SURF };

/**
 * @brief Pipeline for merging overlapping occupancy grids
 * @details Pipeline works on internally stored grids.
 */
class MergingPipeline
{
public:
  template <typename InputIt>
  void feed(InputIt grids_begin, InputIt grids_end);
  bool estimateTransforms(FeatureType feature = FeatureType::AKAZE,
                          double confidence = 1.0);
  nav_msgs::msg::OccupancyGrid::SharedPtr composeGrids();

  std::vector<geometry_msgs::msg::Transform> getTransforms() const;
  template <typename InputIt>
  bool setTransforms(InputIt transforms_begin, InputIt transforms_end);

private:
  std::vector<nav_msgs::msg::OccupancyGrid::ConstSharedPtr> grids_;
  std::vector<cv::Mat> images_;
  std::vector<cv::Mat> transforms_;
  double max_confidence_achieved_ = 0.0;
};

template <typename InputIt>
void MergingPipeline::feed(InputIt grids_begin, InputIt grids_end)
{
  static_assert(std::is_assignable<nav_msgs::msg::OccupancyGrid::ConstSharedPtr&,
                                   decltype(*grids_begin)>::value,
                "grids_begin must point to nav_msgs::msg::OccupancyGrid::ConstSharedPtr "
                "data");

  // we can't reserve anything, because we want to support just InputIt and
  // their guarantee validity for only single-pass algos
  images_.clear();
  grids_.clear();
  for (InputIt it = grids_begin; it != grids_end; ++it) {
    if (*it && !(*it)->data.empty()) {
      grids_.push_back(*it);
      /* convert to opencv images. it creates only a view for opencv and does
       * not copy or own actual data. */
      images_.emplace_back((*it)->info.height, (*it)->info.width, CV_8UC1,
                           const_cast<signed char*>((*it)->data.data()));
    } else {
      grids_.emplace_back();
      images_.emplace_back();
    }
  }
}

template <typename InputIt>
bool MergingPipeline::setTransforms(InputIt transforms_begin,
                                    InputIt transforms_end)
{
  static_assert(std::is_assignable<geometry_msgs::msg::Transform&,
                                   decltype(*transforms_begin)>::value,
                "transforms_begin must point to geometry_msgs::msg::Transform "
                "data");

  decltype(transforms_) transforms_buf;
  for (InputIt it = transforms_begin; it != transforms_end; ++it) {
    const geometry_msgs::msg::Quaternion& q = it->rotation;
    if ((q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) <
        std::numeric_limits<double>::epsilon()) {
      // represents invalid transform
      transforms_buf.emplace_back();
      continue;
    }
    double s = 2.0 / (q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
    double a = 1 - q.y * q.y * s - q.z * q.z * s;
    double b = q.x * q.y * s + q.z * q.w * s;
    double tx = it->translation.x;
    double ty = it->translation.y;
    cv::Mat transform = cv::Mat::eye(3, 3, CV_64F);
    transform.at<double>(0, 0) = transform.at<double>(1, 1) = a;
    transform.at<double>(1, 0) = b;
    transform.at<double>(0, 1) = -b;
    transform.at<double>(0, 2) = tx;
    transform.at<double>(1, 2) = ty;

    transforms_buf.emplace_back(std::move(transform));
  }

  if (transforms_buf.size() != images_.size()) {
    return false;
  }
  std::swap(transforms_, transforms_buf);

  return true;
}

}  // namespace combine_grids

#endif  // MERGING_PIPELINE_H_