grid_compositor.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 
38 
39 #include <opencv2/stitching/detail/util.hpp>
40 
41 #include <ros/assert.h>
42 
43 namespace combine_grids
44 {
45 namespace internal
46 {
47 nav_msgs::OccupancyGrid::Ptr GridCompositor::compose(
48  const std::vector<cv::Mat>& grids, const std::vector<cv::Rect>& rois)
49 {
50  ROS_ASSERT(grids.size() == rois.size());
51 
52  nav_msgs::OccupancyGrid::Ptr result_grid(new nav_msgs::OccupancyGrid());
53 
54  std::vector<cv::Point> corners;
55  corners.reserve(grids.size());
56  std::vector<cv::Size> sizes;
57  sizes.reserve(grids.size());
58  for (auto& roi : rois) {
59  corners.push_back(roi.tl());
60  sizes.push_back(roi.size());
61  }
62  cv::Rect dst_roi = cv::detail::resultRoi(corners, sizes);
63 
64  result_grid->info.width = static_cast<uint>(dst_roi.width);
65  result_grid->info.height = static_cast<uint>(dst_roi.height);
66  result_grid->data.resize(static_cast<size_t>(dst_roi.area()), -1);
67  // create view for opencv pointing to newly allocated grid
68  cv::Mat result(dst_roi.size(), CV_8S, result_grid->data.data());
69 
70  for (size_t i = 0; i < grids.size(); ++i) {
71  // we need to compensate global offset
72  cv::Rect roi = cv::Rect(corners[i] - dst_roi.tl(), sizes[i]);
73  cv::Mat result_roi(result, roi);
74  // reinterpret warped matrix as signed
75  // we will not change this matrix, but opencv does not support const matrices
76  cv::Mat warped_signed (grids[i].size(), CV_8S, const_cast<uchar*>(grids[i].ptr()));
77  // compose img into result matrix
78  cv::max(result_roi, warped_signed, result_roi);
79  }
80 
81  return result_grid;
82 }
83 
84 } // namespace internal
85 
86 } // namespace combine_grids
nav_msgs::OccupancyGrid::Ptr compose(const std::vector< cv::Mat > &grids, const std::vector< cv::Rect > &rois)
#define ROS_ASSERT(cond)


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