Geometry.hpp File Reference
#include <exception>#include <tuple>#include <lanelet2_core/primitives/Lanelet.h>#include <lanelet2_core/primitives/Point.h>#include <lanelet2_core/primitives/Polygon.h>#include <lanelet2_core/utility/Optional.h>#include <geometry_msgs/msg/pose.hpp>#include <geometry_msgs/msg/vector3.hpp>#include <tf2_geometry_msgs/tf2_geometry_msgs.h>#include <tf2/LinearMath/Quaternion.h>#include <tf2/LinearMath/Matrix3x3.h>#include <carma_wm/TrackPos.hpp>
Include dependency graph for Geometry.hpp:
This graph shows which files directly or indirectly include this file:
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 | |
| 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... | |
| 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... | |
| 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... | |
| 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... | |
| 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... | |
| 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... | |
| 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... | |
| std::vector< double > | carma_wm::geometry::compute_arc_lengths (const lanelet::BasicLineString2d &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... | |
| 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... | |
| 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... | |
| double | carma_wm::geometry::getAngleBetweenVectors (const Eigen::Vector2d &vec1, const Eigen::Vector2d &vec2) |
| Calculates the angle between two vectors. 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... | |
| 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... | |
| 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... | |
