Loading...
Searching...
No Matches
MagPoseFactor.h
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/geometry/concepts.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
template <class POSE>
class MagPoseFactor: public NoiseModelFactorN<POSE> {
private:
using This = MagPoseFactor<POSE>;
using Base = NoiseModelFactorN<POSE>;
using Point = typename POSE::Translation;
using Rot = typename POSE::Rotation;
const Point measured_;
const Point nM_;
const Point bias_;
boost::optional<POSE> body_P_sensor_;
static const int MeasDim = Point::RowsAtCompileTime;
static const int PoseDim = traits<POSE>::dimension;
static const int RotDim = traits<Rot>::dimension;
using shared_ptr = boost::shared_ptr<MagPoseFactor<POSE>>;
GTSAM_CONCEPT_TESTABLE_TYPE(POSE)
GTSAM_CONCEPT_POSE_TYPE(POSE)
public:
~MagPoseFactor() override {}
MagPoseFactor() {}
MagPoseFactor(Key pose_key,
const Point& measured,
double scale,
const Point& direction,
const Point& bias,
const SharedNoiseModel& model,
const boost::optional<POSE>& body_P_sensor)
: Base(model, pose_key),
measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured),
nM_(scale * direction.normalized()),
bias_(body_P_sensor ? body_P_sensor->rotation() * bias : bias),
body_P_sensor_(body_P_sensor) {}
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this)));
}
// Print out the factor.
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s, keyFormatter);
gtsam::print(Vector(nM_), "local field (nM): ");
gtsam::print(Vector(measured_), "measured field (bM): ");
gtsam::print(Vector(bias_), "magnetometer bias: ");
}
bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
const This *e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) &&
gtsam::equal_with_abs_tol(this->measured_, e->measured_, tol) &&
gtsam::equal_with_abs_tol(this->nM_, e->nM_, tol) &&
gtsam::equal_with_abs_tol(this->bias_, e->bias_, tol);
}
Vector evaluateError(const POSE& nPb, boost::optional<Matrix&> H = boost::none) const override {
// Predict the measured magnetic field h(x) in the *body* frame.
// If body_P_sensor was given, bias_ will have been rotated into the body frame.
Matrix H_rot = Matrix::Zero(MeasDim, RotDim);
const Point hx = nPb.rotation().unrotate(nM_, H_rot, boost::none) + bias_;
if (H) {
// Fill in the relevant part of the Jacobian (just rotation columns).
*H = Matrix::Zero(MeasDim, PoseDim);
const size_t rot_col0 = nPb.rotationInterval().first;
(*H).block(0, rot_col0, MeasDim, RotDim) = H_rot;
}
return (hx - measured_);
}
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(nM_);
ar & BOOST_SERIALIZATION_NVP(bias_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // \class MagPoseFactor
}
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::KeyFormatter
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition Key.h:35
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::equal_with_abs_tol
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
equals with a tolerance
Definition Matrix.h:81
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
gtsam::Factor::equals
bool equals(const This &other, double tol=1e-9) const
check equality
Definition Factor.cpp:42
gtsam::MagPoseFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Implement functions needed for Testable.
Definition MagPoseFactor.h:89
gtsam::MagPoseFactor::~MagPoseFactor
~MagPoseFactor() override
Concept check by type.
Definition MagPoseFactor.h:52
gtsam::MagPoseFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
Equals function.
Definition MagPoseFactor.h:97
gtsam::MagPoseFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagPoseFactor.h:81
gtsam::MagPoseFactor::access
friend class boost::serialization::access
Serialization function.
Definition MagPoseFactor.h:129
gtsam::MagPoseFactor::MagPoseFactor
MagPoseFactor(Key pose_key, const Point &measured, double scale, const Point &direction, const Point &bias, const SharedNoiseModel &model, const boost::optional< POSE > &body_P_sensor)
Construct the factor.
Definition MagPoseFactor.h:67
gtsam::MagPoseFactor::evaluateError
Vector evaluateError(const POSE &nPb, boost::optional< Matrix & > H=boost::none) const override
Implement functions needed to derive from Factor.
Definition MagPoseFactor.h:111
gtsam::MagPoseFactor::MagPoseFactor
MagPoseFactor()
Default constructor - only use for serialization.
Definition MagPoseFactor.h:55
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NoiseModelFactorN< POSE >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
concepts.h
Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...