120 lines
3.3 KiB
C++
120 lines
3.3 KiB
C++
/**
|
|
* @file PoseTranslationPrior.h
|
|
*
|
|
* @brief Implements a prior on the translation component of a pose
|
|
*
|
|
* @date Jun 14, 2012
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/geometry/concepts.h>
|
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/** simple pose traits for building factors */
|
|
namespace pose_traits {
|
|
|
|
/** checks whether parameterization of rotation is before or after translation in Lie algebra */
|
|
template<class POSE>
|
|
bool isRotFirst() {
|
|
throw std::invalid_argument("PoseTrait: no implementation for this pose type");
|
|
return false;
|
|
}
|
|
|
|
// Instantiate for common poses
|
|
template<> inline bool isRotFirst<Pose3>() { return true; }
|
|
template<> inline bool isRotFirst<Pose2>() { return false; }
|
|
|
|
} // \namespace pose_traits
|
|
|
|
/**
|
|
* A prior on the translation part of a pose
|
|
*/
|
|
template<class POSE>
|
|
class PoseTranslationPrior : public NoiseModelFactor1<POSE> {
|
|
public:
|
|
typedef PoseTranslationPrior<POSE> This;
|
|
typedef NoiseModelFactor1<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(Translation)
|
|
|
|
protected:
|
|
|
|
Translation measured_;
|
|
|
|
public:
|
|
|
|
/** standard constructor */
|
|
PoseTranslationPrior(Key key, const Translation& measured, const noiseModel::Base::shared_ptr& model)
|
|
: Base(model, key), measured_(measured) {
|
|
}
|
|
|
|
/** Constructor that pulls the translation from an incoming POSE */
|
|
PoseTranslationPrior(Key key, const POSE& pose_z, const noiseModel::Base::shared_ptr& model)
|
|
: Base(model, key), measured_(pose_z.translation()) {
|
|
}
|
|
|
|
virtual ~PoseTranslationPrior() {}
|
|
|
|
const Translation& measured() const { return measured_; }
|
|
|
|
/// @return a deep copy of this factor
|
|
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
|
|
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
|
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
|
|
|
|
/** h(x)-z */
|
|
Vector evaluateError(const Pose& pose, boost::optional<Matrix&> H = boost::none) const {
|
|
const Translation& newTrans = pose.translation();
|
|
const Rotation& R = pose.rotation();
|
|
const size_t tDim = newTrans.dim(), xDim = pose.dim();
|
|
if (H) {
|
|
*H = gtsam::zeros(tDim, xDim);
|
|
if (pose_traits::isRotFirst<Pose>())
|
|
(*H).rightCols(tDim) = R.matrix();
|
|
else
|
|
(*H).leftCols(tDim) = R.matrix();
|
|
}
|
|
|
|
return newTrans.vector() - measured_.vector();
|
|
}
|
|
|
|
/** equals specialized to this factor */
|
|
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
|
|
const This *e = dynamic_cast<const This*> (&expected);
|
|
return e != NULL && Base::equals(*e, tol) && measured_.equals(e->measured_, tol);
|
|
}
|
|
|
|
/** print contents */
|
|
void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
|
Base::print(s + "PoseTranslationPrior", keyFormatter);
|
|
measured_.print("Measured Translation");
|
|
}
|
|
|
|
private:
|
|
|
|
/** Serialization function */
|
|
friend class boost::serialization::access;
|
|
template<class ARCHIVE>
|
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
ar & boost::serialization::make_nvp("NoiseModelFactor1",
|
|
boost::serialization::base_object<Base>(*this));
|
|
ar & BOOST_SERIALIZATION_NVP(measured_);
|
|
}
|
|
|
|
};
|
|
|
|
} // \namespace gtsam
|
|
|
|
|
|
|
|
|