utilities.h File Reference

#include <config_reader/config_reader.h>
#include <ff_msgs/CombinedNavState.h>
#include <ff_msgs/GraphVIOState.h>
#include <ff_msgs/VisualLandmarks.h>
#include <localization_common/combined_nav_state.h>
#include <localization_common/combined_nav_state_covariances.h>
#include <localization_common/pose_with_covariance.h>
#include <localization_common/time.h>
#include <msg_conversions/msg_conversions.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <Eigen/Core>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <std_msgs/Header.h>
#include <string>
#include <utility>
#include <vector>

Include dependency graph for utilities.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Namespaces
	localization_common

Functions
gtsam::Pose3	localization_common::LoadTransform (config_reader::ConfigReader &config, const std::string &transform_config_name, const std::string &prefix="")
gtsam::Vector3	localization_common::LoadVector3 (config_reader::ConfigReader &config, const std::string &config_name, const std::string &prefix="")
Eigen::Matrix3d	localization_common::LoadCameraIntrinsicsMatrix (config_reader::ConfigReader &config, const std::string &intrinsics_config_name, const std::string &prefix="")
gtsam::Cal3_S2	localization_common::LoadCameraIntrinsics (config_reader::ConfigReader &config, const std::string &intrinsics_config_name, const std::string &prefix="")
gtsam::Pose3	localization_common::GtPose (const Eigen::Isometry3d &eigen_pose)
Eigen::Isometry3d	localization_common::EigenPose (const CombinedNavState &combined_nav_state)
Eigen::Isometry3d	localization_common::EigenPose (const gtsam::Pose3 &pose)
gtsam::Pose3	localization_common::PoseFromMsgWithExtrinsics (const geometry_msgs::Pose &pose, const gtsam::Pose3 &sensor_T_body)
void	localization_common::LoadGraphLocalizerConfig (config_reader::ConfigReader &config, const std::string &path_prefix="")
void	localization_common::LoadGraphVIOConfig (config_reader::ConfigReader &config, const std::string &path_prefix="")
void	localization_common::SetEnvironmentConfigs (const std::string &world="iss", const std::string &robot_config_file="bumble.config")
ros::Time	localization_common::RosTimeFromHeader (const std_msgs::Header &header)
Time	localization_common::TimeFromHeader (const std_msgs::Header &header)
Time	localization_common::TimeFromRosTime (const ros::Time &time)
void	localization_common::TimeToHeader (const Time timestamp, std_msgs::Header &header)
void	localization_common::TimeToMsg (const Time timestamp, ros::Time &time_msg)
gtsam::Pose3	localization_common::PoseFromMsg (const geometry_msgs::PoseStamped &msg)
gtsam::Pose3	localization_common::PoseFromMsg (const geometry_msgs::Pose &msg_pose)
void	localization_common::PoseToMsg (const gtsam::Pose3 &pose, geometry_msgs::Pose &msg_pose)
geometry_msgs::TransformStamped	localization_common::PoseToTF (const Eigen::Isometry3d &pose, const std::string &parent_frame, const std::string &child_frame, const Time timestamp, const std::string &platform_name="")
geometry_msgs::TransformStamped	localization_common::PoseToTF (const gtsam::Pose3 &pose, const std::string &parent_frame, const std::string &child_frame, const Time timestamp, const std::string &platform_name="")
CombinedNavState	localization_common::CombinedNavStateFromMsg (const ff_msgs::CombinedNavState &msg)
CombinedNavStateCovariances	localization_common::CombinedNavStateCovariancesFromMsg (const ff_msgs::CombinedNavState &msg)
TimestampedSet< PoseCovariance >	localization_common::CorrelationCovariancesFromMsg (const ff_msgs::CombinedNavState &msg)
gtsam::Vector3	localization_common::RemoveGravityFromAccelerometerMeasurement (const gtsam::Vector3 &global_F_gravity, const gtsam::Pose3 &body_T_imu, const gtsam::Pose3 &global_T_body, const gtsam::Vector3 &uncorrected_accelerometer_measurement)
ff_msgs::CombinedNavState	localization_common::CombinedNavStateToMsg (const CombinedNavState &combined_nav_state, const PoseCovariance &pose_covariance, const Eigen::Matrix3d &velocity_covariance, const Eigen::Matrix< double, 6, 6 > &imu_bias_covariance, const TimestampedSet< PoseCovariance > &correlation_covariances={})
template<class LocMsgType >
void	localization_common::CombinedNavStateToLocMsg (const CombinedNavState &combined_nav_state, LocMsgType &loc_msg)
template<class LocMsgType >
void	localization_common::CombinedNavStateCovariancesToLocMsg (const CombinedNavStateCovariances &covariances, LocMsgType &loc_msg)
template<typename MatrixType >
double	localization_common::LogDeterminant (const MatrixType &matrix)
double	localization_common::PoseCovarianceSane (const Eigen::Matrix< double, 6, 6 > &pose_covariance, const double position_covariance_threshold=0, const double orientation_covariance_threshold=0, const bool check_position_covariance=true, const bool check_orientation_covariance=true)
template<typename RotationType >
Eigen::Isometry3d	localization_common::Isometry3d (const Eigen::Vector3d &translation, const RotationType &rotation)
double	localization_common::Deg2Rad (const double degrees)
double	localization_common::Rad2Deg (const double radians)
Eigen::Vector3d	localization_common::CylindricalToCartesian (const Eigen::Vector3d &cylindrical_coordinates)
Eigen::Matrix3d	localization_common::RotationFromEulerAngles (const double yaw, const double pitch, const double roll)
PoseCovariance	localization_common::RelativePoseCovariance (const PoseWithCovariance &a, const PoseWithCovariance &b, const boost::optional< PoseCovariance > covariance_a_b=boost::none)
Eigen::Isometry3d	localization_common::FrameChangeRelativePose (const Eigen::Isometry3d &a_F_relative_pose, const Eigen::Isometry3d &b_T_a)
Eigen::Matrix< double, 6, 6 >	localization_common::FrameChangeRelativeCovariance (const Eigen::Matrix< double, 6, 6 > &a_F_relative_pose_covariance, const Eigen::Isometry3d &b_T_a)
PoseWithCovariance	localization_common::FrameChangeRelativePoseWithCovariance (const PoseWithCovariance &a_F_relative_pose_with_covariance, const Eigen::Isometry3d &b_T_a)
PoseWithCovariance	localization_common::InvertPoseWithCovariance (const PoseWithCovariance &pose_with_covariance)
template<int CostDim, int StateDim>
boost::optional< Eigen::Matrix< double, StateDim, StateDim > >	localization_common::LeastSquaresCovariance (const std::vector< Eigen::Matrix< double, CostDim, StateDim >> &cost_jacobians)
template<typename T >
std::vector< T >	localization_common::Transform (const std::vector< T > &a_T_elements, const Eigen::Isometry3d &b_T_a)
template<typename T >
std::vector< T >	localization_common::Rotate (const std::vector< T > &a_F_a_T_elements, const Eigen::Matrix3d &b_R_a)
std::pair< std::vector< Eigen::Vector3d >, std::vector< Eigen::Vector3d > >	localization_common::TransformPointsWithNormals (const std::vector< Eigen::Vector3d > &points, const std::vector< Eigen::Vector3d > &normals, const Eigen::Isometry3d &b_T_a)
Eigen::Isometry3d	localization_common::Interpolate (const Eigen::Isometry3d &lower_bound_pose, const Eigen::Isometry3d &upper_bound_pose, const double alpha)
PoseWithCovariance	localization_common::Interpolate (const PoseWithCovariance &lower_bound_pose, const PoseWithCovariance &upper_bound_pose, const double alpha)
gtsam::noiseModel::Robust::shared_ptr	localization_common::Robust (const gtsam::SharedNoiseModel &noise, const double huber_k)
template<typename FactorType >
void	localization_common::DeleteFactors (gtsam::NonlinearFactorGraph &graph)
template<typename FactorType >
int	localization_common::NumFactors (const gtsam::NonlinearFactorGraph &graph)
template<typename FactorType >
std::vector< const FactorType * >	localization_common::Factors (const gtsam::NonlinearFactorGraph &graph)