merging_pipeline.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015-2016, Jiri Horner.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Jiri Horner nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  *********************************************************************/
36 
37 #ifndef MERGING_PIPELINE_H_
38 #define MERGING_PIPELINE_H_
39 
40 #include <vector>
41 
42 #include <geometry_msgs/Transform.h>
43 #include <nav_msgs/OccupancyGrid.h>
44 
45 #include <opencv2/core/utility.hpp>
46 
47 namespace combine_grids
48 {
49 enum class FeatureType { AKAZE, ORB, SURF };
50 
56 {
57 public:
58  template <typename InputIt>
59  void feed(InputIt grids_begin, InputIt grids_end);
60  bool estimateTransforms(FeatureType feature = FeatureType::AKAZE,
61  double confidence = 1.0);
62  nav_msgs::OccupancyGrid::Ptr composeGrids();
63 
64  std::vector<geometry_msgs::Transform> getTransforms() const;
65  template <typename InputIt>
66  bool setTransforms(InputIt transforms_begin, InputIt transforms_end);
67 
68 private:
69  std::vector<nav_msgs::OccupancyGrid::ConstPtr> grids_;
70  std::vector<cv::Mat> images_;
71  std::vector<cv::Mat> transforms_;
72 };
73 
74 template <typename InputIt>
75 void MergingPipeline::feed(InputIt grids_begin, InputIt grids_end)
76 {
77  static_assert(std::is_assignable<nav_msgs::OccupancyGrid::ConstPtr&,
78  decltype(*grids_begin)>::value,
79  "grids_begin must point to nav_msgs::OccupancyGrid::ConstPtr "
80  "data");
81 
82  // we can't reserve anything, because we want to support just InputIt and
83  // their guarantee validity for only single-pass algos
84  images_.clear();
85  grids_.clear();
86  for (InputIt it = grids_begin; it != grids_end; ++it) {
87  if (*it && !(*it)->data.empty()) {
88  grids_.push_back(*it);
89  /* convert to opencv images. it creates only a view for opencv and does
90  * not copy or own actual data. */
91  images_.emplace_back((*it)->info.height, (*it)->info.width, CV_8UC1,
92  const_cast<signed char*>((*it)->data.data()));
93  } else {
94  grids_.emplace_back();
95  images_.emplace_back();
96  }
97  }
98 }
99 
100 template <typename InputIt>
101 bool MergingPipeline::setTransforms(InputIt transforms_begin,
102  InputIt transforms_end)
103 {
104  static_assert(std::is_assignable<geometry_msgs::Transform&,
105  decltype(*transforms_begin)>::value,
106  "transforms_begin must point to geometry_msgs::Transform "
107  "data");
108 
109  decltype(transforms_) transforms_buf;
110  for (InputIt it = transforms_begin; it != transforms_end; ++it) {
111  const geometry_msgs::Quaternion& q = it->rotation;
112  if ((q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w) <
113  std::numeric_limits<double>::epsilon()) {
114  // represents invalid transform
115  transforms_buf.emplace_back();
116  continue;
117  }
118  double s = 2.0 / (q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
119  double a = 1 - q.y * q.y * s - q.z * q.z * s;
120  double b = q.x * q.y * s + q.z * q.w * s;
121  double tx = it->translation.x;
122  double ty = it->translation.y;
123  cv::Mat transform = cv::Mat::eye(3, 3, CV_64F);
124  transform.at<double>(0, 0) = transform.at<double>(1, 1) = a;
125  transform.at<double>(1, 0) = b;
126  transform.at<double>(0, 1) = -b;
127  transform.at<double>(0, 2) = tx;
128  transform.at<double>(1, 2) = ty;
129 
130  transforms_buf.emplace_back(std::move(transform));
131  }
132 
133  if (transforms_buf.size() != images_.size()) {
134  return false;
135  }
136  std::swap(transforms_, transforms_buf);
137 
138  return true;
139 }
140 
141 } // namespace combine_grids
142 
143 #endif // MERGING_PIPELINE_H_
std::vector< cv::Mat > transforms_
std::vector< cv::Mat > images_
Pipeline for merging overlapping occupancy grids.
XmlRpcServer s
bool setTransforms(InputIt transforms_begin, InputIt transforms_end)
void feed(InputIt grids_begin, InputIt grids_end)
std::vector< nav_msgs::OccupancyGrid::ConstPtr > grids_


map_merge
Author(s): Jiri Horner
autogenerated on Mon Jun 10 2019 13:56:52