Autoware.Auto
localization/ndt/include/ndt/utils.hpp File Reference
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <geometry_msgs/msg/transform.hpp>
#include <point_cloud_msg_wrapper/point_cloud_msg_wrapper.hpp>
#include <helper_functions/float_comparisons.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <Eigen/Eigenvalues>
#include <algorithm>
#include <limits>
#include <tuple>
Include dependency graph for localization/ndt/include/ndt/utils.hpp:
This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes

struct  autoware::localization::ndt::PointWithCovariances
 

Namespaces

 autoware
 This file defines the lanelet2_map_provider_node class.
 
 autoware::localization
 
 autoware::localization::ndt
 
 autoware::localization::ndt::transform_adapters
 

Typedefs

using autoware::localization::ndt::PointWithCovariancesFieldGenerators = std::tuple< point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, point_cloud_msg_wrapper::field_z_generator, field_icov_xx_generator, field_icov_xy_generator, field_icov_xz_generator, field_icov_yy_generator, field_icov_yz_generator, field_icov_zz_generator >
 
using autoware::localization::ndt::NdtMapCloudModifier = point_cloud_msg_wrapper::PointCloud2Modifier< PointWithCovariances, PointWithCovariancesFieldGenerators >
 
using autoware::localization::ndt::NdtMapCloudView = point_cloud_msg_wrapper::PointCloud2View< PointWithCovariances, PointWithCovariancesFieldGenerators >
 
template<typename T >
using autoware::localization::ndt::EigenPose = Eigen::Matrix< T, 6U, 1U >
 
template<typename T >
using autoware::localization::ndt::EigenTransform = Eigen::Transform< T, 3, Eigen::Affine, Eigen::ColMajor >
 
using autoware::localization::ndt::RosTransform = geometry_msgs::msg::Transform
 
using autoware::localization::ndt::RosPose = geometry_msgs::msg::Pose
 

Functions

 autoware::localization::ndt::LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER (icov_xx)
 
 autoware::localization::ndt::LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER (icov_xy)
 
 autoware::localization::ndt::LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER (icov_xz)
 
 autoware::localization::ndt::LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER (icov_yy)
 
 autoware::localization::ndt::LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER (icov_yz)
 
 autoware::localization::ndt::LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER (icov_zz)
 
template<typename Derived >
bool autoware::localization::ndt::try_stabilize_covariance (Eigen::MatrixBase< Derived > &covariance, typename Derived::PlainMatrix::Scalar scaling_factor=0.1)
 
template<typename PoseT , typename TransformT >
void autoware::localization::ndt::transform_adapters::pose_to_transform (const PoseT &pose, TransformT &transform)
 
template<typename PoseT , typename TransformT >
void autoware::localization::ndt::transform_adapters::transform_to_pose (const TransformT &transform, PoseT &pose)
 
template<typename T >
void autoware::localization::ndt::transform_adapters::pose_to_transform (const EigenPose< T > &pose, EigenTransform< T > &transform)
 
template<typename T >
void autoware::localization::ndt::transform_adapters::pose_to_transform (const EigenPose< T > &pose, RosTransform &transform)
 
template<typename T >
void autoware::localization::ndt::transform_adapters::pose_to_transform (const EigenPose< T > &pose, RosPose &ros_pose)
 
template<typename T >
void autoware::localization::ndt::transform_adapters::transform_to_pose (const RosTransform &transform, EigenPose< T > &pose)
 
template<typename T >
void autoware::localization::ndt::transform_adapters::transform_to_pose (const RosPose &ros_pose, EigenPose< T > &pose)