grid_map/grid_map_cv/test/GridMapCvProcessingTest.cpp at 3d7c95616796d218d859577c6afef64db11e6799 · ANYbotics/grid_map · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
/*
 * GridMapCvProcessingTest.cpp
 *
 *  Created on: May 3, 2017
 *      Author: Peter Fankhauser
 */

#include <grid_map_core/grid_map_core.hpp>
#include <grid_map_core/gtest_eigen.hpp>

// OpenCV
#include <cv_bridge/cv_bridge.h>

// gtest
#include <gtest/gtest.h>

#include "grid_map_cv/grid_map_cv.hpp"

TEST(GridMapCvProcessing, changeResolution)
{
  // Create grid map.
  grid_map::GridMap mapIn;
  mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01);
  mapIn.add("layer", 1.0);
  for (grid_map::CircleIterator iterator(mapIn, mapIn.getPosition(), 0.2); !iterator.isPastEnd();
    ++iterator)
  {
    mapIn.at("layer", *iterator) = 2.0;
  }

  // Change resolution.
  grid_map::GridMap mapOut;
  EXPECT_TRUE(grid_map::GridMapCvProcessing::changeResolution(mapIn, mapOut, 0.1));

  // Check data.
  EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all());
  EXPECT_TRUE(mapIn.getPosition() == mapOut.getPosition());
  EXPECT_TRUE((mapIn.getSize() == mapOut.getSize() * 10).all());
  EXPECT_EQ(mapIn["layer"](0, 0), mapOut["layer"](0, 0));  // Corner.
  EXPECT_EQ(
    mapIn.atPosition("layer", mapIn.getPosition()),
    mapOut.atPosition("layer", mapOut.getPosition()));  // Center.
}

TEST(GridMapCvProcessing, changeResolutionForMovedMap)
{
  // Create grid map.
  grid_map::GridMap mapIn;
  mapIn.setGeometry(grid_map::Length(2.0, 1.0), 0.01);
  grid_map::Position position(0.3, 0.4);
  mapIn.move(position);
  mapIn.add("layer", 1.0);
  for (grid_map::CircleIterator iterator(mapIn, position, 0.2); !iterator.isPastEnd(); ++iterator) {
    mapIn.at("layer", *iterator) = 2.0;
  }

  // Change resolution.
  grid_map::GridMap mapOut;
  EXPECT_TRUE(grid_map::GridMapCvProcessing::changeResolution(mapIn, mapOut, 0.1));

  // Check data.
  EXPECT_TRUE((mapIn.getLength() == mapOut.getLength()).all());
  EXPECT_TRUE(mapIn.getPosition() == mapOut.getPosition());
  EXPECT_TRUE((mapIn.getSize() == mapOut.getSize() * 10).all());
  EXPECT_EQ(mapIn["layer"](0, 0), mapOut["layer"](0, 0));  // Corner.
  EXPECT_EQ(
    mapIn.atPosition("layer", mapIn.getPosition()),
    mapOut.atPosition("layer", mapOut.getPosition()));  // Center.
}