Geometry.cpp File Reference
#include <carma_wm/Geometry.hpp>#include <lanelet2_core/geometry/Point.h>#include <tf2/LinearMath/Transform.h>#include <tf2_geometry_msgs/tf2_geometry_msgs.h>#include <tf2/transform_datatypes.h>
Include dependency graph for Geometry.cpp:
Go to the source code of this file.
Namespaces | |
| namespace | carma_wm |
| namespace | carma_wm::geometry |
| geometry namespace contains utility geometry functions which do not require the map or route provided by an active world model | |
Functions | |
| double | carma_wm::geometry::safeAcos (double x) |
| acos method that caps the input x value for 1 or -1 to avoid undefined output. This is meant to prevent issues with floating point approximations. The user should still make sure the input data is expected to be within the [-1, 1] range for this method to give accurate results. More... | |
| double | carma_wm::geometry::safeAsin (double x) |
| asin method that caps the input x value for 1 or -1 to avoid undefined output. This is meant to prevent issues with floating point approximations. The user should still make sure the input data is expected to be within the [-1, 1] range for this method to give accurate results. More... | |
| void | carma_wm::geometry::rpyFromQuaternion (const tf2::Quaternion &q, double &roll, double &pitch, double &yaw) |
| Extract extrinsic roll-pitch-yaw from quaternion. More... | |
| void | carma_wm::geometry::rpyFromQuaternion (const geometry_msgs::msg::Quaternion &q_msg, double &roll, double &pitch, double &yaw) |
| Extract extrinsic roll-pitch-yaw from quaternion. More... | |
| double | carma_wm::geometry::getAngleBetweenVectors (const Eigen::Vector2d &vec1, const Eigen::Vector2d &vec2) |
| Calculates the angle between two vectors. More... | |
| TrackPos | carma_wm::geometry::trackPos (const lanelet::BasicPoint2d &p, const lanelet::BasicPoint2d &seg_start, const lanelet::BasicPoint2d &seg_end) |
| Return the track position of a point relative to a line segment defined by two other points. Positive crosstrack will be to the right. Points occuring before the segment will have negative downtrack. More... | |
| TrackPos | carma_wm::geometry::trackPos (const lanelet::ConstLanelet &lanelet, const lanelet::BasicPoint2d &point) |
| Returns the TrackPos, computed in 2d, of the provided point relative to the centerline of the provided lanelet. Positive crosstrack will be to the right. Points occuring before the segment will have negative downtrack. See the matchSegment function for a description of the edge cases associated with the max_crosstrack parameter. More... | |
| bool | carma_wm::geometry::selectFirstSegment (const TrackPos &first_seg_trackPos, const TrackPos &second_seg_trackPos, double first_seg_length, double second_seg_length) |
| Helper function to identify whether to select the preceeding or succeeding segment of a linstring point that is nearest an external point when trying to find the TrackPos of the external point. Of the 3 points comprising the 2 segment linestirng the external point must be closest to the midpoint for this function to be valid. More... | |
| std::tuple< TrackPos, lanelet::BasicSegment2d > | carma_wm::geometry::matchSegment (const lanelet::BasicPoint2d &p, const lanelet::BasicLineString2d &line_string) |
| Get the TrackPos of the provided point along the linestring and the segment of the line string which the provided point should be considered in. More... | |
| double | carma_wm::geometry::computeCurvature (const lanelet::BasicPoint2d &p1, const lanelet::BasicPoint2d &p2, const lanelet::BasicPoint2d &p3) |
| Function for computing curvature from 3 points. More... | |
| lanelet::BasicPolygon2d | carma_wm::geometry::objectToMapPolygon (const geometry_msgs::msg::Pose &pose, const geometry_msgs::msg::Vector3 &size) |
| Uses the provided pose and size vector of an ExternalObject to compute what the polygon would be of that object would be if viewed from the same frame as the pose is defined relative to. More... | |
| lanelet::BasicLineString2d | carma_wm::geometry::concatenate_lanelets (const std::vector< lanelet::ConstLanelet > &lanelets) |
| Helper function to a list of lanelets together and return the result. Neither LineString is modified in this function. More... | |
| template<class P , class A > | |
| std::vector< double > | carma_wm::geometry::templated_local_curvatures (const std::vector< P, A > ¢erline_points) |
| std::vector< double > | carma_wm::geometry::local_curvatures (const lanelet::BasicLineString2d ¢erline_points) |
| Returns a list of local (computed by discrete derivative) curvatures for the input centerlines. The list of returned curvatures matches 1-to-1 with with list of points in the input centerlines. More... | |
| std::vector< double > | carma_wm::geometry::local_curvatures (const std::vector< lanelet::BasicPoint2d > ¢erline_points) |
| std::vector< double > | carma_wm::geometry::local_curvatures (const std::vector< lanelet::ConstLanelet > &lanelets) |
| Specialized overload of the centerline local_curvatures method but for lanelets. More... | |
| lanelet::BasicLineString2d | carma_wm::geometry::concatenate_line_strings (const lanelet::BasicLineString2d &l1, const lanelet::BasicLineString2d &l2) |
| Helper function to concatenate 2 linestrings together and return the result. Neither LineString is modified in this function. More... | |
| template<class P , class A > | |
| std::vector< Eigen::Vector2d > | carma_wm::geometry::compute_templated_finite_differences (const std::vector< P, A > &data) |
| std::vector< Eigen::Vector2d > | carma_wm::geometry::compute_finite_differences (const lanelet::BasicLineString2d &data) |
| Use finite differences methods to compute the derivative of the input data set with respect to index Compute the finite differences using the forward finite difference for the first point, centered finite differences for the middle points and backwards finite difference for the final point. This will result in the middle points being a better approximation of the actual derivative than the endpoints. More... | |
| std::vector< Eigen::Vector2d > | carma_wm::geometry::compute_finite_differences (const std::vector< lanelet::BasicPoint2d > &data) |
| std::vector< double > | carma_wm::geometry::compute_finite_differences (const std::vector< double > &data) |
| Use finite differences methods to compute the derivative of the input data set with respect to index Compute the finite differences using the forward finite difference for the first point, centered finite differences for the middle points and backwards finite difference for the final point. This will result in the middle points being a better approximation of the actual derivative than the endpoints. More... | |
| std::vector< Eigen::Vector2d > | carma_wm::geometry::compute_finite_differences (const std::vector< Eigen::Vector2d > &x, const std::vector< double > &y) |
| Use finite differences methods to compute the derivative of the input data set with respect to the second paramter. More... | |
| template<class P , class A > | |
| std::vector< double > | carma_wm::geometry::compute_templated_arc_lengths (const std::vector< P, A > &data) |
| std::vector< double > | carma_wm::geometry::compute_arc_lengths (const lanelet::BasicLineString2d &data) |
| Compute the arc length at each point around the curve. More... | |
| std::vector< double > | carma_wm::geometry::compute_arc_lengths (const std::vector< lanelet::BasicPoint2d > &data) |
| Compute the arc length at each point around the curve. More... | |
| double | carma_wm::geometry::compute_euclidean_distance (const Eigen::Vector2d &a, const Eigen::Vector2d &b) |
| Compute the Euclidean distance between the two points. More... | |
| std::vector< Eigen::Vector2d > | carma_wm::geometry::normalize_vectors (const std::vector< Eigen::Vector2d > &vectors) |
| Normalize the vectors in the input list such that their magnitudes = 1. More... | |
| std::vector< double > | carma_wm::geometry::compute_magnitude_of_vectors (const std::vector< Eigen::Vector2d > &vectors) |
| Compute the magnitude of each vector in the input list. More... | |
| template<class P , class A > | |
| std::vector< double > | carma_wm::geometry::compute_templated_tangent_orientations (const std::vector< P, A > ¢erline) |
| std::vector< double > | carma_wm::geometry::compute_tangent_orientations (const lanelet::BasicLineString2d ¢erline) |
| Compute an approximate orientation for the vehicle at each point along the provided centerline. More... | |
| std::vector< double > | carma_wm::geometry::compute_tangent_orientations (const std::vector< lanelet::BasicPoint2d > ¢erline) |
| Eigen::Isometry2d | carma_wm::geometry::build2dEigenTransform (const Eigen::Vector2d &position, const Eigen::Rotation2Dd &rotation) |
| Builds a 2D Eigen coordinate frame transform with not applied scaling (only translation and rotation) based on the provided position and rotation parameters. More... | |
| Eigen::Isometry3d | carma_wm::geometry::build3dEigenTransform (const Eigen::Vector3d &position, const Eigen::Quaterniond &rotation) |
| Builds a 3D Eigen coordinate frame transform with not applied scaling (only translation and rotation) based on the provided position and rotation parameters. More... | |
| Eigen::Isometry3d | carma_wm::geometry::build3dEigenTransform (const Eigen::Vector3d &position, const Eigen::AngleAxisd &rotation) |
| double | carma_wm::geometry::point_to_point_yaw (const lanelet::BasicPoint2d &cur_point, const lanelet::BasicPoint2d &next_point) |
| Computes the 2d orientation of the vector from the provided start point to end point. More... | |
| double | carma_wm::geometry::circular_arc_curvature (const lanelet::BasicPoint2d &cur_point, const lanelet::BasicPoint2d &next_point) |
| Computes the curvature of a circular arc that passes through the two provided points. If the two points lie on a curve then larger distances between them will result in less accurate, but also less noisy curvature calculations. More... | |
| std::vector< double > | carma_wm::geometry::local_circular_arc_curvatures (const std::vector< lanelet::BasicPoint2d > &points, int lookahead) |
| Computes the curvature of each point in the provided list by calling circular_arc_curvature using that point and the point at index current point index + lookahead. In this way the curvature of a path is effectively low pass filtered by the size of the lookahead value. More... | |
Variables | |
| constexpr double | carma_wm::geometry::SPATIAL_EPSILON_M = 0.05 |
