Skip to content

Commit

Permalink
Merge branch 'fix/clip_points_to_map' into 'master'
Browse files Browse the repository at this point in the history 
[grid_map] Fix getClosestPositionInMap

GitOrigin-RevId: b7d32e76cec97e5c8d50b06aa17569ef085de0db
  • Loading branch information
@maximilianwulf @kjalloul-anybotics
maximilianwulf authored and kjalloul-anybotics committed Dec 3, 2024
1 parent c313843 commit 3be5358
Show file tree
Hide file tree
Showing 2 changed files with 210 additions and 5 deletions.
11 changes: 6 additions & 5 deletions grid_map_core/src/GridMap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -819,12 +819,13 @@ Position GridMap::getClosestPositionInMap(const Position & position) const
const double maxY = bottomLeftCorner.y();
const double minY = bottomRightCorner.y();

// Clip to box constraints.
positionInMap.x() = std::fmin(positionInMap.x(), maxX);
positionInMap.y() = std::fmin(positionInMap.y(), maxY);
// Clip to box constraints and correct for indexing precision.
// Points on the border can lead to invalid indices because the cells represent half open intervals, i.e. [...).
positionInMap.x() = std::fmin(positionInMap.x(), maxX - std::numeric_limits<double>::epsilon());
positionInMap.y() = std::fmin(positionInMap.y(), maxY - std::numeric_limits<double>::epsilon());

positionInMap.x() = std::fmax(positionInMap.x(), minX);
positionInMap.y() = std::fmax(positionInMap.y(), minY);
positionInMap.x() = std::fmax(positionInMap.x(), minX + std::numeric_limits<double>::epsilon());
positionInMap.y() = std::fmax(positionInMap.y(), minY + std::numeric_limits<double>::epsilon());

return positionInMap;
}
Expand Down
204 changes: 204 additions & 0 deletions grid_map_core/test/GridMapTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,6 +154,210 @@ TEST(GridMap, ClipToMap)
EXPECT_TRUE(map.isInside(clippedPositionOutMap));
}



TEST(GridMap, ClipToMap2)
{
GridMap map({"types"});
map.setGeometry(Length(1.0, 1.0), 0.05, Position(0.0, 0.0));

// Test 8 points outside of map.
/*
* A B C
* +---+
* | | X
* D| |E ^
* | | |
* +---+ Y<--+
* F G H
*
* Note: Position to index alignment is an half open interval.
* An example position of 0.5 is assigned to the upper index.
* The interval in the current example is:
* Position: [...)[0.485 ... 0.5)[0.5 ... 0.505)[...)
* Index: 8 9 10 11
*/

Index insideIndex;
Position outsidePosition;

// Point A
outsidePosition = Position(1.0, 1.0);
auto closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
bool isInside = map.getIndex(closestInsidePosition, insideIndex);

auto expectedPosition = Position(0.5, 0.5);
auto expectedIndex = Index(0, 0);

// Check position.
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());

// Check index.
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;

// Check if index is inside.
EXPECT_TRUE(isInside) << "position is: " << std::endl
<< closestInsidePosition << std::endl
<< " index is: " << std::endl
<< insideIndex << std::endl;

// Point B
outsidePosition = Position(1.0, 0.0);
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
isInside = map.getIndex(closestInsidePosition, insideIndex);

expectedPosition = Position(0.5, 0.0);
expectedIndex = Index(0, 10);

// Check position.
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());

// Check index.
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;

// Check if index is inside.
EXPECT_TRUE(isInside) << "position is: " << std::endl
<< closestInsidePosition << std::endl
<< " index is: " << std::endl
<< insideIndex << std::endl;

// Point C
outsidePosition = Position(1.0, -1.0);
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
isInside = map.getIndex(closestInsidePosition, insideIndex);

expectedPosition = Position(0.5, -0.5);
expectedIndex = Index(0, 19);

// Check position.
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());

// Check index.
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;

// Check if index is inside.
EXPECT_TRUE(isInside) << "position is: " << std::endl
<< closestInsidePosition << std::endl
<< " index is: " << std::endl
<< insideIndex << std::endl;

// Point D
outsidePosition = Position(0.0, 1.0);
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
isInside = map.getIndex(closestInsidePosition, insideIndex);

expectedPosition = Position(0.0, 0.5);
expectedIndex = Index(10, 0);

// Check position.
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());

// Check index.
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;

// Check if index is inside.
EXPECT_TRUE(isInside) << "position is: " << std::endl
<< closestInsidePosition << std::endl
<< " index is: " << std::endl
<< insideIndex << std::endl;

// Point E
outsidePosition = Position(0.0, -1.0);
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
isInside = map.getIndex(closestInsidePosition, insideIndex);

expectedPosition = Position(0.0, -0.5);
expectedIndex = Index(10, 19);

// Check position.
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());

// Check index.
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;

// Check if index is inside.
EXPECT_TRUE(isInside) << "position is: " << std::endl
<< closestInsidePosition << std::endl
<< " index is: " << std::endl
<< insideIndex << std::endl;

// Point F
outsidePosition = Position(-1.0, 1.0);
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
isInside = map.getIndex(closestInsidePosition, insideIndex);

expectedPosition = Position(-0.5, 0.5);
expectedIndex = Index(19, 0);

// Check position.
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());

// Check index.
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;

// Check if index is inside.
EXPECT_TRUE(isInside) << "position is: " << std::endl
<< closestInsidePosition << std::endl
<< " index is: " << std::endl
<< insideIndex << std::endl;

// Point G
outsidePosition = Position(-1.0, 0.0);
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
isInside = map.getIndex(closestInsidePosition, insideIndex);

expectedPosition = Position(-0.5, 0.0);
expectedIndex = Index(19, 10);

// Check position.
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());

// Check index.
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;

// Check if index is inside.
EXPECT_TRUE(isInside) << "position is: " << std::endl
<< closestInsidePosition << std::endl
<< " index is: " << std::endl
<< insideIndex << std::endl;

// Point H
outsidePosition = Position(-1.0, -1.0);
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
isInside = map.getIndex(closestInsidePosition, insideIndex);

expectedPosition = Position(-0.5, -0.5);
expectedIndex = Index(19, 19);

// Check position.
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());

// Check index.
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;

// Check if index is inside.
EXPECT_TRUE(isInside) << "position is: " << std::endl
<< closestInsidePosition << std::endl
<< " index is: " << std::endl
<< insideIndex << std::endl;
}

TEST(AddDataFrom, ExtendMapAligned)
{
grid_map::GridMap map1, map2;
Expand Down

0 comments on commit 3be5358

Please sign in to comment.