My Project 1.0
Holistic Fusion: A Generic Framework for Multimodal Sensor Fusion
Loading...
Searching...
No Matches
holistic_fusion::GmsfUnaryExpressionAbsoluteHeading Class Referencefinal

#include <GmsfUnaryExpressionAbsoluteHeading.h>

Inheritance diagram for holistic_fusion::GmsfUnaryExpressionAbsoluteHeading:
Inheritance graph
[legend]
Collaboration diagram for holistic_fusion::GmsfUnaryExpressionAbsoluteHeading:
Collaboration graph
[legend]

Public Member Functions

 GmsfUnaryExpressionAbsoluteHeading (const std::shared_ptr< UnaryMeasurementXDAbsolute< Eigen::Matrix3d, 1 > > &headingUnaryMeasurementPtr, const std::string &imuFrameName, const Eigen::Isometry3d &T_I_sensorFrame, const double createReferenceAlignmentKeyframeEveryNSeconds)
 ~GmsfUnaryExpressionAbsoluteHeading ()=default
const gtsam::Vector getNoiseDensity () const override
const gtsam::Rot2 getGtsamMeasurementValue () const override
Public Member Functions inherited from holistic_fusion::GmsfUnaryExpressionAbsolut< gtsam::Rot2, 'c'>
 GmsfUnaryExpressionAbsolut (const std::shared_ptr< UnaryMeasurementAbsolute > &gmsfUnaryAbsoluteMeasurementPtr, const std::string &imuFrameName, const Eigen::Isometry3d &T_I_sensorFrame, const double createReferenceAlignmentKeyframeEveryNSeconds)
 ~GmsfUnaryExpressionAbsolut ()=default
Public Member Functions inherited from holistic_fusion::GmsfUnaryExpression< gtsam::Rot2, UnaryExpressionType::Absolute, CALIBRATION_CHAR >
 GmsfUnaryExpression (const std::shared_ptr< UnaryMeasurement > &gmsfBaseUnaryMeasurementPtr, const std::string &imuFrameName, const Eigen::Isometry3d &T_I_sensorFrame)
 ~GmsfUnaryExpression ()=default
gtsam::Expression< gtsam::Rot2 > createAndReturnExpression (const gtsam::Key &closestGeneralKey, DynamicDictionaryContainer &gtsamDynamicExpressionKeys, const gtsam::NavState &W_imuPropagatedState, const bool optimizeReferenceFramePosesWrtWorldFlag, const bool centerMeasurementsAtKeyframePositionBeforeAlignmentFlag, const bool optimizeExtrinsicSensorToSensorCorrectedOffsetFlag)
virtual const std::shared_ptr< UnaryMeasurement > & getGmsfBaseUnaryMeasurementPtr ()
double getTimestamp () const
const gtsam::Values & getNewOnlineGraphStateValues () const
const gtsam::Values & getNewOfflineGraphStateValues () const
const std::vector< gtsam::PriorFactor< gtsam::Pose3 > > & getNewOnlinePosePriorFactors () const
const std::vector< gtsam::PriorFactor< gtsam::Rot2 > > & getNewOnlineDynamicPriorFactors () const
const std::vector< gtsam::BetweenFactor< gtsam::Pose3 > > & getNewOnlineAndOfflinePoseBetweenFactors () const
const Eigen::Isometry3d & getT_I_sensorFrameInit () const

Protected Member Functions

void generateImuStateInWorldFrameAtKey (const gtsam::Key &closestGeneralKey) final
bool measuresPosition () const final
gtsam::Pose3 computeT_W_fixedFrame_initial (const gtsam::NavState &) final
const Eigen::Vector3d getMeasurementPosition () final
void setMeasurementPosition (const Eigen::Vector3d &) final
void transformStateToReferenceFrameMeasurement (const gtsam::Pose3_ &exp_T_W_fixedFrame) override
void transformImuStateToSensorFrameState () final
void transformSensorFrameStateToSensorFrameCorrectedState (DynamicDictionaryContainer &) final
void applyExtrinsicCalibrationCorrection (const gtsam::Expression< gtsam::Rot2 > &) final
gtsam::Pose3 convertToPose3 (const gtsam::Rot2 &) final
gtsam::Rot2 convertFromPose3 (const gtsam::Pose3 &) final
const gtsam::Expression< gtsam::Rot2 > getGtsamExpression () const override
Protected Member Functions inherited from holistic_fusion::GmsfUnaryExpressionAbsolut< gtsam::Rot2, 'c'>
void transformImuStateFromWorldToReferenceFrame (DynamicDictionaryContainer &gtsamDynamicExpressionKeys, const gtsam::NavState &W_currentPropagatedState, const bool centerMeasurementsAtKeyframePositionBeforeAlignmentFlag) final
void transformLandmarkInWorldToImuFrame (DynamicDictionaryContainer &gtsamDynamicExpressionKeys, const gtsam::NavState &W_currentPropagatedState) final

Private Attributes

std::shared_ptr< UnaryMeasurementXDAbsolute< Eigen::Matrix3d, 1 > > headingUnaryMeasurementPtr_
gtsam::Expression< gtsam::Rot3 > exp_R_W_S_
gtsam::Expression< gtsam::Rot3 > exp_R_W_fixedFrame_

Additional Inherited Members

Public Types inherited from holistic_fusion::GmsfUnaryExpression< gtsam::Rot2, UnaryExpressionType::Absolute, CALIBRATION_CHAR >
using template_type
Protected Attributes inherited from holistic_fusion::GmsfUnaryExpression< gtsam::Rot2, UnaryExpressionType::Absolute, CALIBRATION_CHAR >
const std::shared_ptr< UnaryMeasurement > gmsfBaseUnaryMeasurementPtr_
const std::string imuFrame_
Eigen::Isometry3d T_I_sensorFrameInit_
gtsam::Values newOnlineStateValues_
gtsam::Values newOfflineStateValues_
std::vector< gtsam::PriorFactor< gtsam::Pose3 > > newOnlinePosePriorFactors_
std::vector< gtsam::PriorFactor< gtsam::Rot2 > > newOnlineDynamicPriorFactors_
std::vector< gtsam::BetweenFactor< gtsam::Pose3 > > newOnlineAndOfflinePoseBetweenFactors_

Detailed Description

Expression that constrains the heading of a sensor frame S: the rotation about the z-axis of the world between the estimated and the measured orientation of S. Rotations about the horizontal axes of the world leave it unchanged to first order, so it does not compete with gravity over roll and pitch. For equal roll and pitch, it equals the Euler yaw difference. If the fixed frame M of the measurement is not the world frame and fixed frames are optimized, the measurement goes through the alignment R_W_M. A heading measurement has no position, so it reuses the current alignment keyframe of M and never creates one.

Definition at line 53 of file GmsfUnaryExpressionAbsoluteHeading.h.

Constructor & Destructor Documentation

◆ GmsfUnaryExpressionAbsoluteHeading()

holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::GmsfUnaryExpressionAbsoluteHeading ( const std::shared_ptr< UnaryMeasurementXDAbsolute< Eigen::Matrix3d, 1 > > & headingUnaryMeasurementPtr,
const std::string & imuFrameName,
const Eigen::Isometry3d & T_I_sensorFrame,
const double createReferenceAlignmentKeyframeEveryNSeconds )
inline

Definition at line 56 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ ~GmsfUnaryExpressionAbsoluteHeading()

holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::~GmsfUnaryExpressionAbsoluteHeading ( )
default

Member Function Documentation

◆ applyExtrinsicCalibrationCorrection()

void holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::applyExtrinsicCalibrationCorrection ( const gtsam::Expression< gtsam::Rot2 > & )
inlinefinalprotectedvirtual

Implements holistic_fusion::GmsfUnaryExpression< gtsam::Rot2, UnaryExpressionType::Absolute, CALIBRATION_CHAR >.

Definition at line 105 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ computeT_W_fixedFrame_initial()

gtsam::Pose3 holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::computeT_W_fixedFrame_initial ( const gtsam::NavState & )
inlinefinalprotectedvirtual

Implements holistic_fusion::GmsfUnaryExpressionAbsolut< gtsam::Rot2, 'c'>.

Definition at line 84 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ convertFromPose3()

gtsam::Rot2 holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::convertFromPose3 ( const gtsam::Pose3 & )
inlinefinalprotectedvirtual

Implements holistic_fusion::GmsfUnaryExpression< gtsam::Rot2, UnaryExpressionType::Absolute, CALIBRATION_CHAR >.

Definition at line 114 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ convertToPose3()

gtsam::Pose3 holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::convertToPose3 ( const gtsam::Rot2 & )
inlinefinalprotectedvirtual

Implements holistic_fusion::GmsfUnaryExpression< gtsam::Rot2, UnaryExpressionType::Absolute, CALIBRATION_CHAR >.

Definition at line 110 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ generateImuStateInWorldFrameAtKey()

void holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::generateImuStateInWorldFrameAtKey ( const gtsam::Key & closestGeneralKey)
inlinefinalprotectedvirtual

Implements holistic_fusion::GmsfUnaryExpression< gtsam::Rot2, UnaryExpressionType::Absolute, CALIBRATION_CHAR >.

Definition at line 76 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ getGtsamExpression()

const gtsam::Expression< gtsam::Rot2 > holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::getGtsamExpression ( ) const
inlinenodiscardoverrideprotectedvirtual

Implements holistic_fusion::GmsfUnaryExpression< gtsam::Rot2, UnaryExpressionType::Absolute, CALIBRATION_CHAR >.

Definition at line 119 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ getGtsamMeasurementValue()

const gtsam::Rot2 holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::getGtsamMeasurementValue ( ) const
inlinenodiscardoverridevirtual

Implements holistic_fusion::GmsfUnaryExpression< gtsam::Rot2, UnaryExpressionType::Absolute, CALIBRATION_CHAR >.

Definition at line 72 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ getMeasurementPosition()

const Eigen::Vector3d holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::getMeasurementPosition ( )
inlinefinalprotectedvirtual

Implements holistic_fusion::GmsfUnaryExpressionAbsolut< gtsam::Rot2, 'c'>.

Definition at line 86 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ getNoiseDensity()

const gtsam::Vector holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::getNoiseDensity ( ) const
inlinenodiscardoverridevirtual

Implements holistic_fusion::GmsfUnaryExpression< gtsam::Rot2, UnaryExpressionType::Absolute, CALIBRATION_CHAR >.

Definition at line 69 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ measuresPosition()

bool holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::measuresPosition ( ) const
inlinefinalprotectedvirtual

Implements holistic_fusion::GmsfUnaryExpressionAbsolut< gtsam::Rot2, 'c'>.

Definition at line 81 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ setMeasurementPosition()

void holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::setMeasurementPosition ( const Eigen::Vector3d & )
inlinefinalprotectedvirtual

Implements holistic_fusion::GmsfUnaryExpressionAbsolut< gtsam::Rot2, 'c'>.

Definition at line 88 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ transformImuStateToSensorFrameState()

void holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::transformImuStateToSensorFrameState ( )
inlinefinalprotectedvirtual

Implements holistic_fusion::GmsfUnaryExpression< gtsam::Rot2, UnaryExpressionType::Absolute, CALIBRATION_CHAR >.

Definition at line 98 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ transformSensorFrameStateToSensorFrameCorrectedState()

void holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::transformSensorFrameStateToSensorFrameCorrectedState ( DynamicDictionaryContainer & )
inlinefinalprotectedvirtual

Reimplemented from holistic_fusion::GmsfUnaryExpression< gtsam::Rot2, UnaryExpressionType::Absolute, CALIBRATION_CHAR >.

Definition at line 101 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ transformStateToReferenceFrameMeasurement()

void holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::transformStateToReferenceFrameMeasurement ( const gtsam::Pose3_ & exp_T_W_fixedFrame)
inlineoverrideprotectedvirtual

Implements holistic_fusion::GmsfUnaryExpressionAbsolut< gtsam::Rot2, 'c'>.

Definition at line 93 of file GmsfUnaryExpressionAbsoluteHeading.h.

Member Data Documentation

◆ exp_R_W_fixedFrame_

gtsam::Expression<gtsam::Rot3> holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::exp_R_W_fixedFrame_
private

Definition at line 129 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ exp_R_W_S_

gtsam::Expression<gtsam::Rot3> holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::exp_R_W_S_
private

Definition at line 128 of file GmsfUnaryExpressionAbsoluteHeading.h.

◆ headingUnaryMeasurementPtr_

std::shared_ptr<UnaryMeasurementXDAbsolute<Eigen::Matrix3d, 1> > holistic_fusion::GmsfUnaryExpressionAbsoluteHeading::headingUnaryMeasurementPtr_
private

Definition at line 125 of file GmsfUnaryExpressionAbsoluteHeading.h.

The documentation for this class was generated from the following file: