Loading...
Searching...
No Matches
BearingFactor.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/ExpressionFactor.h>
namespace gtsam {
// forward declaration of Bearing functor, assumed partially specified
template <typename A1, typename A2>
struct Bearing;
template <typename A1, typename A2,
typename T = typename Bearing<A1, A2>::result_type>
struct BearingFactor : public ExpressionFactorN<T, A1, A2> {
typedef ExpressionFactorN<T, A1, A2> Base;
BearingFactor() {}
BearingFactor(Key key1, Key key2, const T& measured,
const SharedNoiseModel& model)
: Base({key1, key2}, model, measured) {
this->initialize(expression({key1, key2}));
}
// Return measurement expression
Expression<T> expression(const typename Base::ArrayNKeys &keys) const override {
Expression<A1> a1_(keys[0]);
Expression<A2> a2_(keys[1]);
return Expression<T>(Bearing<A1, A2>(), a1_, a2_);
}
void print(const std::string& s = "",
const KeyFormatter& kf = DefaultKeyFormatter) const override {
std::cout << s << "BearingFactor" << std::endl;
Base::print(s, kf);
}
Vector evaluateError(const A1& a1, const A2& a2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const
{
std::vector<Matrix> Hs(2);
const auto &keys = Factor::keys();
const Vector error = unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}},
Hs);
if (H1) *H1 = Hs[0];
if (H2) *H2 = Hs[1];
return error;
}
private:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Base", boost::serialization::base_object<Base>(*this));
}
}; // BearingFactor
template <typename A1, typename A2, typename T>
struct traits<BearingFactor<A1, A2, T> >
: public Testable<BearingFactor<A1, A2, T> > {};
} // namespace gtsam
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::genericValue
GenericValue< T > genericValue(const T &v)
Functional constructor of GenericValue<T> so T can be automatically deduced.
Definition GenericValue.h:212
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::Bearing
Definition BearingRange.h:34
gtsam::Factor::keys
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
gtsam::Factor::error
virtual double error(const HybridValues &c) const
All factor types need to implement an error function.
Definition Factor.cpp:47
gtsam::Factor::print
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
gtsam::ExpressionFactor::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Error function without the NoiseModel, .
Definition ExpressionFactor.h:99
gtsam::ExpressionFactor::expression
virtual Expression< T > expression() const
Recreate expression from keys_ and measured_, used in load below.
Definition ExpressionFactor.h:193
gtsam::ExpressionFactor::measured
const T & measured() const
return the measurement
Definition ExpressionFactor.h:77
gtsam::ExpressionFactorN::ExpressionFactorN
ExpressionFactorN()=default
Default constructor, for serialization.
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
gtsam::BearingFactor
Binary factor for a bearing measurement Works for any two types A1,A2 for which the functor Bearing<A...
Definition BearingFactor.h:37
gtsam::BearingFactor::print
void print(const std::string &s="", const KeyFormatter &kf=DefaultKeyFormatter) const override
print
Definition BearingFactor.h:58
gtsam::BearingFactor::BearingFactor
BearingFactor(Key key1, Key key2, const T &measured, const SharedNoiseModel &model)
primary constructor
Definition BearingFactor.h:44
gtsam::BearingFactor::BearingFactor
BearingFactor()
default constructor
Definition BearingFactor.h:41