Skip to content

Commit

Permalink
Adds BoundingBox Implementation (#7)
Browse files Browse the repository at this point in the history
  • Loading branch information
francocipollone authored May 4, 2022
1 parent dd49c70 commit 1a0b2bf
Show file tree
Hide file tree
Showing 9 changed files with 532 additions and 0 deletions.
1 change: 1 addition & 0 deletions maliput_object/include/maliput_object/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1,2 +1,3 @@
add_subdirectory(api)
add_subdirectory(base)
add_subdirectory(test_utilities)
1 change: 1 addition & 0 deletions maliput_object/include/maliput_object/base/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
ament_export_include_directories(${CMAKE_CURRENT_SOURCE_DIR})
74 changes: 74 additions & 0 deletions maliput_object/include/maliput_object/base/bounding_box.h
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
1 change: 1 addition & 0 deletions maliput_object/src/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -1 +1,2 @@
add_subdirectory(api)
add_subdirectory(base)
43 changes: 43 additions & 0 deletions maliput_object/src/base/CMakeLists.txt
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)
122 changes: 122 additions & 0 deletions maliput_object/src/base/bounding_box.cc
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
1 change: 1 addition & 0 deletions maliput_object/test/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -2,3 +2,4 @@ find_package(ament_cmake_gtest REQUIRED)
find_package(ament_cmake_gmock REQUIRED)

add_subdirectory(api)
add_subdirectory(base)
21 changes: 21 additions & 0 deletions maliput_object/test/base/CMakeLists.txt
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)
Loading

0 comments on commit 1a0b2bf

Please sign in to comment.