p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added Contains and Get(World/Local)Vertices to BoundingBox

This commit is contained in:
victor.anton 2020-01-23 12:09:38 +01:00 committed by Marc Garcia Puig
parent 542f6f28d3
commit 34f993a7c9
6 changed files with 236 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
LibCarla/source/carla/geom/BoundingBox.h
View File

@ -8,9 +8,12 @@
#include "carla/Debug.h"
#include "carla/MsgPack.h"
#include "carla/geom/Transform.h"
#include "carla/geom/Location.h"
#include "carla/geom/Vector3D.h"
#include <array>
#ifdef LIBCARLA_INCLUDED_FROM_UE4
# include "Carla/Util/BoundingBox.h"
#endif // LIBCARLA_INCLUDED_FROM_UE4
@ -23,6 +26,10 @@ namespace geom {
BoundingBox() = default;
// =========================================================================
// -- Constructors ---------------------------------------------------------
// =========================================================================
explicit BoundingBox(const Location &in_location, const Vector3D &in_extent)
: location(in_location),
extent(in_extent) {}
@ -30,8 +37,59 @@ namespace geom {
explicit BoundingBox(const Vector3D &in_extent)
: extent(in_extent) {}
Location location;
Vector3D extent;
Location location; ///< Center of the BoundingBox in local space
Vector3D extent; ///< Half the size of the BoundingBox in local space
// =========================================================================
// -- Other methods --------------------------------------------------------
// =========================================================================
/**
* Whether this BoundingBox contains @a in_world_point in world space.
* @param in_world_point the point in world space that you want to query whether it is inside or not.
* @param in_bbox_to_world_transform the transformation from BoundingBox space to World space.
*/
bool Contains(const Location &in_world_point, const Transform &in_bbox_to_world_transform) const {
auto point_in_bbox_space = in_world_point;
in_bbox_to_world_transform.InverseTransformPoint(point_in_bbox_space);
point_in_bbox_space -= location;
return point_in_bbox_space.x >= -extent.x && point_in_bbox_space.x <= extent.x &&
point_in_bbox_space.y >= -extent.y && point_in_bbox_space.y <= extent.y &&
point_in_bbox_space.z >= -extent.z && point_in_bbox_space.z <= extent.z;
}
/**
* Returns the positions of the 8 vertices of this BoundingBox in local space.
*/
std::array<Location, 8> GetLocalVertices() const {
return {{
location + Location(-extent.x,-extent.y,-extent.z),
location + Location(-extent.x,-extent.y, extent.z),
location + Location(-extent.x, extent.y,-extent.z),
location + Location(-extent.x, extent.y, extent.z),
location + Location( extent.x,-extent.y,-extent.z),
location + Location( extent.x,-extent.y, extent.z),
location + Location( extent.x, extent.y,-extent.z),
location + Location( extent.x, extent.y, extent.z)
}};
}
/**
* Returns the positions of the 8 vertices of this BoundingBox in world space.
* @param in_bbox_to_world_transform The Transform from this BoundingBox space to world space.
*/
std::array<Location, 8> GetWorldVertices(const Transform &in_bbox_to_world_tr) const {
auto world_vertices = GetLocalVertices();
std::for_each(world_vertices.begin(), world_vertices.end(), [&in_bbox_to_world_tr](auto &world_vertex) {
in_bbox_to_world_tr.TransformPoint(world_vertex);
});
return world_vertices;
}
// =========================================================================
// -- Comparison operators -------------------------------------------------
// =========================================================================
bool operator==(const BoundingBox &rhs) const {
return (location == rhs.location) && (extent == rhs.extent);
@ -41,6 +99,10 @@ namespace geom {
return !(*this == rhs);
}
// =========================================================================
// -- Conversions to UE4 types ---------------------------------------------
// =========================================================================
#ifdef LIBCARLA_INCLUDED_FROM_UE4
BoundingBox(const FBoundingBox &Box)

54
LibCarla/source/carla/geom/Rotation.h
View File

@ -51,6 +51,60 @@ namespace geom {
return Math::GetForwardVector(*this);
}
void RotateVector(Vector3D &in_point) const {
// Rotates Rz(yaw) * Ry(pitch) * Rx(roll) = first x, then y, then z.
const float cy = std::cos(Math::ToRadians(yaw));
const float sy = std::sin(Math::ToRadians(yaw));
const float cr = std::cos(Math::ToRadians(roll));
const float sr = std::sin(Math::ToRadians(roll));
const float cp = std::cos(Math::ToRadians(pitch));
const float sp = std::sin(Math::ToRadians(pitch));
Vector3D out_point;
out_point.x =
in_point.x * (cp * cy) +
in_point.y * (cy * sp * sr - sy * cr) +
in_point.z * (-cy * sp * cr - sy * sr);
out_point.y =
in_point.x * (cp * sy) +
in_point.y * (sy * sp * sr + cy * cr) +
in_point.z * (-sy * sp * cr + cy * sr);
out_point.z =
in_point.x * (sp) +
in_point.y * (-cp * sr) +
in_point.z * (cp * cr);
in_point = out_point;
}
void InverseRotateVector(Vector3D &in_point) const {
// Applies the transposed of the matrix used in RotateVector function,
// which is the rotation inverse.
const float cy = std::cos(Math::ToRadians(yaw));
const float sy = std::sin(Math::ToRadians(yaw));
const float cr = std::cos(Math::ToRadians(roll));
const float sr = std::sin(Math::ToRadians(roll));
const float cp = std::cos(Math::ToRadians(pitch));
const float sp = std::sin(Math::ToRadians(pitch));
Vector3D out_point;
out_point.x =
in_point.x * (cp * cy) +
in_point.y * (cp * sy) +
in_point.z * (sp);
out_point.y = in_point.x * (cy * sp * sr - sy * cr) + in_point.y * (sy * sp * sr + cy * cr) + in_point.z * (-cp * sr);
out_point.z =
in_point.x * (-cy * sp * cr - sy * sr) +
in_point.y * (-sy * sp * cr + cy * sr) +
in_point.z * (cp * cr);
in_point = out_point;
}
// =========================================================================
// -- Comparison operators -------------------------------------------------
// =========================================================================

33
LibCarla/source/carla/geom/Transform.h
View File

@ -53,30 +53,19 @@ namespace geom {
return rotation.GetForwardVector();
}
/// Applies this transformation to @a in_point (first translation then rotation).
void TransformPoint(Vector3D &in_point) const {
// Rotate
const float cy = std::cos(Math::ToRadians(rotation.yaw));
const float sy = std::sin(Math::ToRadians(rotation.yaw));
const float cr = std::cos(Math::ToRadians(rotation.roll));
const float sr = std::sin(Math::ToRadians(rotation.roll));
const float cp = std::cos(Math::ToRadians(rotation.pitch));
const float sp = std::sin(Math::ToRadians(rotation.pitch));
Vector3D out_point;
out_point.x = in_point.x * (cp * cy) +
in_point.y * (cy * sp * sr - sy * cr) +
in_point.z * (-cy * sp * cr - sy * sr);
out_point.y = in_point.x * (cp * sy) +
in_point.y * (sy * sp * sr + cy * cr) +
in_point.z * (-sy * sp * cr + cy * sr);
out_point.z = in_point.x * (sp) +
in_point.y * -(cp * sr) +
in_point.z * (cp * cr);
// Translate
out_point += location;
auto out_point = in_point;
rotation.RotateVector(out_point); // First rotate
out_point += location; // Then translate
in_point = out_point;
}
/// Applies the inverse of this transformation to @a in_point
void InverseTransformPoint(Vector3D &in_point) const {
auto out_point = in_point;
out_point -= location; // First translate inverse
rotation.InverseRotateVector(out_point); // Then rotate inverse
in_point = out_point;
}

48
LibCarla/source/test/common/test_geom.cpp
View File

@ -8,6 +8,7 @@
#include <carla/geom/Vector3D.h>
#include <carla/geom/Math.h>
#include <carla/geom/BoundingBox.h>
#include <carla/geom/Transform.h>
#include <limits>
@ -57,6 +58,53 @@ TEST(geom, single_point_translation) {
ASSERT_NEAR(point.z, result_point.z, error);
}
TEST(geom, single_point_transform_inverse_transform_coherence) {
constexpr double error = 0.001;
const Location point(-3.14f, 1.337f, 4.20f);
const Location translation (1.41f, -4.7f, 9.2f);
const Rotation rotation (-47.0f, 37.0f, 250.2f);
const Transform transform (translation, rotation);
auto transformed_point = point;
transform.TransformPoint(transformed_point);
auto point_back_to_normal = transformed_point;
transform.InverseTransformPoint(point_back_to_normal);
ASSERT_NEAR(point.x, point_back_to_normal.x, error) << "result.x is " << point_back_to_normal.x << " but expected " << point.x;
ASSERT_NEAR(point.y, point_back_to_normal.y, error) << "result.y is " << point_back_to_normal.y << " but expected " << point.y;
ASSERT_NEAR(point.z, point_back_to_normal.z, error) << "result.z is " << point_back_to_normal.z << " but expected " << point.z;
}
TEST(geom, bbox_get_local_vertices_get_world_vertices_coherence) {
constexpr double error = 0.001;
const BoundingBox bbox (Location(10.2f, -32.4f, 15.6f), Vector3D(9.2f, 13.5f, 20.3f));
const Location bbox_location(-3.14f, 1.337f, 4.20f);
const Rotation bbox_rotation (-59.0f, 17.0f, -650.2f);
const Transform bbox_transform(bbox_location, bbox_rotation);
const auto local_vertices = bbox.GetLocalVertices();
const auto world_vertices = bbox.GetWorldVertices(bbox_transform);
for (auto i = 0u; i < local_vertices.size(); ++i){
const auto &local_vertex = local_vertices[i];
auto transformed_local_vertex = local_vertex;
bbox_transform.TransformPoint(transformed_local_vertex);
const auto &world_vertex = world_vertices[i];
ASSERT_NEAR(transformed_local_vertex.x, world_vertex.x, error) << "result.x is " << transformed_local_vertex.x << " but expected " << world_vertex.x;
ASSERT_NEAR(transformed_local_vertex.y, world_vertex.y, error) << "result.y is " << transformed_local_vertex.y << " but expected " << world_vertex.y;
ASSERT_NEAR(transformed_local_vertex.z, world_vertex.z, error) << "result.z is " << transformed_local_vertex.z << " but expected " << world_vertex.z;
}
}
TEST(geom, single_point_rotation) {
constexpr double error = 0.001;

22
PythonAPI/carla/source/libcarla/Geom.cpp
View File

@ -86,6 +86,25 @@ static void TransformList(const carla::geom::Transform &self, boost::python::lis
}
}
template <typename TContainer>
static boost::python::list CreatePythonList(const TContainer &container) {
boost::python::list py_list;
for (auto iter = container.begin(); iter != container.end(); ++iter) {
py_list.append(*iter);
}
return py_list;
}
static boost::python::list BBoxGetLocalVerticesPython(const carla::geom::BoundingBox &self) {
return CreatePythonList(self.GetLocalVertices());
}
static boost::python::list BBoxGetWorldVerticesPython(
const carla::geom::BoundingBox &self,
const carla::geom::Transform &bbox_transform) {
return CreatePythonList(self.GetWorldVertices(bbox_transform));
}
void export_geom() {
using namespace boost::python;
namespace cg = carla::geom;
@ -181,6 +200,9 @@ void export_geom() {
(arg("location")=cg::Location(), arg("extent")=cg::Vector3D())))
.def_readwrite("location", &cg::BoundingBox::location)
.def_readwrite("extent", &cg::BoundingBox::extent)
.def("contains", &cg::BoundingBox::Contains, arg("point"), arg("bbox_transform"))
.def("get_local_vertices", &BBoxGetLocalVerticesPython)
.def("get_world_vertices", &BBoxGetWorldVerticesPython, arg("bbox_transform"))
.def("__eq__", &cg::BoundingBox::operator==)
.def("__ne__", &cg::BoundingBox::operator!=)
.def(self_ns::str(self_ns::self))

44
PythonAPI/docs/geom.yml
View File

@ -16,10 +16,10 @@
methods:
- def_name: __init__
params:
- param_name: x
- param_name: x
type: float
default: 0.0
- param_name: y
- param_name: y
type: float
default: 0.0
doc: >
@ -135,13 +135,13 @@
methods:
- def_name: __init__
params:
- param_name: x
- param_name: x
type: float
default: 0.0
- param_name: 'y'
type: float
default: 0.0
- param_name: z
- param_name: z
type: float
default: 0.0
doc: >
@ -176,7 +176,7 @@
Class that represents a 3D rotation. All rotation angles are stored in degrees.
![UE4_Rotation](https://d26ilriwvtzlb.cloudfront.net/8/83/BRMC_9.jpg)
![UE4_Rotation](https://d26ilriwvtzlb.cloudfront.net/8/83/BRMC_9.jpg)
_Unreal Engine's standard (from [UE4 docs](https://wiki.unrealengine.com/Blueprint_Rotating_Movement_Component))_
# - PROPERTIES -------------------------
instance_variables:
@ -292,7 +292,7 @@
- var_name: extent
type: carla.Vector3D
doc: >
It contains the vector from the center of the bounding box to one of the vertex of the box.
It contains the vector from the center of the bounding box to one of the vertex of the box.
So, if you want to know the _X bounding box size_, you can just do `extent.x * 2`
# - METHODS ----------------------------
@ -305,6 +305,36 @@
type: carla.Vector3D
doc: >
# --------------------------------------
- def_name: contains
return: bool
params:
- param_name: world_point
type: carla.Location
doc: >
The point in world space to be checked whether it is inside this carla.BoundingBox or.
- param_name: transform
type: carla.Transform
doc: >
The carla.Transform that transforms from this carla.BoundingBox space to world space.
doc: >
Returns whether a point in world space is inside this BoundingBox.
# --------------------------------------
- def_name: get_local_vertices
return: a list of carla.Location
params:
doc: >
Returns the vertices of this carla.BoundingBox in local space.
# --------------------------------------
- def_name: get_world_vertices
return: a list of carla.Location
params:
- param_name: transform
type: carla.Transform
doc: >
The carla.Transform that transforms from this carla.BoundingBox space to world space.
doc: >
Returns the vertices of this carla.BoundingBox in world space.
# --------------------------------------
- def_name: __eq__
params:
- param_name: other
@ -324,7 +354,7 @@
- class_name: GeoLocation
# - DESCRIPTION ------------------------
doc: >
Class that contains geolocation simulated data
Class that contains geolocation simulated data
# - PROPERTIES -------------------------
instance_variables:
- var_name: latitude