Loading...
Searching...
No Matches
PoseRotationPrior.h
Go to the documentation of this file.
#pragma once
#include <gtsam/geometry/concepts.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
template<class POSE>
class PoseRotationPrior : public NoiseModelFactorN<POSE> {
public:
typedef PoseRotationPrior<POSE> This;
typedef NoiseModelFactorN<POSE> Base;
typedef POSE Pose;
typedef typename POSE::Translation Translation;
typedef typename POSE::Rotation Rotation;
GTSAM_CONCEPT_POSE_TYPE(Pose)
GTSAM_CONCEPT_GROUP_TYPE(Pose)
GTSAM_CONCEPT_LIE_TYPE(Rotation)
// Get dimensions of pose and rotation type at compile time
static const int xDim = FixedDimension<POSE>::value;
static const int rDim = FixedDimension<typename POSE::Rotation>::value;
protected:
Rotation measured_;
public:
PoseRotationPrior() {}
PoseRotationPrior(Key key, const Rotation& rot_z, const SharedNoiseModel& model)
: Base(model, key), measured_(rot_z) {}
PoseRotationPrior(Key key, const POSE& pose_z, const SharedNoiseModel& model)
: Base(model, key), measured_(pose_z.rotation()) {}
~PoseRotationPrior() override {}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
// access
const Rotation& measured() const { return measured_; }
// testable
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) && measured_.equals(e->measured_, tol);
}
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
Base::print(s + "PoseRotationPrior", keyFormatter);
measured_.print("Measured Rotation");
}
Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const override {
const Rotation& newR = pose.rotation();
if (H) {
*H = Matrix::Zero(rDim, xDim);
std::pair<size_t, size_t> rotInterval = POSE::rotationInterval();
(*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim);
}
return measured_.localCoordinates(newR);
}
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_);
}
};
} // \namespace gtsam
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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::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::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NoiseModelFactorN< POSE >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::NoiseModelFactorN< POSE >::key
Key key() const
Definition NonlinearFactor.h:518
gtsam::PoseRotationPrior
Definition PoseRotationPrior.h:19
gtsam::PoseRotationPrior::PoseRotationPrior
PoseRotationPrior(Key key, const Rotation &rot_z, const SharedNoiseModel &model)
standard constructor
Definition PoseRotationPrior.h:46
gtsam::PoseRotationPrior::PoseRotationPrior
PoseRotationPrior(Key key, const POSE &pose_z, const SharedNoiseModel &model)
Constructor that pulls the translation from an incoming POSE.
Definition PoseRotationPrior.h:50
gtsam::PoseRotationPrior::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition PoseRotationPrior.h:56
gtsam::PoseRotationPrior::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals specialized to this factor
Definition PoseRotationPrior.h:66
gtsam::PoseRotationPrior::PoseRotationPrior
PoseRotationPrior()
default constructor - only use for serialization
Definition PoseRotationPrior.h:43
gtsam::PoseRotationPrior::evaluateError
Vector evaluateError(const Pose &pose, boost::optional< Matrix & > H=boost::none) const override
h(x)-z
Definition PoseRotationPrior.h:78
gtsam::PoseRotationPrior::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print contents
Definition PoseRotationPrior.h:72
gtsam::PoseRotationPrior::access
friend class boost::serialization::access
Serialization function.
Definition PoseRotationPrior.h:92
concepts.h
Concept-checking macros for geometric objects Each macro instantiates a concept check structure,...