Geometry.hpp

Go to the documentation of this file.

#pragma once
 
/*
 * Copyright (C) 2022 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */
 
#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.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <carma_wm/TrackPos.hpp>
 
namespace carma_wm
{
namespace geometry
{
TrackPos trackPos(const lanelet::ConstLanelet& lanelet, const lanelet::BasicPoint2d& point);
 
TrackPos trackPos(const lanelet::BasicPoint2d& p, const lanelet::BasicPoint2d& seg_start,
                  const lanelet::BasicPoint2d& seg_end);
std::tuple<TrackPos, lanelet::BasicSegment2d> matchSegment(const lanelet::BasicPoint2d& p,
                                                           const lanelet::BasicLineString2d& line_string);
 
std::vector<double> local_curvatures(const lanelet::BasicLineString2d& centerline_points);
 
std::vector<double> local_curvatures(const std::vector<lanelet::BasicPoint2d>& centerline_points);
 
std::vector<double> local_curvatures(const std::vector<lanelet::ConstLanelet>& lanelets);
 
lanelet::BasicLineString2d concatenate_line_strings(const lanelet::BasicLineString2d& l1,
                                                    const lanelet::BasicLineString2d& l2);
 
lanelet::BasicLineString2d concatenate_lanelets(const std::vector<lanelet::ConstLanelet>& lanelets);
 
std::vector<Eigen::Vector2d> compute_finite_differences(const lanelet::BasicLineString2d& data);
 
std::vector<Eigen::Vector2d> compute_finite_differences(const std::vector<lanelet::BasicPoint2d>& data);
 
std::vector<double> compute_finite_differences(const std::vector<double>& data);
 
std::vector<Eigen::Vector2d> compute_finite_differences(const std::vector<Eigen::Vector2d>& x,
                                                        const std::vector<double>& y);
 
std::vector<double> compute_arc_lengths(const std::vector<lanelet::BasicPoint2d>& data);
 
std::vector<double> compute_arc_lengths(const lanelet::BasicLineString2d& data);
 
double get_lanelet_centerline_length(const lanelet::ConstLanelet& ll);
 
double compute_euclidean_distance(const Eigen::Vector2d& a, const Eigen::Vector2d& b);
 
std::vector<Eigen::Vector2d> normalize_vectors(const std::vector<Eigen::Vector2d>& vectors);
 
std::vector<double> compute_magnitude_of_vectors(const std::vector<Eigen::Vector2d>& vectors);
 
[[deprecated("computeCurvature is deprecated in favor of using the finite differences-based computeLocalCurvature for "
             "large curves")]] double
computeCurvature(const lanelet::BasicPoint2d& p1, const lanelet::BasicPoint2d& p2, const lanelet::BasicPoint2d& p3);
 
double safeAcos(double x);
 
double safeAsin(double x);
 
double getAngleBetweenVectors(const Eigen::Vector2d& vec1, const Eigen::Vector2d& vec2);
 
lanelet::BasicPolygon2d objectToMapPolygon(const geometry_msgs::msg::Pose& pose, const geometry_msgs::msg::Vector3& size);
 
void rpyFromQuaternion(const tf2::Quaternion& q, double& roll, double& pitch, double& yaw);
 
void rpyFromQuaternion(const geometry_msgs::msg::Quaternion& q_msg, double& roll, double& pitch, double& yaw);
 
std::vector<double> compute_tangent_orientations(const lanelet::BasicLineString2d& centerline);
 
std::vector<double> compute_tangent_orientations(const std::vector<lanelet::BasicPoint2d>& centerline);
 
Eigen::Isometry2d build2dEigenTransform(const Eigen::Vector2d& position, const Eigen::Rotation2Dd& rotation);
 
Eigen::Isometry3d build3dEigenTransform(const Eigen::Vector3d& position, const Eigen::Quaterniond& rotation);
 
Eigen::Isometry3d build3dEigenTransform(const Eigen::Vector3d& position, const Eigen::AngleAxisd& rotation);
 
double point_to_point_yaw(const lanelet::BasicPoint2d& cur_point, const lanelet::BasicPoint2d& next_point);
 
double circular_arc_curvature(const lanelet::BasicPoint2d& cur_point, const lanelet::BasicPoint2d& next_point);
 
std::vector<double> local_circular_arc_curvatures(const std::vector<lanelet::BasicPoint2d>& points, int lookahead);
 
}  // namespace geometry
 
}  // namespace carma_wm