Loading...
Searching...
No Matches
ReferenceFrameFactor.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
* -------------------------------------------------------------------------- */
/*
* @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 {
template<class T, class P>
P transform_point(
const T& trans, const P& global,
boost::optional<Matrix&> Dtrans,
boost::optional<Matrix&> Dglobal) {
return trans.transformFrom(global, Dtrans, Dglobal);
}
template<class POINT, class TRANSFORM>
class ReferenceFrameFactor : public NoiseModelFactorN<POINT, TRANSFORM, POINT> {
protected:
ReferenceFrameFactor() {}
public:
typedef NoiseModelFactorN<POINT, TRANSFORM, POINT> Base;
typedef ReferenceFrameFactor<POINT, TRANSFORM> This;
typedef POINT Point;
typedef TRANSFORM Transform;
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr& model)
: Base(model,globalKey, transKey, localKey) {}
ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey)
: Base(globalKey, transKey, localKey, Point().dim(), mu) {}
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2)
: Base(noiseModel::Isotropic::Sigma(traits<POINT>::dimension, sigma),
globalKey, transKey, localKey) {}
~ReferenceFrameFactor() override{}
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); }
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 override {
Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
if (Dlocal)
*Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);
return traits<Point>::Local(local,newlocal);
}
void print(const std::string& s="",
const gtsam::KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
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:
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));
}
};
template<class T1, class T2>
struct traits<ReferenceFrameFactor<T1, T2> > : public Testable<ReferenceFrameFactor<T1, T2> > {};
} // \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::transform_point
P transform_point(const T &trans, const P &global, boost::optional< Matrix & > Dtrans, boost::optional< Matrix & > Dglobal)
Transform function that must be specialized specific domains.
Definition ReferenceFrameFactor.h:30
gtsam::trans
Matrix trans(const Matrix &A)
static transpose function, just calls Eigen transpose member function
Definition Matrix.h:269
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::noiseModel
All noise models live in the noiseModel namespace.
Definition LossFunctions.cpp:27
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::Testable
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
gtsam::NoiseModelFactor::dim
size_t dim() const override
get the dimension of the factor (number of rows on linearization)
Definition NonlinearFactor.h:218
gtsam::NoiseModelFactorN
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
gtsam::NoiseModelFactorN< POINT, TRANSFORM, POINT >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::ReferenceFrameFactor
A constraint between two landmarks in separate maps Templated on: Point : Type of landmark Transform ...
Definition ReferenceFrameFactor.h:57
gtsam::ReferenceFrameFactor::print
void print(const std::string &s="", const gtsam::KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition ReferenceFrameFactor.h:107
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, const noiseModel::Base::shared_ptr &model)
General constructor with arbitrary noise model (constrained or otherwise).
Definition ReferenceFrameFactor.h:72
gtsam::ReferenceFrameFactor::evaluateError
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 override
Combined cost and derivative function using boost::optional.
Definition ReferenceFrameFactor.h:97
gtsam::ReferenceFrameFactor::clone
NonlinearFactor::shared_ptr clone() const override
Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses.
Definition ReferenceFrameFactor.h:92
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma=1e-2)
Simple soft constraint constructor for frame of reference, with equal weighting for each degree of fr...
Definition ReferenceFrameFactor.h:86
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor(double mu, Key globalKey, Key transKey, Key localKey)
Construct a hard frame of reference reference constraint with equal mu values for each degree of free...
Definition ReferenceFrameFactor.h:79
gtsam::ReferenceFrameFactor::access
friend class boost::serialization::access
Serialization function.
Definition ReferenceFrameFactor.h:123
gtsam::ReferenceFrameFactor::ReferenceFrameFactor
ReferenceFrameFactor()
default constructor for serialization only
Definition ReferenceFrameFactor.h:60