Loading...
Searching...
No Matches
BearingRangeFactor.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>
#include <gtsam/geometry/BearingRange.h>
#include <boost/concept/assert.hpp>
namespace gtsam {
template <typename A1, typename A2,
typename B = typename Bearing<A1, A2>::result_type,
typename R = typename Range<A1, A2>::result_type>
class BearingRangeFactor
: public ExpressionFactorN<BearingRange<A1, A2>, A1, A2> {
private:
typedef BearingRange<A1, A2> T;
typedef ExpressionFactorN<T, A1, A2> Base;
typedef BearingRangeFactor<A1, A2> This;
public:
typedef boost::shared_ptr<This> shared_ptr;
BearingRangeFactor() {}
BearingRangeFactor(Key key1, Key key2, const T &bearingRange,
const SharedNoiseModel &model)
: Base({{key1, key2}}, model, T(bearingRange)) {
this->initialize(expression({{key1, key2}}));
}
BearingRangeFactor(Key key1, Key key2, const B &measuredBearing,
const R &measuredRange, const SharedNoiseModel &model)
: Base({{key1, key2}}, model, T(measuredBearing, measuredRange)) {
this->initialize(expression({{key1, key2}}));
}
~BearingRangeFactor() override {}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
// Return measurement expression
Expression<T> expression(const typename Base::ArrayNKeys& keys) const override {
return Expression<T>(T::Measure, Expression<A1>(keys[0]),
Expression<A2>(keys[1]));
}
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 = this->unwhitenedError(
{{keys[0], genericValue(a1)}, {keys[1], genericValue(a2)}}, Hs);
if (H1) *H1 = Hs[0];
if (H2) *H2 = Hs[1];
return error;
}
void print(const std::string& s = "",
const KeyFormatter& kf = DefaultKeyFormatter) const override {
std::cout << s << "BearingRangeFactor" << std::endl;
Base::print(s, kf);
}
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));
}
}; // BearingRangeFactor
template <typename A1, typename A2, typename B, typename R>
struct traits<BearingRangeFactor<A1, A2, B, R> >
: public Testable<BearingRangeFactor<A1, A2, B, R> > {};
} // namespace gtsam
BearingRange.h
Bearing-Range product.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
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::Range
Definition BearingRange.h:40
gtsam::BearingRange
Bearing-Range product for a particular A1,A2 combination will use the functors above to create a simi...
Definition BearingRange.h:51
gtsam::BearingRange::Measure
static BearingRange Measure(const A1 &a1, const A2 &a2, OptionalJacobian< dimension, traits< A1 >::dimension > H1=boost::none, OptionalJacobian< dimension, traits< A2 >::dimension > H2=boost::none)
Prediction function that stacks measurements.
Definition BearingRange.h:78
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< BearingRange< A1, A2 > >::unwhitenedError
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Definition ExpressionFactor.h:99
gtsam::ExpressionFactor< BearingRange< A1, A2 > >::expression
virtual Expression< BearingRange< A1, A2 > > expression() const
Definition ExpressionFactor.h:193
gtsam::Expression
Expression class that supports automatic differentiation.
Definition Expression.h:48
gtsam::ExpressionFactorN< BearingRange< A1, A2 >, A1, A2 >::ExpressionFactorN
ExpressionFactorN()=default
gtsam::NoiseModelFactor::error
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
gtsam::BearingRangeFactor
Binary factor for a bearing/range measurement.
Definition BearingRangeFactor.h:36
gtsam::BearingRangeFactor::BearingRangeFactor
BearingRangeFactor()
Default constructor.
Definition BearingRangeFactor.h:46
gtsam::BearingRangeFactor::BearingRangeFactor
BearingRangeFactor(Key key1, Key key2, const B &measuredBearing, const R &measuredRange, const SharedNoiseModel &model)
Construct from separate bearing and range.
Definition BearingRangeFactor.h:56
gtsam::BearingRangeFactor::BearingRangeFactor
BearingRangeFactor(Key key1, Key key2, const T &bearingRange, const SharedNoiseModel &model)
Construct from BearingRange instance.
Definition BearingRangeFactor.h:49
gtsam::BearingRangeFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition BearingRangeFactor.h:65
gtsam::BearingRangeFactor::print
void print(const std::string &s="", const KeyFormatter &kf=DefaultKeyFormatter) const override
print
Definition BearingRangeFactor.h:90