My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
holistic_fusion Namespace Reference

Absolute. More...

Classes

struct  GraphConfig
class  StaticTransforms
class  DynamicDictionaryContainer
class  DynamicFactorGraphStateKey
class  DynamicTransformDictionary
struct  DynamicVariableType
struct  FileFormats
class  FileLogger
class  GraphManager
 GraphManager class. More...
class  GraphState
struct  ImuMeasurement
class  OptimizerBase
class  OptimizerIsam2
class  OptimizerIsam2Batch
class  OptimizerIsam2FixedLag
class  OptimizerLM
class  OptimizerLMBatch
class  OptimizerLMFixedLag
class  TimeGraphKeyBuffer
class  TransformsDictionary
class  GmsfUnaryExpression
class  GmsfUnaryExpressionAbsolut
class  GmsfUnaryExpressionAbsoluteHeading
class  GmsfUnaryExpressionAbsolutePose3
class  GmsfUnaryExpressionAbsolutePosition3
class  GmsfUnaryExpressionLandmark
class  GmsfUnaryExpressionLandmarkPosition3
class  GmsfUnaryExpressionLocal
class  GmsfUnaryExpressionLocalVelocity3
class  PitchFactor
class  RollFactor
class  YawFactor
class  Angle
 Class for holding an angle. More...
class  Pose
 Class for holding a pose. More...
class  Trajectory
 Class for holding a trajectory. More...
class  Gnss
class  GnssHandler
class  ImuBuffer
class  ImuSignalLowPassFilter
 Second-order Butterworth low-pass filter for IMU signals. More...
struct  BatchOptimizedNavState
struct  BatchOptimizationResult
class  HolisticFusion
class  HolisticFusionClassic
class  HolisticFusionDualGraph
class  HolisticFusionHolistic
class  NavState
class  SafeIntegratedNavState
class  SafeNavStateWithCovarianceAndBias
class  BinaryMeasurement
 Class to represent a binary measurement in the graph. More...
class  BinaryMeasurementXD
class  RobustNorm
 Class to represent a robust norm for a measurement. More...
class  Measurement
 Class to represent a measurement in the graph. It is a pure virtual class. More...
class  UnaryMeasurement
 Class to represent a unary measurement in the graph. More...
class  UnaryMeasurementAbsolute
 Class to represent an absolute unary measurement in the graph. Exact dimension is not known. More...
class  UnaryMeasurementLandmark
class  UnaryMeasurementXD
 Class to represent x-dimensional unary measurement in the graph. More...
class  UnaryMeasurementXDAbsolute
 Class to represent x-dimensional absolute unary measurement in the graph. More...
class  UnaryMeasurementXDLandmark
 Class to represent x-dimensional landmark unary measurement in the graph. More...
class  TrajectoryAlignment
class  TrajectoryAlignmentHandler
class  ElementToRoot
class  StaticTransformsTf
class  StaticTransformsUrdf
class  HolisticFusionRos

Typedefs

typedef std::map< double, gtsam::Key > TimeToKeyMap
typedef std::map< gtsam::Key, double > KeyToTimeMap
using GMSF_MEASUREMENT_CLASS = UnaryMeasurementXDAbsolute<Eigen::Isometry3d, 6>
typedef std::map< double, holistic_fusion::ImuMeasurement, std::less< double >, Eigen::aligned_allocator< std::pair< const double, holistic_fusion::ImuMeasurement > > > TimeToImuMap

Enumerations

enum  VerbosityLevels {
  kDefault0 = 0 , kFunctioningAndStateMachine1 = 1 , kOptimizationEffort2 = 2 , kOperationInformation3 = 3 ,
  kFactorAdding4 = 4
}
enum class  DynamicVariableTypeEnum { Global , RefFrame , Landmark }
enum class  FileFormatType { kCsv , kTum , kG2o }
enum class  UnaryExpressionType { Absolute , Local , Landmark }
enum class  MeasurementTypeEnum { Unary , Binary }
enum class  RobustNormEnum {
  None , Huber , Cauchy , Tukey ,
  GemanMcClure
}

Functions

template<char SYMBOL_CHAR>
constexpr int getSymbolIndex ()
template<int n>
constexpr int countNDStates ()
template<int n, int numStates>
constexpr std::array< char, numStates > getSymbolArrayForNDStates ()
template<typename T, size_t N>
constexpr auto makeValuedArray (T value) -> std::array< T, N >
template<char SYMBOL_CHAR>
constexpr bool isAdmissibleSymbol ()
template<char SYMBOL_CHAR>
constexpr void checkSymbol ()
template<int ARRAY_SIZE>
bool isCharInCharArray (const char c, std::array< char, ARRAY_SIZE > charArray)
gtsam::Pose3_ composeRigidTransformations (const gtsam::Pose3_ &T_frame1_frame2, const gtsam::Pose3_ &T_frame2_frame3)
gtsam::Rot3_ inverseRot3 (const gtsam::Rot3_ &R)
gtsam::Rot2 worldRotationErrorAsHeadingError (const gtsam::Rot3 &R_W_Wmeas, gtsam::OptionalJacobian< 1, 3 > H)
gtsam::Expression< gtsam::Rot2 > headingInWorld (const gtsam::Rot3_ &exp_R_W_S, const gtsam::Rot3_ &exp_R_W_M, const gtsam::Rot3 &R_M_Smeas)
Eigen::Matrix< double, 6, 6 > convertCovarianceGtsamConventionToRosConvention (const Eigen::Matrix< double, 6, 6 > &covGtsam)
 Convert a 6x6 covariance matrix from ROS convention to GTSAM convention.
double rad2deg (double radians)
 Convert the given radian angle to degrees.
float rad2deg (float radians)
 Convert the given radian angle to degrees.
double deg2rad (double degrees)
 Convert the given degree angle to radian.
float deg2rad (float degrees)
 Convert the given degree angle to radian.
template<typename PointT>
float calcSquaredDiff (const PointT &a, const PointT &b)
 Calculate the squared difference of the given two points.
template<typename PointT>
float calcSquaredDiff (const PointT &a, const PointT &b, const float &wb)
 Calculate the squared difference of the given two points.
template<typename PointT>
float calcPointDistance (const PointT &p)
 Calculate the absolute distance of the point to the origin.
template<typename PointT>
float calcSquaredPointDistance (const PointT &p)
 Calculate the squared distance of the point to the origin.
template<typename PointT>
void rotX (PointT &p, const Angle &ang)
 Rotate the given point by the specified angle around the x-axis.
template<typename PointT>
void rotY (PointT &p, const Angle &ang)
 Rotate the given point by the specified angle around the y-axis.
template<typename PointT>
void rotZ (PointT &p, const Angle &ang)
 Rotate the given point by the specified angle around the z-axis.
template<typename PointT>
void rotateZXY (PointT &p, const Angle &angZ, const Angle &angX, const Angle &angY)
 Rotate the given point by the specified angles around the z-, x- respectively y-axis.
template<typename PointT>
void rotateYXZ (PointT &p, const Angle &angY, const Angle &angX, const Angle &angZ)
 Rotate the given point by the specified angles around the y-, x- respectively z-axis.
void invertHomogenousMatrix (const Eigen::Matrix4d &m_in, Eigen::Matrix4d &m_out)
 Invert a homogenous transformation matrix.
void inPlaceRemoveRollPitch (Eigen::Isometry3d &T)
double getYawFromIsometry (const Eigen::Isometry3d &T)
void createPose3CsvFileStream (std::map< std::string, std::ofstream > &fileStreams, const std::string &savePath, const std::string &transformIdentifier, const std::string &timeString, const bool saveCovarianceFlag)
void createLatLonAltCsvFileStream (std::map< std::string, std::ofstream > &fileStreams, const std::string &savePath, const std::string &transformIdentifier, const std::string &timeString)
void writePose3ToCsvFile (std::map< std::string, std::ofstream > &fileStreams, const Eigen::Isometry3d pose, const std::string &transformIdentifier, const double timeStamp, const bool saveCovarianceFlag, const std::optional< Eigen::Matrix< double, 6, 6 > > &optionalPoseCovarianceInWorldRos=std::nullopt)
void writeLatLonAltToCsvFile (std::map< std::string, std::ofstream > &fileStreams, const Eigen::Vector3d &latLonAlt, const std::string &transformIdentifier, const double timeStamp)
std::string getLatestSubdirectory (const std::string &directoryPath)
template<typename T>
void printKey (const std::string &key, T value)
template<>
void printKey (const std::string &key, std::vector< double > vector)
template<typename T>
T tryGetParam (const std::string &key, const ros::NodeHandle &privateNode)
void odomMsgToEigen (const nav_msgs::Odometry &odomLidar, Eigen::Matrix4d &T)
void geometryPoseToEigen (const geometry_msgs::Pose &pose, Eigen::Matrix4d &T)
void geometryPoseToEigen (const geometry_msgs::PoseWithCovarianceStamped &odomLidar, Eigen::Matrix4d &T)
void odomMsgToTf (const nav_msgs::Odometry &odomLidar, tf::Transform &T)
tf::Transform matrix3ToTf (const Eigen::Matrix3d &R)
tf::Transform matrix4ToTf (const Eigen::Matrix4d &T)
tf::Transform isometry3ToTf (const Eigen::Isometry3d &T)
void tfToMatrix4 (const tf::Transform &tf_T, Eigen::Matrix4d &T)
void tfToIsometry3 (const tf::Transform &tf_T, Eigen::Isometry3d &T)
tf::Transform pose3ToTf (const Eigen::Matrix3d &T)

Variables

constexpr char ADMISSIBLE_DYNAMIC_SYMBOL_CHARS [4] = {'r', 'c', 'd', 'l'}
constexpr int ADMISSIBLE_DYNAMIC_STATE_DIM [sizeof(ADMISSIBLE_DYNAMIC_SYMBOL_CHARS)] = {6, 6, 3, 3}

Detailed Description

Absolute.

Local

Typedef Documentation

◆ GMSF_MEASUREMENT_CLASS

◆ KeyToTimeMap

typedef std::map<gtsam::Key, double> holistic_fusion::KeyToTimeMap

Definition at line 21 of file TimeGraphKeyBuffer.h.

◆ TimeToImuMap

typedef std::map<double, holistic_fusion::ImuMeasurement, std::less<double>, Eigen::aligned_allocator<std::pair<const double, holistic_fusion::ImuMeasurement> > > holistic_fusion::TimeToImuMap

Definition at line 35 of file ImuBuffer.hpp.

◆ TimeToKeyMap

typedef std::map<double, gtsam::Key> holistic_fusion::TimeToKeyMap

Definition at line 20 of file TimeGraphKeyBuffer.h.

Enumeration Type Documentation

◆ DynamicVariableTypeEnum

Enumerator
Global 
RefFrame 
Landmark 

Definition at line 10 of file DynamicVariableType.h.

◆ FileFormatType

Enumerator
kCsv 
kTum 
kG2o 

Definition at line 32 of file FileLogger.h.

◆ MeasurementTypeEnum

Enumerator
Unary 
Binary 

Definition at line 17 of file Measurement.h.

◆ RobustNormEnum

Enumerator
None 
Huber 
Cauchy 
Tukey 
GemanMcClure 

Definition at line 20 of file Measurement.h.

◆ UnaryExpressionType

Enumerator
Absolute 
Local 
Landmark 

Definition at line 28 of file GmsfUnaryExpression.h.

◆ VerbosityLevels

Enumerator
kDefault0 
kFunctioningAndStateMachine1 
kOptimizationEffort2 
kOperationInformation3 
kFactorAdding4 

Definition at line 15 of file GraphConfig.h.

Function Documentation

◆ calcPointDistance()

template<typename PointT>
float holistic_fusion::calcPointDistance ( const PointT & p)
inline

Calculate the absolute distance of the point to the origin.

Parameters
pThe point.
Returns
The distance to the point.

Definition at line 89 of file math_utils.h.

◆ calcSquaredDiff() [1/2]

template<typename PointT>
float holistic_fusion::calcSquaredDiff ( const PointT & a,
const PointT & b )
inline

Calculate the squared difference of the given two points.

Parameters
aThe first point.
bThe second point.
Returns
The squared difference between point a and b.

Definition at line 59 of file math_utils.h.

◆ calcSquaredDiff() [2/2]

template<typename PointT>
float holistic_fusion::calcSquaredDiff ( const PointT & a,
const PointT & b,
const float & wb )
inline

Calculate the squared difference of the given two points.

Parameters
aThe first point.
bThe second point.
wbThe weighting factor for the SECOND point.
Returns
The squared difference between point a and b.

Definition at line 75 of file math_utils.h.

◆ calcSquaredPointDistance()

template<typename PointT>
float holistic_fusion::calcSquaredPointDistance ( const PointT & p)
inline

Calculate the squared distance of the point to the origin.

Parameters
pThe point.
Returns
The squared distance to the point.

Definition at line 99 of file math_utils.h.

◆ checkSymbol()

template<char SYMBOL_CHAR>
void holistic_fusion::checkSymbol ( )
constexpr

Definition at line 77 of file AdmissibleGtsamSymbols.h.

◆ composeRigidTransformations()

gtsam::Pose3_ holistic_fusion::composeRigidTransformations ( const gtsam::Pose3_ & T_frame1_frame2,
const gtsam::Pose3_ & T_frame2_frame3 )
inline

Definition at line 31 of file GmsfUnaryExpression.h.

◆ convertCovarianceGtsamConventionToRosConvention()

Eigen::Matrix< double, 6, 6 > holistic_fusion::convertCovarianceGtsamConventionToRosConvention ( const Eigen::Matrix< double, 6, 6 > & covGtsam)
inline

Convert a 6x6 covariance matrix from ROS convention to GTSAM convention.

Parameters
covGtsamThe 6x6 covariance matrix in GTSAM convention.
Returns
The 6x6 covariance matrix in ROS convention.

Definition at line 18 of file conversions.h.

◆ countNDStates()

template<int n>
int holistic_fusion::countNDStates ( )
constexpr

Definition at line 32 of file AdmissibleGtsamSymbols.h.

◆ createLatLonAltCsvFileStream()

void holistic_fusion::createLatLonAltCsvFileStream ( std::map< std::string, std::ofstream > & fileStreams,
const std::string & savePath,
const std::string & transformIdentifier,
const std::string & timeString )

Definition at line 45 of file eigen_wrapped_gtsam_utils.cpp.

◆ createPose3CsvFileStream()

void holistic_fusion::createPose3CsvFileStream ( std::map< std::string, std::ofstream > & fileStreams,
const std::string & savePath,
const std::string & transformIdentifier,
const std::string & timeString,
const bool saveCovarianceFlag )

Definition at line 38 of file eigen_wrapped_gtsam_utils.cpp.

◆ deg2rad() [1/2]

double holistic_fusion::deg2rad ( double degrees)
inline

Convert the given degree angle to radian.

Parameters
degreesThe degree angle to convert.
Returns
The radian angle.

Definition at line 39 of file math_utils.h.

◆ deg2rad() [2/2]

float holistic_fusion::deg2rad ( float degrees)
inline

Convert the given degree angle to radian.

Parameters
degreesThe degree angle to convert.
Returns
The radian angle.

Definition at line 48 of file math_utils.h.

◆ geometryPoseToEigen() [1/2]

void holistic_fusion::geometryPoseToEigen ( const geometry_msgs::Pose & pose,
Eigen::Matrix4d & T )

Definition at line 6 of file conversions.cpp.

◆ geometryPoseToEigen() [2/2]

void holistic_fusion::geometryPoseToEigen ( const geometry_msgs::PoseWithCovarianceStamped & odomLidar,
Eigen::Matrix4d & T )

Definition at line 16 of file conversions.cpp.

◆ getLatestSubdirectory()

std::string holistic_fusion::getLatestSubdirectory ( const std::string & directoryPath)
inline

Definition at line 22 of file input_output.h.

◆ getSymbolArrayForNDStates()

template<int n, int numStates>
std::array< char, numStates > holistic_fusion::getSymbolArrayForNDStates ( )
constexpr

Definition at line 44 of file AdmissibleGtsamSymbols.h.

◆ getSymbolIndex()

template<char SYMBOL_CHAR>
int holistic_fusion::getSymbolIndex ( )
constexpr

Definition at line 20 of file AdmissibleGtsamSymbols.h.

◆ getYawFromIsometry()

double holistic_fusion::getYawFromIsometry ( const Eigen::Isometry3d & T)

Definition at line 31 of file eigen_wrapped_gtsam_utils.cpp.

◆ headingInWorld()

gtsam::Expression< gtsam::Rot2 > holistic_fusion::headingInWorld ( const gtsam::Rot3_ & exp_R_W_S,
const gtsam::Rot3_ & exp_R_W_M,
const gtsam::Rot3 & R_M_Smeas )
inline

Definition at line 40 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ inPlaceRemoveRollPitch()

void holistic_fusion::inPlaceRemoveRollPitch ( Eigen::Isometry3d & T)

Definition at line 22 of file eigen_wrapped_gtsam_utils.cpp.

◆ inverseRot3()

gtsam::Rot3_ holistic_fusion::inverseRot3 ( const gtsam::Rot3_ & R)
inline

Definition at line 36 of file GmsfUnaryExpression.h.

◆ invertHomogenousMatrix()

void holistic_fusion::invertHomogenousMatrix ( const Eigen::Matrix4d & m_in,
Eigen::Matrix4d & m_out )
inline

Invert a homogenous transformation matrix.

Parameters
m_ininput 4x4 matrix
m_outinverted 4x4 matrix

Definition at line 172 of file math_utils.h.

◆ isAdmissibleSymbol()

template<char SYMBOL_CHAR>
bool holistic_fusion::isAdmissibleSymbol ( )
constexpr

Definition at line 66 of file AdmissibleGtsamSymbols.h.

◆ isCharInCharArray()

template<int ARRAY_SIZE>
bool holistic_fusion::isCharInCharArray ( const char c,
std::array< char, ARRAY_SIZE > charArray )
inline

Definition at line 84 of file AdmissibleGtsamSymbols.h.

◆ isometry3ToTf()

tf::Transform holistic_fusion::isometry3ToTf ( const Eigen::Isometry3d & T)

Definition at line 53 of file conversions.cpp.

◆ makeValuedArray()

template<typename T, size_t N>
auto holistic_fusion::makeValuedArray ( T value) ->std::array< T, N >
constexpr

Definition at line 58 of file AdmissibleGtsamSymbols.h.

◆ matrix3ToTf()

tf::Transform holistic_fusion::matrix3ToTf ( const Eigen::Matrix3d & R)

Definition at line 37 of file conversions.cpp.

◆ matrix4ToTf()

tf::Transform holistic_fusion::matrix4ToTf ( const Eigen::Matrix4d & T)

Definition at line 45 of file conversions.cpp.

◆ odomMsgToEigen()

void holistic_fusion::odomMsgToEigen ( const nav_msgs::Odometry & odomLidar,
Eigen::Matrix4d & T )

Definition at line 20 of file conversions.cpp.

◆ odomMsgToTf()

void holistic_fusion::odomMsgToTf ( const nav_msgs::Odometry & odomLidar,
tf::Transform & T )

Definition at line 30 of file conversions.cpp.

◆ pose3ToTf()

tf::Transform holistic_fusion::pose3ToTf ( const Eigen::Matrix3d & T)

Definition at line 79 of file conversions.cpp.

◆ printKey() [1/2]

template<>
void holistic_fusion::printKey ( const std::string & key,
std::vector< double > vector )
inline

Definition at line 22 of file read_ros_params.h.

◆ printKey() [2/2]

template<typename T>
void holistic_fusion::printKey ( const std::string & key,
T value )
inline

Definition at line 17 of file read_ros_params.h.

◆ rad2deg() [1/2]

double holistic_fusion::rad2deg ( double radians)
inline

Convert the given radian angle to degrees.

Parameters
radiansThe radian angle to convert.
Returns
The angle in degrees.

Definition at line 21 of file math_utils.h.

◆ rad2deg() [2/2]

float holistic_fusion::rad2deg ( float radians)
inline

Convert the given radian angle to degrees.

Parameters
radiansThe radian angle to convert.
Returns
The angle in degrees.

Definition at line 30 of file math_utils.h.

◆ rotateYXZ()

template<typename PointT>
void holistic_fusion::rotateYXZ ( PointT & p,
const Angle & angY,
const Angle & angX,
const Angle & angZ )
inline

Rotate the given point by the specified angles around the y-, x- respectively z-axis.

Parameters
pthe point to rotate
angYthe rotation angle around the y-axis
angXthe rotation angle around the x-axis
angZthe rotation angle around the z-axis

Definition at line 161 of file math_utils.h.

◆ rotateZXY()

template<typename PointT>
void holistic_fusion::rotateZXY ( PointT & p,
const Angle & angZ,
const Angle & angX,
const Angle & angY )
inline

Rotate the given point by the specified angles around the z-, x- respectively y-axis.

Parameters
pthe point to rotate
angZthe rotation angle around the z-axis
angXthe rotation angle around the x-axis
angYthe rotation angle around the y-axis

Definition at line 147 of file math_utils.h.

◆ rotX()

template<typename PointT>
void holistic_fusion::rotX ( PointT & p,
const Angle & ang )
inline

Rotate the given point by the specified angle around the x-axis.

Parameters
pthe point to rotate
angthe rotation angle

Definition at line 109 of file math_utils.h.

◆ rotY()

template<typename PointT>
void holistic_fusion::rotY ( PointT & p,
const Angle & ang )
inline

Rotate the given point by the specified angle around the y-axis.

Parameters
pthe point to rotate
angthe rotation angle

Definition at line 121 of file math_utils.h.

◆ rotZ()

template<typename PointT>
void holistic_fusion::rotZ ( PointT & p,
const Angle & ang )
inline

Rotate the given point by the specified angle around the z-axis.

Parameters
pthe point to rotate
angthe rotation angle

Definition at line 133 of file math_utils.h.

◆ tfToIsometry3()

void holistic_fusion::tfToIsometry3 ( const tf::Transform & tf_T,
Eigen::Isometry3d & T )

Definition at line 70 of file conversions.cpp.

◆ tfToMatrix4()

void holistic_fusion::tfToMatrix4 ( const tf::Transform & tf_T,
Eigen::Matrix4d & T )

Definition at line 61 of file conversions.cpp.

◆ tryGetParam()

template<typename T>
T holistic_fusion::tryGetParam ( const std::string & key,
const ros::NodeHandle & privateNode )

Definition at line 32 of file read_ros_params.h.

◆ worldRotationErrorAsHeadingError()

gtsam::Rot2 holistic_fusion::worldRotationErrorAsHeadingError ( const gtsam::Rot3 & R_W_Wmeas,
gtsam::OptionalJacobian< 1, 3 > H )
inline

Definition at line 29 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ writeLatLonAltToCsvFile()

void holistic_fusion::writeLatLonAltToCsvFile ( std::map< std::string, std::ofstream > & fileStreams,
const Eigen::Vector3d & latLonAlt,
const std::string & transformIdentifier,
const double timeStamp )

Definition at line 63 of file eigen_wrapped_gtsam_utils.cpp.

◆ writePose3ToCsvFile()

void holistic_fusion::writePose3ToCsvFile ( std::map< std::string, std::ofstream > & fileStreams,
const Eigen::Isometry3d pose,
const std::string & transformIdentifier,
const double timeStamp,
const bool saveCovarianceFlag,
const std::optional< Eigen::Matrix< double, 6, 6 > > & optionalPoseCovarianceInWorldRos = std::nullopt )

Definition at line 53 of file eigen_wrapped_gtsam_utils.cpp.

Variable Documentation

◆ ADMISSIBLE_DYNAMIC_STATE_DIM

int holistic_fusion::ADMISSIBLE_DYNAMIC_STATE_DIM[sizeof(ADMISSIBLE_DYNAMIC_SYMBOL_CHARS)] = {6, 6, 3, 3}
constexpr

Definition at line 16 of file AdmissibleGtsamSymbols.h.

◆ ADMISSIBLE_DYNAMIC_SYMBOL_CHARS

char holistic_fusion::ADMISSIBLE_DYNAMIC_SYMBOL_CHARS[4] = {'r', 'c', 'd', 'l'}
constexpr

Definition at line 15 of file AdmissibleGtsamSymbols.h.