test_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 
38 #include <gtest/gtest.h>
39 #include <ros/console.h>
41 #include <opencv2/core/utility.hpp>
42 #include "testing_helpers.h"
43 
44 #define private public
46 
47 const std::array<const char*, 2> hector_maps = {
48  "map00.pgm", "map05.pgm",
49 };
50 
51 const std::array<const char*, 2> gmapping_maps = {
52  "2011-08-09-12-22-52.pgm", "2012-01-28-11-12-01.pgm",
53 };
54 
55 constexpr bool verbose_tests = false;
56 
57 TEST(MergingPipeline, canStich0Grid)
58 {
59  std::vector<nav_msgs::OccupancyGridConstPtr> maps;
61  merger.feed(maps.begin(), maps.end());
62  EXPECT_TRUE(merger.estimateTransforms());
63  EXPECT_EQ(merger.composeGrids(), nullptr);
64  EXPECT_EQ(merger.getTransforms().size(), 0);
65 }
66 
67 TEST(MergingPipeline, canStich1Grid)
68 {
69  auto map = loadMap(hector_maps[1]);
71  merger.feed(&map, &map + 1);
72  merger.estimateTransforms();
73  auto merged_grid = merger.composeGrids();
74 
75  // sanity of merged grid
76  ASSERT_TRUE(static_cast<bool>(merged_grid));
77  EXPECT_FALSE(merged_grid->data.empty());
78  EXPECT_EQ((merged_grid->info.width) * (merged_grid->info.height),
79  merged_grid->data.size());
80  // merged must be the same with original
81  EXPECT_EQ(merged_grid->info.width, map->info.width);
82  EXPECT_EQ(merged_grid->info.height, map->info.height);
83  EXPECT_EQ(merged_grid->data.size(), map->data.size());
84  for (size_t i = 0; i < merged_grid->data.size(); ++i) {
85  EXPECT_EQ(merged_grid->data[i], map->data[i]);
86  }
87  // check estimated transforms
88  auto transforms = merger.getTransforms();
89  EXPECT_EQ(transforms.size(), 1);
91  tf2::fromMsg(transforms[0], t);
92  EXPECT_EQ(tf2::Transform::getIdentity(), t);
93 }
94 
95 TEST(MergingPipeline, canStich2Grids)
96 {
97  auto maps = loadMaps(hector_maps.begin(), hector_maps.end());
99  merger.feed(maps.begin(), maps.end());
100  merger.estimateTransforms();
101  auto merged_grid = merger.composeGrids();
102 
103  // sanity of merged grid
104  ASSERT_TRUE(static_cast<bool>(merged_grid));
105  EXPECT_FALSE(merged_grid->data.empty());
106  EXPECT_EQ((merged_grid->info.width) * (merged_grid->info.height),
107  merged_grid->data.size());
108  // grid size should indicate sucessful merge
109  EXPECT_NEAR(2091, merged_grid->info.width, 30);
110  EXPECT_NEAR(2091, merged_grid->info.height, 30);
111 
112  if (verbose_tests) {
113  saveMap("test_canStich2Grids.pgm", merged_grid);
114  }
115 }
116 
117 TEST(MergingPipeline, canStichGridsGmapping)
118 {
119  auto maps = loadMaps(gmapping_maps.begin(), gmapping_maps.end());
121  merger.feed(maps.begin(), maps.end());
122  merger.estimateTransforms();
123  auto merged_grid = merger.composeGrids();
124 
125  // sanity of merged grid
126  ASSERT_TRUE(static_cast<bool>(merged_grid));
127  EXPECT_FALSE(merged_grid->data.empty());
128  EXPECT_EQ((merged_grid->info.width) * (merged_grid->info.height),
129  merged_grid->data.size());
130  // grid size should indicate sucessful merge
131  EXPECT_NEAR(5427, merged_grid->info.width, 30);
132  EXPECT_NEAR(5427, merged_grid->info.height, 30);
133 
134  if (verbose_tests) {
135  saveMap("canStichGridsGmapping.pgm", merged_grid);
136  }
137 }
138 
139 TEST(MergingPipeline, estimationAccuracy)
140 {
141  // for this test we measure estimation on the same grid artificially
142  // transformed
143  double angle = 0.523599 /* 30 deg in rads*/;
144  // TODO test also translations
145  double tx = 0;
146  double ty = 0;
147  cv::Matx23d transform{std::cos(angle), -std::sin(angle), tx,
148  std::sin(angle), std::cos(angle), ty};
149 
150  auto map = loadMap(hector_maps[1]);
152  merger.feed(&map, &map + 1);
153 
154  // warp the map with Affine Transform
156  cv::Mat warped;
157  auto roi = warper.warp(merger.images_[0], cv::Mat(transform), warped);
158 
159  // add warped map
160  // this relies on internal implementation of merging pipeline
161  merger.grids_.emplace_back();
162  merger.images_.push_back(warped);
163 
164  merger.estimateTransforms();
165  auto merged_grid = merger.composeGrids();
166 
167  // sanity of merged grid
168  ASSERT_TRUE(static_cast<bool>(merged_grid));
169  EXPECT_FALSE(merged_grid->data.empty());
170  EXPECT_EQ((merged_grid->info.width) * (merged_grid->info.height),
171  merged_grid->data.size());
172  // transforms
173  auto transforms = merger.getTransforms();
174  EXPECT_EQ(transforms.size(), 2);
176  tf2::fromMsg(transforms[0], t);
177  EXPECT_EQ(tf2::Transform::getIdentity(), t);
178  tf2::fromMsg(transforms[1], t);
179 
180  EXPECT_NEAR(angle, t.getRotation().getAngle(), 1e-2);
181  EXPECT_NEAR(tx - roi.tl().x, t.getOrigin().x(), 2);
182  EXPECT_NEAR(ty - roi.tl().y, t.getOrigin().y(), 2);
183 }
184 
185 TEST(MergingPipeline, transformsRoundTrip)
186 {
187  auto map = loadMap(hector_maps[0]);
189  merger.feed(&map, &map + 1);
190  for (size_t i = 0; i < 1000; ++i) {
191  auto t = randomTransform();
192  auto in_transform = toMsg(t);
193  // normalize input quaternion such that w > 0 (q and -q represents the same
194  // transformation)
195  if (in_transform.rotation.w < 0.) {
196  in_transform.rotation.x *= -1.;
197  in_transform.rotation.y *= -1.;
198  in_transform.rotation.z *= -1.;
199  in_transform.rotation.w *= -1.;
200  }
201  merger.setTransforms(&in_transform, &in_transform + 1);
202 
203  auto out_transforms = merger.getTransforms();
204  ASSERT_EQ(out_transforms.size(), 1);
205  auto out_transform = out_transforms[0];
206  EXPECT_FLOAT_EQ(in_transform.translation.x, out_transform.translation.x);
207  EXPECT_FLOAT_EQ(in_transform.translation.y, out_transform.translation.y);
208  EXPECT_FLOAT_EQ(in_transform.translation.z, out_transform.translation.z);
209  EXPECT_FLOAT_EQ(in_transform.rotation.x, out_transform.rotation.x);
210  EXPECT_FLOAT_EQ(in_transform.rotation.y, out_transform.rotation.y);
211  EXPECT_FLOAT_EQ(in_transform.rotation.z, out_transform.rotation.z);
212  EXPECT_FLOAT_EQ(in_transform.rotation.w, out_transform.rotation.w);
213  }
214 }
215 
216 TEST(MergingPipeline, setTransformsInternal)
217 {
218  auto map = loadMap(hector_maps[0]);
220  merger.feed(&map, &map + 1);
221 
222  for (size_t i = 0; i < 1000; ++i) {
223  auto transform = randomTransform();
224  geometry_msgs::Transform t = toMsg(transform);
225  merger.setTransforms(&t, &t + 1);
226 
227  ASSERT_EQ(merger.transforms_.size(), 1);
228  auto& transform_internal = merger.transforms_[0];
229  // verify that transforms are the same in 2D
230  tf2::Vector3 a[2] = {{1., 0., 1.}, {0., 1., 1.}};
231  cv::Point3d b[2] = {{1., 0., 1.}, {0., 1., 1.}};
232  for (auto j : {0, 1}) {
233  auto p1 = transform * a[j];
234  cv::Mat p2 = transform_internal * cv::Mat(b[j]);
235  // some precision is naturally lost during conversion, float precision is
236  // still good for us
237  EXPECT_FLOAT_EQ(p1.x(), p2.at<double>(0));
238  EXPECT_FLOAT_EQ(p1.y(), p2.at<double>(1));
239  }
240  }
241 }
242 
243 TEST(MergingPipeline, getTransformsInternal)
244 {
245  auto map = loadMap(hector_maps[0]);
247  merger.feed(&map, &map + 1);
248 
249  // set internal transform
250  merger.transforms_.resize(1);
251  for (size_t i = 0; i < 1000; ++i) {
252  cv::Mat transform_internal = randomTransformMatrix();
253  merger.transforms_[0] = transform_internal;
254  auto transforms = merger.getTransforms();
255  ASSERT_EQ(transforms.size(), 1);
256  // output quaternion should be normalized
257  auto& q = transforms[0].rotation;
258  EXPECT_DOUBLE_EQ(1., q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
259 
260  // verify that transforms are the same in 2D
261  tf2::Transform transform;
262  fromMsg(transforms[0], transform);
263  tf2::Vector3 a[2] = {{1., 0., 1.}, {0., 1., 1.}};
264  cv::Point3d b[2] = {{1., 0., 1.}, {0., 1., 1.}};
265  for (auto j : {0, 1}) {
266  auto p1 = transform * a[j];
267  cv::Mat p2 = transform_internal * cv::Mat(b[j]);
268  EXPECT_FLOAT_EQ(p1.x(), p2.at<double>(0));
269  EXPECT_FLOAT_EQ(p1.y(), p2.at<double>(1));
270  }
271  }
272 }
273 
274 TEST(MergingPipeline, setEmptyTransforms)
275 {
276  constexpr size_t size = 2;
277  std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
278  std::vector<geometry_msgs::Transform> transforms(size);
280  merger.feed(maps.begin(), maps.end());
281  merger.setTransforms(transforms.begin(), transforms.end());
282  EXPECT_EQ(merger.composeGrids(), nullptr);
283  EXPECT_EQ(merger.getTransforms().size(), size);
284 }
285 
286 /* empty image may end with identity transform. */
287 TEST(MergingPipeline, emptyImageWithTransform)
288 {
289  constexpr size_t size = 1;
290  std::vector<nav_msgs::OccupancyGridConstPtr> maps(size);
291  std::vector<geometry_msgs::Transform> transforms(size);
292  transforms[0].rotation.z = 1; // set transform to identity
294  merger.feed(maps.begin(), maps.end());
295  merger.setTransforms(transforms.begin(), transforms.end());
296  EXPECT_EQ(merger.composeGrids(), nullptr);
297  EXPECT_EQ(merger.getTransforms().size(), size);
298 }
299 
300 int main(int argc, char** argv)
301 {
302  ros::Time::init();
303  if (verbose_tests &&
307  }
308  testing::InitGoogleTest(&argc, argv);
309  return RUN_ALL_TESTS();
310 }
std::vector< cv::Mat > transforms_
std::vector< nav_msgs::OccupancyGridConstPtr > loadMaps(InputIt filenames_begin, InputIt filenames_end)
ROSCONSOLE_DECL void notifyLoggerLevelsChanged()
std::vector< cv::Mat > images_
TF2SIMD_FORCE_INLINE tf2Scalar angle(const Quaternion &q1, const Quaternion &q2)
nav_msgs::OccupancyGrid::Ptr composeGrids()
Pipeline for merging overlapping occupancy grids.
cv::Rect warp(const cv::Mat &grid, const cv::Mat &transform, cv::Mat &warped_grid)
Definition: grid_warper.cpp:47
cv::Mat randomTransformMatrix()
constexpr bool verbose_tests
std::vector< geometry_msgs::Transform > getTransforms() const
geometry_msgs::TransformStamped t
nav_msgs::OccupancyGridConstPtr loadMap(const std::string &filename)
bool estimateTransforms(FeatureType feature=FeatureType::AKAZE, double confidence=1.0)
static void init()
void fromMsg(const A &, B &b)
bool setTransforms(InputIt transforms_begin, InputIt transforms_end)
static const Transform & getIdentity()
void feed(InputIt grids_begin, InputIt grids_end)
B toMsg(const A &a)
void saveMap(const std::string &filename, const nav_msgs::OccupancyGridConstPtr &map)
std::vector< nav_msgs::OccupancyGrid::ConstPtr > grids_
ROSCONSOLE_DECL bool set_logger_level(const std::string &name, levels::Level level)
TEST(MergingPipeline, canStich0Grid)
tf2::Transform randomTransform()
#define ROSCONSOLE_DEFAULT_NAME
const std::array< const char *, 2 > gmapping_maps
const std::array< const char *, 2 > hector_maps
int main(int argc, char **argv)


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