Loading...
Searching...
No Matches
AttitudeFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* 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/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
namespace gtsam {
class AttitudeFactor {
protected:
Unit3 nZ_, bRef_;
public:
AttitudeFactor() {
}
AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) :
nZ_(nZ), bRef_(bRef) {
}
Vector attitudeError(const Rot3& p,
OptionalJacobian<2,3> H = boost::none) const;
const Unit3& nZ() const {
return nZ_;
}
const Unit3& bRef() const {
return bRef_;
}
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("nZ_", nZ_);
ar & boost::serialization::make_nvp("bRef_", bRef_);
}
};
class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN<Rot3>, public AttitudeFactor {
typedef NoiseModelFactorN<Rot3> Base;
public:
typedef boost::shared_ptr<Rot3AttitudeFactor> shared_ptr;
typedef Rot3AttitudeFactor This;
Rot3AttitudeFactor() {
}
~Rot3AttitudeFactor() override {
}
Rot3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
const Unit3& bRef = Unit3(0, 0, 1)) :
Base(model, key), AttitudeFactor(nZ, bRef) {
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
Vector evaluateError(const Rot3& nRb, //
boost::optional<Matrix&> H = boost::none) const override {
return attitudeError(nRb, H);
}
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::make_nvp("AttitudeFactor",
boost::serialization::base_object<AttitudeFactor>(*this));
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
template<> struct traits<Rot3AttitudeFactor> : public Testable<Rot3AttitudeFactor> {};
class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN<Pose3>,
public AttitudeFactor {
typedef NoiseModelFactorN<Pose3> Base;
public:
typedef boost::shared_ptr<Pose3AttitudeFactor> shared_ptr;
typedef Pose3AttitudeFactor This;
Pose3AttitudeFactor() {
}
~Pose3AttitudeFactor() override {
}
Pose3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model,
const Unit3& bRef = Unit3(0, 0, 1)) :
Base(model, key), AttitudeFactor(nZ, bRef) {
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
Vector evaluateError(const Pose3& nTb, //
boost::optional<Matrix&> H = boost::none) const override {
Vector e = attitudeError(nTb.rotation(), H);
if (H) {
Matrix H23 = *H;
*H = Matrix::Zero(2,6);
H->block<2,3>(0,0) = H23;
}
return e;
}
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::make_nvp("AttitudeFactor",
boost::serialization::base_object<AttitudeFactor>(*this));
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
template<> struct traits<Pose3AttitudeFactor> : public Testable<Pose3AttitudeFactor> {};
}
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Pose3.h
3D Pose
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::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::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition Pose3.cpp:315
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
gtsam::AttitudeFactor::attitudeError
Vector attitudeError(const Rot3 &p, OptionalJacobian< 2, 3 > H=boost::none) const
vector of errors
Definition AttitudeFactor.cpp:26
gtsam::AttitudeFactor::bRef_
Unit3 bRef_
Position measurement in.
Definition AttitudeFactor.h:38
gtsam::AttitudeFactor::AttitudeFactor
AttitudeFactor(const Unit3 &nZ, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition AttitudeFactor.h:51
gtsam::AttitudeFactor::access
friend class boost::serialization::access
Serialization function.
Definition AttitudeFactor.h:67
gtsam::AttitudeFactor::AttitudeFactor
AttitudeFactor()
default constructor - only use for serialization
Definition AttitudeFactor.h:43
gtsam::Rot3AttitudeFactor
Version of AttitudeFactor for Rot3.
Definition AttitudeFactor.h:79
gtsam::Rot3AttitudeFactor::evaluateError
Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition AttitudeFactor.h:124
gtsam::Rot3AttitudeFactor::Rot3AttitudeFactor
Rot3AttitudeFactor()
default constructor - only use for serialization
Definition AttitudeFactor.h:92
gtsam::Rot3AttitudeFactor::shared_ptr
boost::shared_ptr< Rot3AttitudeFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition AttitudeFactor.h:86
gtsam::Rot3AttitudeFactor::Rot3AttitudeFactor
Rot3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition AttitudeFactor.h:105
gtsam::Rot3AttitudeFactor::This
Rot3AttitudeFactor This
Typedef to this class.
Definition AttitudeFactor.h:89
gtsam::Rot3AttitudeFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition AttitudeFactor.h:111
gtsam::Pose3AttitudeFactor
Version of AttitudeFactor for Pose3.
Definition AttitudeFactor.h:154
gtsam::Pose3AttitudeFactor::evaluateError
Vector evaluateError(const Pose3 &nTb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition AttitudeFactor.h:199
gtsam::Pose3AttitudeFactor::shared_ptr
boost::shared_ptr< Pose3AttitudeFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition AttitudeFactor.h:161
gtsam::Pose3AttitudeFactor::This
Pose3AttitudeFactor This
Typedef to this class.
Definition AttitudeFactor.h:164
gtsam::Pose3AttitudeFactor::Pose3AttitudeFactor
Pose3AttitudeFactor()
default constructor - only use for serialization
Definition AttitudeFactor.h:167
gtsam::Pose3AttitudeFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition AttitudeFactor.h:186
gtsam::Pose3AttitudeFactor::Pose3AttitudeFactor
Pose3AttitudeFactor(Key key, const Unit3 &nZ, const SharedNoiseModel &model, const Unit3 &bRef=Unit3(0, 0, 1))
Constructor.
Definition AttitudeFactor.h:180
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NoiseModelFactorN< Rot3 >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::NoiseModelFactorN< Rot3 >::key
Key key() const
Definition NonlinearFactor.h:518