-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Adds BoundingBox Implementation (#7)
- Loading branch information
1 parent
dd49c70
commit 1a0b2bf
Showing
9 changed files
with
532 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,2 +1,3 @@ | ||
add_subdirectory(api) | ||
add_subdirectory(base) | ||
add_subdirectory(test_utilities) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1 @@ | ||
ament_export_include_directories(${CMAKE_CURRENT_SOURCE_DIR}) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,74 @@ | ||
// Copyright 2022 Toyota Research Institute | ||
#pragma once | ||
|
||
#include <vector> | ||
|
||
#include <maliput/common/maliput_copyable.h> | ||
#include <maliput/math/roll_pitch_yaw.h> | ||
#include <maliput/math/vector.h> | ||
|
||
#include "maliput_object/api/bounding_region.h" | ||
#include "maliput_object/api/overlapping_type.h" | ||
|
||
namespace maliput { | ||
namespace object { | ||
|
||
/// Implements BoundingRegion abstract class for non-axis-aligned-box-shaped bounding regions. | ||
class BoundingBox : public api::BoundingRegion<maliput::math::Vector3> { | ||
public: | ||
MALIPUT_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(BoundingBox) | ||
|
||
/// Constructs a BoundingBox object. | ||
/// The box is defined by a position, dimensions(length, width and height) and orientation. | ||
/// @param position Position of the bounding box in the Inertial-frame. The position matches with the centroid of the | ||
/// box. | ||
/// @param box_size The size of the bounding box on XYZ (length/width,height) | ||
/// @param orientation Orientation of the box in the Inertial-frame. | ||
BoundingBox(const maliput::math::Vector3& position, const maliput::math::Vector3& box_size, | ||
const maliput::math::RollPitchYaw& orientation, double tolerance); | ||
|
||
~BoundingBox() = default; | ||
|
||
/// @returns The vertices of the bounding box in the Inertial-frame. | ||
std::vector<maliput::math::Vector3> get_vertices() const; | ||
|
||
/// @returns The orientation of the box in the Inertial-frame. | ||
const maliput::math::RollPitchYaw& get_orientation() const; | ||
|
||
/// @returns The size of the box in length, width and height. | ||
const maliput::math::Vector3& box_size() const; | ||
|
||
/// @returns True when this region contains @p other . | ||
bool IsBoxContained(const BoundingBox& other) const; | ||
|
||
/// @returns True when this region intersects @p other . | ||
bool IsBoxIntersected(const BoundingBox& other) const; | ||
|
||
private: | ||
/// Implements api::BoundingRegion::do_position() method. | ||
/// @returns Position of the box. | ||
const maliput::math::Vector3& do_position() const override; | ||
|
||
/// Implements api::BoundingRegion::DoContains() method. | ||
/// @param position Inertial-frame's coordinate. | ||
/// @returns True when @p position is contained in this bounding region. | ||
bool DoContains(const maliput::math::Vector3& position) const override; | ||
|
||
/// Implements api::BoundingRegion::DoOverlaps() method. | ||
/// Valid @p other 's implementations: | ||
/// - BoundingBox | ||
/// @param other Another bounding region. | ||
/// @returns The overlapping type. | ||
api::OverlappingType DoOverlaps(const api::BoundingRegion<maliput::math::Vector3>& other) const override; | ||
|
||
maliput::math::Vector3 position_; | ||
maliput::math::Vector3 box_size_; | ||
maliput::math::RollPitchYaw orientation_; | ||
double tolerance_{}; | ||
|
||
// Half sized box dimensions. | ||
maliput::math::Vector3 xyz_2_; | ||
}; | ||
|
||
} // namespace object | ||
} // namespace maliput |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1 +1,2 @@ | ||
add_subdirectory(api) | ||
add_subdirectory(base) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,43 @@ | ||
############################################################################## | ||
# Sources | ||
############################################################################## | ||
|
||
set(BASE_SOURCES | ||
bounding_box.cc | ||
) | ||
|
||
add_library(base ${BASE_SOURCES}) | ||
|
||
add_library(maliput_object::base ALIAS base) | ||
|
||
set_target_properties(base | ||
PROPERTIES | ||
OUTPUT_NAME maliput_object_base | ||
) | ||
|
||
target_include_directories(base | ||
PUBLIC | ||
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include> | ||
$<INSTALL_INTERFACE:include>) | ||
|
||
target_link_libraries(base | ||
PUBLIC | ||
maliput::common | ||
maliput::math | ||
) | ||
|
||
############################################################################## | ||
# Export | ||
############################################################################## | ||
|
||
include(CMakePackageConfigHelpers) | ||
|
||
install( | ||
TARGETS base | ||
EXPORT ${PROJECT_NAME}-targets | ||
ARCHIVE DESTINATION lib | ||
LIBRARY DESTINATION lib | ||
RUNTIME DESTINATION bin | ||
) | ||
|
||
ament_export_libraries(base) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,122 @@ | ||
// Copyright 2022 Toyota Research Institute | ||
#include "maliput_object/base/bounding_box.h" | ||
|
||
namespace maliput { | ||
namespace object { | ||
|
||
BoundingBox::BoundingBox(const maliput::math::Vector3& position, const maliput::math::Vector3& box_size, | ||
const maliput::math::RollPitchYaw& orientation, double tolerance) | ||
: position_(position), | ||
box_size_(box_size), | ||
orientation_(orientation), | ||
tolerance_(tolerance), | ||
xyz_2_(box_size.x() / 2., box_size.y() / 2., box_size.z() / 2.) { | ||
MALIPUT_THROW_UNLESS(tolerance >= 0.); | ||
} | ||
|
||
const maliput::math::Vector3& BoundingBox::do_position() const { return position_; } | ||
|
||
std::vector<maliput::math::Vector3> BoundingBox::get_vertices() const { | ||
const std::vector<maliput::math::Vector3> vertices_box_frame{ | ||
{xyz_2_.x(), xyz_2_.y(), xyz_2_.z()}, {-xyz_2_.x(), xyz_2_.y(), xyz_2_.z()}, | ||
{xyz_2_.x(), -xyz_2_.y(), xyz_2_.z()}, {xyz_2_.x(), xyz_2_.y(), -xyz_2_.z()}, | ||
{-xyz_2_.x(), -xyz_2_.y(), xyz_2_.z()}, {xyz_2_.x(), -xyz_2_.y(), -xyz_2_.z()}, | ||
{-xyz_2_.x(), xyz_2_.y(), -xyz_2_.z()}, {-xyz_2_.x(), -xyz_2_.y(), -xyz_2_.z()}, | ||
}; | ||
std::vector<maliput::math::Vector3> vertices; | ||
for (const auto& vertex : vertices_box_frame) { | ||
vertices.push_back(orientation_.ToMatrix().inverse() * vertex + position_); | ||
} | ||
return vertices; | ||
} | ||
|
||
const maliput::math::RollPitchYaw& BoundingBox::get_orientation() const { return orientation_; } | ||
|
||
bool BoundingBox::DoContains(const maliput::math::Vector3& position) const { | ||
const maliput::math::Vector3 box_frame_position = orientation_.ToMatrix() * (position - position_); | ||
return box_frame_position.x() <= xyz_2_.x() + tolerance_ && box_frame_position.x() >= -xyz_2_.x() - tolerance_ && | ||
box_frame_position.y() <= xyz_2_.y() + tolerance_ && box_frame_position.y() >= -xyz_2_.y() - tolerance_ && | ||
box_frame_position.z() <= xyz_2_.z() + tolerance_ && box_frame_position.z() >= -xyz_2_.z() - tolerance_; | ||
} | ||
|
||
api::OverlappingType BoundingBox::DoOverlaps(const api::BoundingRegion<maliput::math::Vector3>& other) const { | ||
auto other_box = dynamic_cast<const BoundingBox*>(&other); | ||
MALIPUT_VALIDATE(other_box != nullptr, "BoundingRegion's implementations supported: BoundingBox."); | ||
if (IsBoxContained(*other_box)) { | ||
return api::OverlappingType::kContained; | ||
} | ||
if (IsBoxIntersected(*other_box)) { | ||
return api::OverlappingType::kIntersected; | ||
} | ||
return api::OverlappingType::kDisjointed; | ||
} | ||
|
||
bool BoundingBox::IsBoxContained(const BoundingBox& other) const { | ||
const auto vertices = other.get_vertices(); | ||
return std::all_of(vertices.begin(), vertices.end(), [this](const auto& vertex) { return this->DoContains(vertex); }); | ||
} | ||
|
||
bool BoundingBox::IsBoxIntersected(const BoundingBox& other) const { | ||
// The following is based on Drake's implementation of drake::geometry::internal::BoxesOverlap() method. | ||
// See https://github.com/RobotLocomotion/drake/blob/master/geometry/proximity/boxes_overlap.cc | ||
|
||
// Let's compute the relative transformation from this box to the other box. | ||
// For the purposes of streamlining the math below, translation and rotation | ||
// will be named `t` and `r` respectively. | ||
const maliput::math::Vector3 t = other.position() - position(); | ||
const maliput::math::Matrix3 r = get_orientation().ToMatrix().inverse() * other.get_orientation().ToMatrix(); | ||
|
||
// Compute some common subexpressions and add epsilon to counteract | ||
// arithmetic error, e.g. when two edges are parallel. We use the value as | ||
// specified from Gottschalk's OBB robustness tests. | ||
const double kEpsilon = 0.000001; | ||
maliput::math::Matrix3 abs_r = r; | ||
for (int i = 0; i < 3; ++i) { | ||
for (int j = 0; j < 3; ++j) { | ||
abs_r[i][j] = std::abs(abs_r[i][j]) + kEpsilon; | ||
} | ||
} | ||
|
||
// First category of cases separating along a's axes. | ||
for (int i = 0; i < 3; ++i) { | ||
// if (std::abs(t[i]) > xyz_2_[i] + other.xyz_2_.dot(abs_r.block<1, 3>(i, 0))) { | ||
const maliput::math::Vector3 abs_r_i = abs_r.col(i); | ||
if (std::abs(t[i]) > xyz_2_[i] + other.xyz_2_.dot(abs_r_i)) { | ||
return false; | ||
} | ||
} | ||
|
||
// Second category of cases separating along b's axes. | ||
for (int i = 0; i < 3; ++i) { | ||
// if (std::abs(t.dot(r.block<3, 1>(0, i))) > | ||
// other.xyz_2_[i] + xyz_2_.dot(abs_r.block<3, 1>(0, i))) { | ||
const maliput::math::Vector3 r_i = r.col(i); | ||
const maliput::math::Vector3 abs_r_i = abs_r.col(i); | ||
if (std::abs(t.dot(r_i)) > other.xyz_2_[i] + xyz_2_.dot(abs_r_i)) { | ||
return false; | ||
} | ||
} | ||
|
||
// Third category of cases separating along the axes formed from the cross | ||
// products of a's and b's axes. | ||
int i1 = 1; | ||
for (int i = 0; i < 3; ++i) { | ||
const int i2 = (i1 + 1) % 3; // Calculate common sub expressions. | ||
int j1 = 1; | ||
for (int j = 0; j < 3; ++j) { | ||
const int j2 = (j1 + 1) % 3; | ||
if (std::abs(t[i2] * r[i1][j] - t[i1] * r[i2][j]) > xyz_2_[i1] * abs_r[i2][j] + xyz_2_[i2] * abs_r[i1][j] + | ||
other.xyz_2_[j1] * abs_r[i][j2] + | ||
other.xyz_2_[j2] * abs_r[i][j1]) { | ||
return false; | ||
} | ||
j1 = j2; | ||
} | ||
i1 = i2; | ||
} | ||
|
||
return true; | ||
} | ||
|
||
} // namespace object | ||
} // namespace maliput |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,21 @@ | ||
ament_add_gtest(bounding_box_test bounding_box_test.cc) | ||
|
||
macro(add_dependencies_to_test target) | ||
if (TARGET ${target}) | ||
|
||
target_include_directories(${target} | ||
PRIVATE | ||
${PROJECT_SOURCE_DIR}/include | ||
${CMAKE_CURRENT_SOURCE_DIR} | ||
${PROJECT_SOURCE_DIR}/test | ||
) | ||
|
||
target_link_libraries(${target} | ||
maliput::common | ||
maliput_object::base | ||
) | ||
|
||
endif() | ||
endmacro() | ||
|
||
add_dependencies_to_test(bounding_box_test) |
Oops, something went wrong.