NASA Astrobee Robot Software  0.16.6
Flight software for the Astrobee robot operating inside the International Space Station.
localization_common Namespace Reference

Classes

class  Averager
 
class  CombinedNavState
 
class  CombinedNavStateCovariances
 
struct  CombinedNavStateNoise
 
struct  FeaturePoint3d
 
struct  FeaturePoint3dNoise
 
struct  ImageCorrespondences
 
class  MeasurementBuffer
 
class  PoseInterpolater
 
struct  PoseWithCovariance
 
class  RateTimer
 
class  RosTimer
 
class  Sampler
 
class  Timer
 
class  TimestampedSet
 
struct  TimestampedValue
 

Typedefs

using FeatureId = int
 
using FeaturePoint3ds = std::vector< FeaturePoint3d >
 
using PoseCovariance = Eigen::Matrix< double, 6, 6 >
 
using Time = double
 

Enumerations

enum  Confidence { kGood, kPoor, kLost }
 

Functions

double RandomDouble ()
 
double RandomPositiveDouble ()
 
double RandomDouble (const double min, const double max)
 
double RandomGaussianDouble (const double mean, const double stddev)
 
bool RandomBool ()
 
double Noise (const double stddev)
 
template<int Dim>
Eigen::Matrix< double, Dim, 1 > RandomVector ()
 
Eigen::Vector3d RandomVector3d ()
 
Eigen::Vector3d RandomPoint3d ()
 
Eigen::Vector2d RandomVector2d ()
 
Eigen::Vector2d RandomPoint2d ()
 
gtsam::Pose3 RandomPose ()
 
Eigen::Isometry3d RandomIsometry3d ()
 
Eigen::Affine3d RandomAffine3d ()
 
Eigen::Matrix3d RandomIntrinsics ()
 
Eigen::Isometry3d RandomIdentityCenteredIsometry3d (const double translation_stddev, const double rotation_stddev)
 
Eigen::Isometry3d AddNoiseToIsometry3d (const Eigen::Isometry3d &pose, const double translation_stddev, const double rotation_stddev)
 
gtsam::Pose3 AddNoiseToPose (const gtsam::Pose3 &pose, const double translation_stddev, const double rotation_stddev)
 
template<int N>
Eigen::Matrix< double, N, 1 > AddNoiseToVector (const Eigen::Matrix< double, N, 1 > &vector, const double noise_stddev)
 
Eigen::Isometry3d RandomFrontFacingPose (const double rho_min, const double rho_max, const double phi_min, const double phi_max, const double z_rho_scale, const double yaw_min, const double yaw_max, const double pitch_min, const double pitch_max, const double roll_min, const double roll_max)
 
Eigen::Isometry3d RandomFrontFacingPose ()
 
std::vector< Eigen::Vector3d > RandomFrontFacingPoints (const int num_points)
 
Eigen::Vector3d RandomFrontFacingPoint ()
 
Eigen::Vector3d RandomFrontFacingPoint (const double rho_min, const double rho_max, const double phi_min, const double phi_max, const double z_rho_scale)
 
bool MatrixEquality (const Eigen::MatrixXd &lhs, const Eigen::MatrixXd &rhs, const double tolerance)
 
Time GetTime (const int seconds, const int nanoseconds)
 
gtsam::Pose3 LoadTransform (config_reader::ConfigReader &config, const std::string &transform_config_name)
 
gtsam::Vector3 LoadVector3 (config_reader::ConfigReader &config, const std::string &config_name)
 
gtsam::Cal3_S2 LoadCameraIntrinsics (config_reader::ConfigReader &config, const std::string &intrinsics_config_name)
 
gtsam::Pose3 GtPose (const Eigen::Isometry3d &eigen_pose)
 
Eigen::Isometry3d EigenPose (const CombinedNavState &combined_nav_state)
 
Eigen::Isometry3d EigenPose (const gtsam::Pose3 &pose)
 
gtsam::Pose3 PoseFromMsgWithExtrinsics (const geometry_msgs::Pose &pose, const gtsam::Pose3 &sensor_T_body)
 
void LoadGraphLocalizerConfig (config_reader::ConfigReader &config, const std::string &path_prefix="")
 
void SetEnvironmentConfigs (const std::string &astrobee_configs_path, const std::string &world, const std::string &robot_config_file)
 
ros::Time RosTimeFromHeader (const std_msgs::Header &header)
 
Time TimeFromHeader (const std_msgs::Header &header)
 
Time TimeFromRosTime (const ros::Time &time)
 
void TimeToHeader (const Time timestamp, std_msgs::Header &header)
 
void TimeToMsg (const Time timestamp, ros::Time &time_msg)
 
gtsam::Pose3 PoseFromMsg (const geometry_msgs::PoseStamped &msg)
 
gtsam::Pose3 PoseFromMsg (const geometry_msgs::Pose &msg_pose)
 
void PoseToMsg (const gtsam::Pose3 &pose, geometry_msgs::Pose &msg_pose)
 
geometry_msgs::TransformStamped 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 PoseToTF (const gtsam::Pose3 &pose, const std::string &parent_frame, const std::string &child_frame, const Time timestamp, const std::string &platform_name="")
 
CombinedNavState CombinedNavStateFromMsg (const ff_msgs::GraphState &loc_msg)
 
CombinedNavStateCovariances CombinedNavStateCovariancesFromMsg (const ff_msgs::GraphState &loc_msg)
 
gtsam::Vector3 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)
 
template<class LocMsgType >
void CombinedNavStateToMsg (const CombinedNavState &combined_nav_state, LocMsgType &loc_msg)
 
template<class LocMsgType >
void CombinedNavStateCovariancesToMsg (const CombinedNavStateCovariances &covariances, LocMsgType &loc_msg)
 
template<typename MatrixType >
double LogDeterminant (const MatrixType &matrix)
 
double 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 Isometry3d (const Eigen::Vector3d &translation, const RotationType &rotation)
 
double Deg2Rad (const double degrees)
 
double Rad2Deg (const double radians)
 
Eigen::Vector3d CylindricalToCartesian (const Eigen::Vector3d &cylindrical_coordinates)
 
Eigen::Matrix3d RotationFromEulerAngles (const double yaw, const double pitch, const double roll)
 
Eigen::Isometry3d FrameChangeRelativePose (const Eigen::Isometry3d &a_F_relative_pose, const Eigen::Isometry3d &b_T_a)
 
Eigen::Matrix< double, 6, 6 > FrameChangeRelativeCovariance (const Eigen::Matrix< double, 6, 6 > &a_F_relative_pose_covariance, const Eigen::Isometry3d &b_T_a)
 
PoseWithCovariance FrameChangeRelativePoseWithCovariance (const PoseWithCovariance &a_F_relative_pose_with_covariance, const Eigen::Isometry3d &b_T_a)
 
PoseWithCovariance InvertPoseWithCovariance (const PoseWithCovariance &pose_with_covariance)
 
template<int CostDim, int StateDim>
boost::optional< Eigen::Matrix< double, StateDim, StateDim > > LeastSquaresCovariance (const std::vector< Eigen::Matrix< double, CostDim, StateDim >> &cost_jacobians)
 
template<typename T >
std::vector< T > Transform (const std::vector< T > &a_T_elements, const Eigen::Isometry3d &b_T_a)
 
template<typename T >
std::vector< T > 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 > > TransformPointsWithNormals (const std::vector< Eigen::Vector3d > &points, const std::vector< Eigen::Vector3d > &normals, const Eigen::Isometry3d &b_T_a)
 
Eigen::Isometry3d Interpolate (const Eigen::Isometry3d &lower_bound_pose, const Eigen::Isometry3d &upper_bound_pose, const double alpha)
 

Typedef Documentation

◆ FeatureId

using localization_common::FeatureId = typedef int

◆ FeaturePoint3ds

◆ PoseCovariance

using localization_common::PoseCovariance = typedef Eigen::Matrix<double, 6, 6>

◆ Time

using localization_common::Time = typedef double

Enumeration Type Documentation

◆ Confidence

Enumerator
kGood 
kPoor 
kLost 

Function Documentation

◆ AddNoiseToIsometry3d()

Eigen::Isometry3d localization_common::AddNoiseToIsometry3d ( const Eigen::Isometry3d &  pose,
const double  translation_stddev,
const double  rotation_stddev 
)

◆ AddNoiseToPose()

gtsam::Pose3 localization_common::AddNoiseToPose ( const gtsam::Pose3 &  pose,
const double  translation_stddev,
const double  rotation_stddev 
)

◆ AddNoiseToVector()

template<int N>
Eigen::Matrix< double, N, 1 > localization_common::AddNoiseToVector ( const Eigen::Matrix< double, N, 1 > &  vector,
const double  noise_stddev 
)

◆ CombinedNavStateCovariancesFromMsg()

CombinedNavStateCovariances localization_common::CombinedNavStateCovariancesFromMsg ( const ff_msgs::GraphState &  loc_msg)

◆ CombinedNavStateCovariancesToMsg()

template<class LocMsgType >
void localization_common::CombinedNavStateCovariancesToMsg ( const CombinedNavStateCovariances covariances,
LocMsgType &  loc_msg 
)

◆ CombinedNavStateFromMsg()

CombinedNavState localization_common::CombinedNavStateFromMsg ( const ff_msgs::GraphState &  loc_msg)

◆ CombinedNavStateToMsg()

template<class LocMsgType >
void localization_common::CombinedNavStateToMsg ( const CombinedNavState combined_nav_state,
LocMsgType &  loc_msg 
)

◆ CylindricalToCartesian()

Eigen::Vector3d localization_common::CylindricalToCartesian ( const Eigen::Vector3d &  cylindrical_coordinates)

◆ Deg2Rad()

double localization_common::Deg2Rad ( const double  degrees)

◆ EigenPose() [1/2]

Eigen::Isometry3d localization_common::EigenPose ( const CombinedNavState combined_nav_state)

◆ EigenPose() [2/2]

Eigen::Isometry3d localization_common::EigenPose ( const gtsam::Pose3 &  pose)

◆ FrameChangeRelativeCovariance()

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 
)

◆ FrameChangeRelativePose()

Eigen::Isometry3d localization_common::FrameChangeRelativePose ( const Eigen::Isometry3d &  a_F_relative_pose,
const Eigen::Isometry3d &  b_T_a 
)

◆ FrameChangeRelativePoseWithCovariance()

PoseWithCovariance localization_common::FrameChangeRelativePoseWithCovariance ( const PoseWithCovariance a_F_relative_pose_with_covariance,
const Eigen::Isometry3d &  b_T_a 
)

◆ GetTime()

Time localization_common::GetTime ( const int  seconds,
const int  nanoseconds 
)

◆ GtPose()

gtsam::Pose3 localization_common::GtPose ( const Eigen::Isometry3d &  eigen_pose)

◆ Interpolate()

Eigen::Isometry3d localization_common::Interpolate ( const Eigen::Isometry3d &  lower_bound_pose,
const Eigen::Isometry3d &  upper_bound_pose,
const double  alpha 
)

◆ InvertPoseWithCovariance()

PoseWithCovariance localization_common::InvertPoseWithCovariance ( const PoseWithCovariance pose_with_covariance)

◆ Isometry3d()

template<typename RotationType >
Eigen::Isometry3d localization_common::Isometry3d ( const Eigen::Vector3d &  translation,
const RotationType &  rotation 
)

◆ LeastSquaresCovariance()

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)

◆ LoadCameraIntrinsics()

gtsam::Cal3_S2 localization_common::LoadCameraIntrinsics ( config_reader::ConfigReader config,
const std::string &  intrinsics_config_name 
)

◆ LoadGraphLocalizerConfig()

void localization_common::LoadGraphLocalizerConfig ( config_reader::ConfigReader config,
const std::string &  path_prefix = "" 
)

◆ LoadTransform()

gtsam::Pose3 localization_common::LoadTransform ( config_reader::ConfigReader config,
const std::string &  transform_config_name 
)

◆ LoadVector3()

gtsam::Vector3 localization_common::LoadVector3 ( config_reader::ConfigReader config,
const std::string &  config_name 
)

◆ LogDeterminant()

template<typename MatrixType >
double localization_common::LogDeterminant ( const MatrixType &  matrix)

◆ MatrixEquality()

bool localization_common::MatrixEquality ( const Eigen::MatrixXd &  lhs,
const Eigen::MatrixXd &  rhs,
const double  tolerance 
)

◆ Noise()

double localization_common::Noise ( const double  stddev)

◆ PoseCovarianceSane()

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 
)

◆ PoseFromMsg() [1/2]

gtsam::Pose3 localization_common::PoseFromMsg ( const geometry_msgs::Pose &  msg_pose)

◆ PoseFromMsg() [2/2]

gtsam::Pose3 localization_common::PoseFromMsg ( const geometry_msgs::PoseStamped &  msg)

◆ PoseFromMsgWithExtrinsics()

gtsam::Pose3 localization_common::PoseFromMsgWithExtrinsics ( const geometry_msgs::Pose &  pose,
const gtsam::Pose3 &  sensor_T_body 
)

◆ PoseToMsg()

void localization_common::PoseToMsg ( const gtsam::Pose3 &  pose,
geometry_msgs::Pose &  msg_pose 
)

◆ PoseToTF() [1/2]

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 = "" 
)

◆ PoseToTF() [2/2]

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 = "" 
)

◆ Rad2Deg()

double localization_common::Rad2Deg ( const double  radians)

◆ RandomAffine3d()

Eigen::Affine3d localization_common::RandomAffine3d ( )

◆ RandomBool()

bool localization_common::RandomBool ( )

◆ RandomDouble() [1/2]

double localization_common::RandomDouble ( )

◆ RandomDouble() [2/2]

double localization_common::RandomDouble ( const double  min,
const double  max 
)

◆ RandomFrontFacingPoint() [1/2]

Eigen::Vector3d localization_common::RandomFrontFacingPoint ( )

◆ RandomFrontFacingPoint() [2/2]

Eigen::Vector3d localization_common::RandomFrontFacingPoint ( const double  rho_min,
const double  rho_max,
const double  phi_min,
const double  phi_max,
const double  z_rho_scale 
)

◆ RandomFrontFacingPoints()

std::vector< Eigen::Vector3d > localization_common::RandomFrontFacingPoints ( const int  num_points)

◆ RandomFrontFacingPose() [1/2]

Eigen::Isometry3d localization_common::RandomFrontFacingPose ( )

◆ RandomFrontFacingPose() [2/2]

Eigen::Isometry3d localization_common::RandomFrontFacingPose ( const double  rho_min,
const double  rho_max,
const double  phi_min,
const double  phi_max,
const double  z_rho_scale,
const double  yaw_min,
const double  yaw_max,
const double  pitch_min,
const double  pitch_max,
const double  roll_min,
const double  roll_max 
)

◆ RandomGaussianDouble()

double localization_common::RandomGaussianDouble ( const double  mean,
const double  stddev 
)

◆ RandomIdentityCenteredIsometry3d()

Eigen::Isometry3d localization_common::RandomIdentityCenteredIsometry3d ( const double  translation_stddev,
const double  rotation_stddev 
)

◆ RandomIntrinsics()

Eigen::Matrix3d localization_common::RandomIntrinsics ( )

◆ RandomIsometry3d()

Eigen::Isometry3d localization_common::RandomIsometry3d ( )

◆ RandomPoint2d()

Eigen::Vector2d localization_common::RandomPoint2d ( )

◆ RandomPoint3d()

Eigen::Vector3d localization_common::RandomPoint3d ( )

◆ RandomPose()

gtsam::Pose3 localization_common::RandomPose ( )

◆ RandomPositiveDouble()

double localization_common::RandomPositiveDouble ( )

◆ RandomVector()

template<int Dim>
Eigen::Matrix< double, Dim, 1 > localization_common::RandomVector ( )

◆ RandomVector2d()

Eigen::Vector2d localization_common::RandomVector2d ( )

◆ RandomVector3d()

Eigen::Vector3d localization_common::RandomVector3d ( )

◆ RemoveGravityFromAccelerometerMeasurement()

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 
)

◆ RosTimeFromHeader()

ros::Time localization_common::RosTimeFromHeader ( const std_msgs::Header &  header)

◆ Rotate()

template<typename T >
std::vector< T > localization_common::Rotate ( const std::vector< T > &  a_F_a_T_elements,
const Eigen::Matrix3d &  b_R_a 
)

◆ RotationFromEulerAngles()

Eigen::Matrix3d localization_common::RotationFromEulerAngles ( const double  yaw,
const double  pitch,
const double  roll 
)

◆ SetEnvironmentConfigs()

void localization_common::SetEnvironmentConfigs ( const std::string &  astrobee_configs_path,
const std::string &  world,
const std::string &  robot_config_file 
)

◆ TimeFromHeader()

Time localization_common::TimeFromHeader ( const std_msgs::Header &  header)

◆ TimeFromRosTime()

Time localization_common::TimeFromRosTime ( const ros::Time &  time)

◆ TimeToHeader()

void localization_common::TimeToHeader ( const Time  timestamp,
std_msgs::Header &  header 
)

◆ TimeToMsg()

void localization_common::TimeToMsg ( const Time  timestamp,
ros::Time &  time_msg 
)

◆ Transform()

template<typename T >
std::vector< T > localization_common::Transform ( const std::vector< T > &  a_T_elements,
const Eigen::Isometry3d &  b_T_a 
)

◆ TransformPointsWithNormals()

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 
)