class mola::factors::Pose3RotationYawBiasFactor

Overview

Like Pose3RotationFactor, but the observed orientation carries an unknown azimuth bias b [rad], a scalar variable, applied as a rotation around the {enu} vertical axis:

observed = Rz(b) * R(T_enu2map (+) Pi (+) sensorPoseOnVehicle)

Typical of magnetometer-based headings: hard/soft-iron residuals or a mounting offset bias all readings by the same, slowly varying, angle.

#include <Pose3RotationYawBiasFactor.h>

class Pose3RotationYawBiasFactor: public gtsam::ExpressionFactorN< gtsam::Rot3, gtsam::Pose3, gtsam::Pose3, double >
{
public:
    // construction

    Pose3RotationYawBiasFactor();

    Pose3RotationYawBiasFactor(
        gtsam::Key kT_enu2map,
        gtsam::Key kPi,
        gtsam::Key kYawBias,
        const gtsam::Pose3& sensorPoseOnVehicle,
        const gtsam::Rot3& observedOrientation,
        const gtsam::SharedNoiseModel& model
        );

    // methods

    gtsam::NonlinearFactor::shared_ptr clone() const;
    gtsam::Expression<gtsam::Rot3> expression(const std::array<gtsam::Key, NARY_EXPRESSION_SIZE>& keys) const;

    void print(
        const std::string& s,
        const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter
        ) const;

    bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) const;
};

Construction

Pose3RotationYawBiasFactor()

default constructor

Methods

gtsam::NonlinearFactor::shared_ptr clone() const

Returns:

a deep copy of this factor

void print(
    const std::string& s,
    const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter
    ) const

implement functions needed for Testable print

bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) const

equals