134 lines
4.4 KiB
C++
134 lines
4.4 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* 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
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/*
|
|
* @file ReferenceFrameFactor.h
|
|
* @brief A constraint for combining graphs by common landmarks and a transform node
|
|
* @author Alex Cunningham
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/nonlinear/NonlinearFactor.h>
|
|
|
|
namespace gtsam {
|
|
|
|
/**
|
|
* Transform function that must be specialized specific domains
|
|
* @tparam T is a Transform type
|
|
* @tparam P is a point type
|
|
*/
|
|
template<class T, class P>
|
|
P transform_point(
|
|
const T& trans, const P& global,
|
|
boost::optional<Matrix&> Dtrans,
|
|
boost::optional<Matrix&> Dglobal) {
|
|
return trans.transform_from(global, Dtrans, Dglobal);
|
|
}
|
|
|
|
/**
|
|
* A constraint between two landmarks in separate maps
|
|
* Templated on:
|
|
* Point : Type of landmark
|
|
* Transform : Transform variable class
|
|
*
|
|
* The transform is defined as transforming global to local:
|
|
* l = lTg * g
|
|
*
|
|
* The Point and Transform concepts must be Lie types, and the transform
|
|
* relationship "Point = transform_from(Transform, Point)" must exist.
|
|
*
|
|
* To implement this function in new domains, specialize a new version of
|
|
* Point transform_point<Transform,Point>(transform, global, Dtrans, Dglobal)
|
|
* to use the correct point and transform types.
|
|
*
|
|
* This base class should be specialized to implement the cost function for
|
|
* specific classes of landmarks
|
|
*/
|
|
template<class POINT, class TRANSFORM>
|
|
class ReferenceFrameFactor : public NoiseModelFactor3<POINT, TRANSFORM, POINT> {
|
|
protected:
|
|
/** default constructor for serialization only */
|
|
ReferenceFrameFactor() {}
|
|
|
|
public:
|
|
typedef NoiseModelFactor3<POINT, TRANSFORM, POINT> Base;
|
|
typedef ReferenceFrameFactor<POINT, TRANSFORM> This;
|
|
|
|
typedef POINT Point;
|
|
typedef TRANSFORM Transform;
|
|
|
|
/**
|
|
* General constructor with arbitrary noise model (constrained or otherwise)
|
|
*/
|
|
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr& model)
|
|
: Base(model,globalKey, transKey, localKey) {}
|
|
|
|
/**
|
|
* Construct a hard frame of reference reference constraint with equal mu values for
|
|
* each degree of freedom.
|
|
*/
|
|
ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey)
|
|
: Base(globalKey, transKey, localKey, Point().dim(), mu) {}
|
|
|
|
/**
|
|
* Simple soft constraint constructor for frame of reference, with equal weighting for
|
|
* each degree of freedom.
|
|
*/
|
|
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2)
|
|
: Base(noiseModel::Isotropic::Sigma(POINT().dim(), sigma),
|
|
globalKey, transKey, localKey) {}
|
|
|
|
virtual ~ReferenceFrameFactor(){}
|
|
|
|
virtual NonlinearFactor::shared_ptr clone() const {
|
|
return boost::static_pointer_cast<NonlinearFactor>(
|
|
NonlinearFactor::shared_ptr(new This(*this))); }
|
|
|
|
/** Combined cost and derivative function using boost::optional */
|
|
virtual Vector evaluateError(const Point& global, const Transform& trans, const Point& local,
|
|
boost::optional<Matrix&> Dforeign = boost::none,
|
|
boost::optional<Matrix&> Dtrans = boost::none,
|
|
boost::optional<Matrix&> Dlocal = boost::none) const {
|
|
Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
|
|
if (Dlocal) {
|
|
Point dummy;
|
|
*Dlocal = -1* gtsam::eye(dummy.dim());
|
|
}
|
|
return local.localCoordinates(newlocal);
|
|
}
|
|
|
|
virtual void print(const std::string& s="",
|
|
const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
|
std::cout << s << ": ReferenceFrameFactor("
|
|
<< "Global: " << keyFormatter(this->key1()) << ","
|
|
<< " Transform: " << keyFormatter(this->key2()) << ","
|
|
<< " Local: " << keyFormatter(this->key3()) << ")\n";
|
|
this->noiseModel_->print(" noise model");
|
|
}
|
|
|
|
// access - convenience functions
|
|
Key global_key() const { return this->key1(); }
|
|
Key transform_key() const { return this->key2(); }
|
|
Key local_key() const { return this->key3(); }
|
|
|
|
private:
|
|
/** Serialization function */
|
|
friend class boost::serialization::access;
|
|
template<class ARCHIVE>
|
|
void serialize(ARCHIVE & ar, const unsigned int version) {
|
|
ar & boost::serialization::make_nvp("NonlinearFactor3",
|
|
boost::serialization::base_object<Base>(*this));
|
|
}
|
|
};
|
|
|
|
} // \namespace gtsam
|