Skip to content

Commit 8e68932

Browse files
rubengrandiarubengrandia
committed
move plane projection code to public repo
1 parent b68f6f1 commit 8e68932

File tree

4 files changed

+330
-0
lines changed

4 files changed

+330
-0
lines changed

‎plane_segmentation/convex_plane_decomposition/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,7 @@ add_library(${PROJECT_NAME}
6565
src/PlanarRegion.cpp
6666
src/PlaneDecompositionPipeline.cpp
6767
src/Postprocessing.cpp
68+
src/SegmentedPlaneProjection.cpp
6869
)
6970
add_dependencies(${PROJECT_NAME}
7071
${catkin_EXPORTED_TARGETS}
@@ -97,6 +98,7 @@ install(DIRECTORY include/${PROJECT_NAME}/
9798
#############
9899

99100
catkin_add_gtest(test_${PROJECT_NAME}
101+
test/testConvexApproximation.cpp
100102
test/testPipeline.cpp
101103
test/testPlanarRegion.cpp
102104
test/testRegionGrowing.cpp

‎plane_segmentation/convex_plane_decomposition/include/convex_plane_decomposition/SegmentedPlaneProjection.h‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
//
2+
// Created by rgrandia on 12.10.21.
3+
//
4+
5+
#pragma once
6+
7+
#include <functional>
8+
9+
#include <convex_plane_decomposition/PlanarRegion.h>
10+
#include <convex_plane_decomposition/PolygonTypes.h>
11+
12+
namespace convex_plane_decomposition {
13+
14+
/**
15+
* Projects a point in the plane to the closest point on the contour of a planar region. We take the inset (slight inward offset from the
16+
* boundary) as the contour to project to.
17+
* @param queryProjectedToPlane : 2D point in the frame of the planar regions
18+
* @param planarRegion : planar region to project to
19+
* @return projected 2D point in the frame of the planar regions
20+
*/
21+
CgalPoint2d projectToPlanarRegion(const CgalPoint2d& queryProjectedToPlane, const PlanarRegion& planarRegion);
22+
23+
/**
24+
* Sorting information as an intermediate step to find the best plane projection
25+
*/
26+
struct RegionSortingInfo {
27+
const PlanarRegion* regionPtr{nullptr};
28+
CgalPoint2d positionInTerrainFrame{0.0, 0.0};
29+
double boundingBoxSquareDistance{0.0};
30+
};
31+
32+
/**
33+
* Compute sorting info and sort according to the bounding box distances.
34+
*
35+
* @param positionInWorld : Query point in world frame
36+
* @param planarRegions : Candidate planar regions
37+
* @return RegionSortingInfo, sorted according to the bounding box distance
38+
*/
39+
std::vector<RegionSortingInfo> sortWithBoundingBoxes(const Eigen::Vector3d& positionInWorld,
40+
const std::vector<PlanarRegion>& planarRegions);
41+
42+
struct PlanarTerrainProjection {
43+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
44+
45+
/// Selected region
46+
const PlanarRegion* regionPtr{nullptr};
47+
48+
/// Projected point in terrain frame of the selected region
49+
CgalPoint2d positionInTerrainFrame{0.0, 0.0};
50+
51+
/// Projected point in world frame
52+
Eigen::Vector3d positionInWorld{0.0, 0.0, 0.0};
53+
54+
/// Projection cost, see getBestPlanarRegionAtPositionInWorld
55+
double cost{0.0};
56+
};
57+
58+
/**
59+
* This function considers the projection of a 3D query point to a set of candidate regions.
60+
*
61+
* The "best" region is picked according to the following cost:
62+
* cost = |p - p_projected|^2 + penaltyFunction(p_projected),
63+
* where p is the query point, and p_projected is the Euclidean projection to the candidate region, both in world frame.
64+
*
65+
* The bounding box of each region is used to find a lower bound on this cost, it is therefore important the user defined penalty is
66+
* non-negative.
67+
*
68+
* @param positionInWorld : Query point in world frame
69+
* @param planarRegions : Candidate planar regions
70+
* @param penaltyFunction : a non-negative (!) scoring function.
71+
* @return Projection and information
72+
*/
73+
PlanarTerrainProjection getBestPlanarRegionAtPositionInWorld(const Eigen::Vector3d& positionInWorld,
74+
const std::vector<PlanarRegion>& planarRegions,
75+
const std::function<double(const Eigen::Vector3d&)>& penaltyFunction);
76+
77+
} // namespace convex_plane_decomposition

‎plane_segmentation/convex_plane_decomposition/src/SegmentedPlaneProjection.cpp‎

Lines changed: 150 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,150 @@
1+
//
2+
// Created by rgrandia on 12.10.21.
3+
//
4+
5+
#include "convex_plane_decomposition/SegmentedPlaneProjection.h"
6+
7+
#include "convex_plane_decomposition/GeometryUtils.h"
8+
9+
namespace convex_plane_decomposition {
10+
11+
namespace { // Helper functions that only make sense in this context
12+
13+
double distanceCost(const Eigen::Vector3d& query, const Eigen::Vector3d& terrainPoint) {
14+
const double dx = query.x() - terrainPoint.x();
15+
const double dy = query.y() - terrainPoint.y();
16+
const double dz = query.z() - terrainPoint.z();
17+
return dx * dx + dy * dy + dz * dz;
18+
}
19+
20+
double distanceCostLowerbound(double distanceSquared) {
21+
return distanceSquared;
22+
}
23+
24+
double intervalSquareDistance(double value, double min, double max) {
25+
// - | 0 | +
26+
// min max
27+
// Returns 0.0 if between min and max. Returns the distance to one boundary otherwise.
28+
if (value < min) {
29+
double diff = min - value;
30+
return diff * diff;
31+
} else if (value < max) {
32+
return 0.0;
33+
} else {
34+
double diff = max - value;
35+
return diff * diff;
36+
}
37+
}
38+
39+
double squaredDistanceToBoundingBox(const CgalPoint2d& point, const CgalBbox2d& boundingBox) {
40+
const double dxdx = intervalSquareDistance(point.x(), boundingBox.xmin(), boundingBox.xmax());
41+
const double dydy = intervalSquareDistance(point.y(), boundingBox.ymin(), boundingBox.ymax());
42+
return dxdx + dydy;
43+
}
44+
45+
const CgalPolygonWithHoles2d* findInsetContainingThePoint(const CgalPoint2d& point, const std::vector<CgalPolygonWithHoles2d>& insets) {
46+
for (const auto& inset : insets) {
47+
if (isInside(point, inset.outer_boundary())) {
48+
return &inset;
49+
}
50+
}
51+
return nullptr;
52+
}
53+
54+
} // namespace
55+
56+
CgalPoint2d projectToPlanarRegion(const CgalPoint2d& queryProjectedToPlane, const PlanarRegion& planarRegion) {
57+
// First search if the projected point is inside any of the insets.
58+
// Note: we know that all insets are non-overlapping, and are not nested (no shape is contained in the hole of another shape)
59+
const auto* const insetPtrContainingPoint = findInsetContainingThePoint(queryProjectedToPlane, planarRegion.boundaryWithInset.insets);
60+
61+
// Compute the projection
62+
CgalPoint2d projectedPoint;
63+
if (insetPtrContainingPoint == nullptr) {
64+
// Not inside any of the insets. Need to look for the closest one. The projection will be to the boundary
65+
double minDistSquared = std::numeric_limits<double>::max();
66+
for (const auto& inset : planarRegion.boundaryWithInset.insets) {
67+
const auto closestPoint = projectToClosestPoint(queryProjectedToPlane, inset.outer_boundary());
68+
double distSquare = squaredDistance(closestPoint, queryProjectedToPlane);
69+
if (distSquare < minDistSquared) {
70+
projectedPoint = closestPoint;
71+
minDistSquared = distSquare;
72+
}
73+
}
74+
} else {
75+
// Query point is inside and does not need projection...
76+
projectedPoint = queryProjectedToPlane;
77+
78+
// ... unless it is inside a hole
79+
for (const auto& hole : insetPtrContainingPoint->holes()) {
80+
if (isInside(queryProjectedToPlane, hole)) {
81+
projectedPoint = projectToClosestPoint(queryProjectedToPlane, hole);
82+
break; // No need to search other holes. Holes are not overlapping
83+
}
84+
}
85+
}
86+
87+
return projectedPoint;
88+
}
89+
90+
std::vector<RegionSortingInfo> sortWithBoundingBoxes(const Eigen::Vector3d& positionInWorld,
91+
const std::vector<convex_plane_decomposition::PlanarRegion>& planarRegions) {
92+
// Compute distance to bounding boxes
93+
std::vector<RegionSortingInfo> regionsAndBboxSquareDistances;
94+
regionsAndBboxSquareDistances.reserve(planarRegions.size());
95+
for (const auto& planarRegion : planarRegions) {
96+
const Eigen::Vector3d positionInTerrainFrame = planarRegion.transformPlaneToWorld.inverse() * positionInWorld;
97+
const double dzdz = positionInTerrainFrame.z() * positionInTerrainFrame.z();
98+
99+
RegionSortingInfo regionSortingInfo;
100+
regionSortingInfo.regionPtr = &planarRegion;
101+
regionSortingInfo.positionInTerrainFrame = {positionInTerrainFrame.x(), positionInTerrainFrame.y()};
102+
regionSortingInfo.boundingBoxSquareDistance =
103+
squaredDistanceToBoundingBox(regionSortingInfo.positionInTerrainFrame, planarRegion.bbox2d) + dzdz;
104+
105+
regionsAndBboxSquareDistances.push_back(regionSortingInfo);
106+
}
107+
108+
// Sort regions close to far
109+
std::sort(regionsAndBboxSquareDistances.begin(), regionsAndBboxSquareDistances.end(),
110+
[](const RegionSortingInfo& lhs, const RegionSortingInfo& rhs) {
111+
return lhs.boundingBoxSquareDistance < rhs.boundingBoxSquareDistance;
112+
});
113+
114+
return regionsAndBboxSquareDistances;
115+
}
116+
117+
PlanarTerrainProjection getBestPlanarRegionAtPositionInWorld(const Eigen::Vector3d& positionInWorld,
118+
const std::vector<PlanarRegion>& planarRegions,
119+
const std::function<double(const Eigen::Vector3d&)>& penaltyFunction) {
120+
const auto sortedRegions = sortWithBoundingBoxes(positionInWorld, planarRegions);
121+
122+
// Look for closest planar region.
123+
PlanarTerrainProjection projection;
124+
projection.cost = std::numeric_limits<double>::max();
125+
for (const auto& regionInfo : sortedRegions) {
126+
// Skip based on lower bound
127+
if (distanceCostLowerbound(regionInfo.boundingBoxSquareDistance) > projection.cost) {
128+
continue;
129+
}
130+
131+
// Project onto planar region
132+
const auto projectedPointInTerrainFrame = projectToPlanarRegion(regionInfo.positionInTerrainFrame, *regionInfo.regionPtr);
133+
134+
// Express projected point in World frame
135+
const auto projectionInWorldFrame =
136+
positionInWorldFrameFromPosition2dInPlane(projectedPointInTerrainFrame, regionInfo.regionPtr->transformPlaneToWorld);
137+
138+
const auto cost = distanceCost(positionInWorld, projectionInWorldFrame) + penaltyFunction(projectionInWorldFrame);
139+
if (cost < projection.cost) {
140+
projection.cost = cost;
141+
projection.regionPtr = regionInfo.regionPtr;
142+
projection.positionInTerrainFrame = projectedPointInTerrainFrame;
143+
projection.positionInWorld = projectionInWorldFrame;
144+
}
145+
}
146+
147+
return projection;
148+
}
149+
150+
} // namespace convex_plane_decomposition

‎plane_segmentation/convex_plane_decomposition/test/testConvexApproximation.cpp‎

Lines changed: 101 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
//
2+
// Created by rgrandia on 08.06.22.
3+
//
4+
5+
#include <gtest/gtest.h>
6+
7+
#include <boost/filesystem/path.hpp>
8+
9+
#include <algorithm>
10+
11+
#include "convex_plane_decomposition/ConvexRegionGrowing.h"
12+
#include "convex_plane_decomposition/GeometryUtils.h"
13+
#include "convex_plane_decomposition/LoadGridmapFromImage.h"
14+
#include "convex_plane_decomposition/PlaneDecompositionPipeline.h"
15+
#include "convex_plane_decomposition/SegmentedPlaneProjection.h"
16+
17+
using namespace convex_plane_decomposition;
18+
19+
namespace {
20+
21+
/**
22+
* Brute force version of getBestPlanarRegionAtPositionInWorld, searches through all candidates without shortcuts
23+
*/
24+
PlanarTerrainProjection getBestPlanarRegionAtPositionInWorldNaive(const Eigen::Vector3d& positionInWorld,
25+
const std::vector<PlanarRegion>& planarRegions,
26+
const std::function<double(const Eigen::Vector3d&)>& penaltyFunction) {
27+
// Do full project per region first
28+
std::vector<PlanarTerrainProjection> individualProjections;
29+
std::for_each(planarRegions.begin(), planarRegions.end(), [&](const PlanarRegion& planarRegion) {
30+
const Eigen::Vector3d positionInTerrainFrame = planarRegion.transformPlaneToWorld.inverse() * positionInWorld;
31+
32+
PlanarTerrainProjection projection;
33+
projection.regionPtr = &planarRegion;
34+
projection.positionInTerrainFrame = projectToPlanarRegion({positionInTerrainFrame.x(), positionInTerrainFrame.y()}, planarRegion);
35+
projection.positionInWorld =
36+
positionInWorldFrameFromPosition2dInPlane(projection.positionInTerrainFrame, planarRegion.transformPlaneToWorld);
37+
projection.cost = (positionInWorld - projection.positionInWorld).squaredNorm() + penaltyFunction(projection.positionInWorld);
38+
individualProjections.push_back(projection);
39+
});
40+
41+
// Find the minimum cost projection
42+
return *std::min_element(individualProjections.begin(), individualProjections.end(),
43+
[](const PlanarTerrainProjection& lhs, const PlanarTerrainProjection& rhs) { return lhs.cost < rhs.cost; });
44+
}
45+
} // namespace
46+
47+
TEST(TestConvexApproximation, runOnDemoMap) {
48+
// Config
49+
PlaneDecompositionPipeline::Config config;
50+
const auto resolution = config.preprocessingParameters.resolution;
51+
const std::string elevationLayer{"elevation_test"};
52+
const std::string frameId{"odom_test"};
53+
const Eigen::Array2d submapSize(3.0, 3.0);
54+
std::string file = "terrain.png";
55+
double heightScale = 1.25;
56+
57+
// Terrain Loading
58+
boost::filesystem::path filePath(__FILE__);
59+
std::string folder = filePath.parent_path().generic_string() + std::string{"/data/"};
60+
const auto loadedMap = loadGridmapFromImage(folder + file, elevationLayer, frameId, resolution, heightScale);
61+
bool success = false;
62+
auto elevationMap = loadedMap.getSubmap(loadedMap.getPosition(), submapSize, success);
63+
ASSERT_TRUE(success);
64+
65+
// Run pipeline.
66+
PlaneDecompositionPipeline pipeline(config);
67+
pipeline.update(std::move(elevationMap), elevationLayer);
68+
const auto& planarTerrain = pipeline.getPlanarTerrain();
69+
70+
// Query a range of points
71+
for (double x = -submapSize.x() / 2.0; x < submapSize.x() / 2.0; x += submapSize.x() / 4.0) {
72+
for (double y = -submapSize.y() / 2.0; y < submapSize.y() / 2.0; y += submapSize.y() / 4.0) {
73+
for (double z = -heightScale; z < heightScale; z += heightScale / 2.0) {
74+
Eigen::Vector3d query{x, y, z};
75+
auto penaltyFunction = [](const Eigen::Vector3d& projectedPoint) { return 0.1 * std::abs(projectedPoint.z()); };
76+
77+
// Run projection and naive projection
78+
const auto projection = getBestPlanarRegionAtPositionInWorld(query, planarTerrain.planarRegions, penaltyFunction);
79+
const auto projectionCheck = getBestPlanarRegionAtPositionInWorldNaive(query, planarTerrain.planarRegions, penaltyFunction);
80+
81+
// Check they are the same
82+
ASSERT_EQ(projection.regionPtr, projectionCheck.regionPtr);
83+
ASSERT_DOUBLE_EQ(projection.cost, projectionCheck.cost);
84+
ASSERT_DOUBLE_EQ(projection.positionInTerrainFrame.x(), projectionCheck.positionInTerrainFrame.x());
85+
ASSERT_DOUBLE_EQ(projection.positionInTerrainFrame.y(), projectionCheck.positionInTerrainFrame.y());
86+
ASSERT_TRUE(projection.positionInWorld.isApprox(projectionCheck.positionInWorld));
87+
88+
// Check convex approximation with a range of settings
89+
for (int numberOfVertices = 3; numberOfVertices < 7; ++numberOfVertices) {
90+
for (double growthFactor = 1.01; growthFactor < 1.3; growthFactor += 0.1) {
91+
const auto convexRegion = convex_plane_decomposition::growConvexPolygonInsideShape(
92+
projection.regionPtr->boundaryWithInset.boundary, projection.positionInTerrainFrame, numberOfVertices, growthFactor);
93+
94+
ASSERT_TRUE(std::all_of(convexRegion.vertices_begin(), convexRegion.vertices_end(),
95+
[&](const CgalPoint2d& p) { return isInside(p, projection.regionPtr->boundaryWithInset.boundary); }));
96+
}
97+
}
98+
}
99+
}
100+
}
101+
}

0 commit comments

Comments
 (0)