merging_pipeline.cpp
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 
40 #include <ros/assert.h>
41 #include <ros/console.h>
42 #include <opencv2/stitching/detail/matchers.hpp>
43 #include <opencv2/stitching/detail/motion_estimators.hpp>
44 #include "estimation_internal.h"
45 
46 namespace combine_grids
47 {
49  double confidence)
50 {
51  std::vector<cv::detail::ImageFeatures> image_features;
52  std::vector<cv::detail::MatchesInfo> pairwise_matches;
53  std::vector<cv::detail::CameraParams> transforms;
54  std::vector<int> good_indices;
55  // TODO investigate value translation effect on features
56  cv::Ptr<cv::detail::FeaturesFinder> finder =
57  internal::chooseFeatureFinder(feature_type);
58  cv::Ptr<cv::detail::FeaturesMatcher> matcher =
59  cv::makePtr<cv::detail::AffineBestOf2NearestMatcher>();
60  cv::Ptr<cv::detail::Estimator> estimator =
61  cv::makePtr<cv::detail::AffineBasedEstimator>();
62  cv::Ptr<cv::detail::BundleAdjusterBase> adjuster =
63  cv::makePtr<cv::detail::BundleAdjusterAffinePartial>();
64 
65  if (images_.empty()) {
66  return true;
67  }
68 
69  /* find features in images */
70  ROS_DEBUG("computing features");
71  image_features.reserve(images_.size());
72  for (const cv::Mat& image : images_) {
73  image_features.emplace_back();
74  if (!image.empty()) {
75  (*finder)(image, image_features.back());
76  }
77  }
78  finder->collectGarbage();
79 
80  /* find corespondent features */
81  ROS_DEBUG("pairwise matching features");
82  (*matcher)(image_features, pairwise_matches);
83  matcher->collectGarbage();
84 
85 #ifndef NDEBUG
86  internal::writeDebugMatchingInfo(images_, image_features, pairwise_matches);
87 #endif
88 
89  /* use only matches that has enough confidence. leave out matches that are not
90  * connected (small components) */
91  good_indices = cv::detail::leaveBiggestComponent(
92  image_features, pairwise_matches, static_cast<float>(confidence));
93 
94  /* estimate transform */
95  ROS_DEBUG("calculating transforms in global reference frame");
96  // note: currently used estimator never fails
97  if (!(*estimator)(image_features, pairwise_matches, transforms)) {
98  return false;
99  }
100 
101  /* levmarq optimization */
102  // openCV just accepts float transforms
103  for (auto& transform : transforms) {
104  transform.R.convertTo(transform.R, CV_32F);
105  }
106  ROS_DEBUG("optimizing global transforms");
107  adjuster->setConfThresh(confidence);
108  if (!(*adjuster)(image_features, pairwise_matches, transforms)) {
109  ROS_WARN("Bundle adjusting failed. Could not estimate transforms.");
110  return false;
111  }
112 
113  transforms_.clear();
114  transforms_.resize(images_.size());
115  size_t i = 0;
116  for (auto& j : good_indices) {
117  // we want to work with transforms as doubles
118  transforms[i].R.convertTo(transforms_[static_cast<size_t>(j)], CV_64F);
119  ++i;
120  }
121 
122  return true;
123 }
124 
125 nav_msgs::OccupancyGrid::Ptr MergingPipeline::composeGrids()
126 {
127  ROS_ASSERT(images_.size() == transforms_.size());
128  ROS_ASSERT(images_.size() == grids_.size());
129 
130  if (images_.empty()) {
131  return nullptr;
132  }
133 
134  ROS_DEBUG("warping grids");
135  internal::GridWarper warper;
136  std::vector<cv::Mat> imgs_warped;
137  imgs_warped.reserve(images_.size());
138  std::vector<cv::Rect> rois;
139  rois.reserve(images_.size());
140 
141  for (size_t i = 0; i < images_.size(); ++i) {
142  if (!transforms_[i].empty() && !images_[i].empty()) {
143  imgs_warped.emplace_back();
144  rois.emplace_back(
145  warper.warp(images_[i], transforms_[i], imgs_warped.back()));
146  }
147  }
148 
149  if (imgs_warped.empty()) {
150  return nullptr;
151  }
152 
153  ROS_DEBUG("compositing result grid");
154  nav_msgs::OccupancyGrid::Ptr result;
155  internal::GridCompositor compositor;
156  result = compositor.compose(imgs_warped, rois);
157  result->info.map_load_time = ros::Time::now();
158  // TODO is this correct?
159  result->info.resolution = grids_[0]->info.resolution;
160 
161  return result;
162 }
163 
164 std::vector<geometry_msgs::Transform> MergingPipeline::getTransforms() const
165 {
166  std::vector<geometry_msgs::Transform> result;
167  result.reserve(transforms_.size());
168 
169  for (auto& transform : transforms_) {
170  if (transform.empty()) {
171  result.emplace_back();
172  continue;
173  }
174 
175  ROS_ASSERT(transform.type() == CV_64F);
176  geometry_msgs::Transform ros_transform;
177  ros_transform.translation.x = transform.at<double>(0, 2);
178  ros_transform.translation.y = transform.at<double>(1, 2);
179  ros_transform.translation.z = 0.;
180 
181  // our rotation is in fact only 2D, thus quaternion can be simplified
182  double a = transform.at<double>(0, 0);
183  double b = transform.at<double>(1, 0);
184  ros_transform.rotation.w = std::sqrt(2. + 2. * a) * 0.5;
185  ros_transform.rotation.x = 0.;
186  ros_transform.rotation.y = 0.;
187  ros_transform.rotation.z = std::copysign(std::sqrt(2. - 2. * a) * 0.5, b);
188 
189  result.push_back(ros_transform);
190  }
191 
192  return result;
193 }
194 
195 } // namespace combine_grids
std::vector< cv::Mat > transforms_
std::vector< cv::Mat > images_
nav_msgs::OccupancyGrid::Ptr composeGrids()
cv::Rect warp(const cv::Mat &grid, const cv::Mat &transform, cv::Mat &warped_grid)
Definition: grid_warper.cpp:47
static void writeDebugMatchingInfo(const std::vector< cv::Mat > &images, const std::vector< cv::detail::ImageFeatures > &image_features, const std::vector< cv::detail::MatchesInfo > &pairwise_matches)
std::vector< geometry_msgs::Transform > getTransforms() const
#define ROS_WARN(...)
bool estimateTransforms(FeatureType feature=FeatureType::AKAZE, double confidence=1.0)
nav_msgs::OccupancyGrid::Ptr compose(const std::vector< cv::Mat > &grids, const std::vector< cv::Rect > &rois)
static Time now()
static cv::Ptr< cv::detail::FeaturesFinder > chooseFeatureFinder(FeatureType type)
std::vector< nav_msgs::OccupancyGrid::ConstPtr > grids_
#define ROS_ASSERT(cond)
#define ROS_DEBUG(...)


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