22#include <gtsam/nonlinear/ExpressionFactor.h>
24#include <boost/concept/assert.hpp>
32template <
typename A1,
typename A2,
33 typename B =
typename Bearing<A1, A2>::result_type,
34 typename R =
typename Range<A1, A2>::result_type>
43 typedef boost::shared_ptr<This> shared_ptr;
51 :
Base({{key1, key2}}, model, T(bearingRange)) {
52 this->initialize(expression({{key1, key2}}));
58 :
Base({{key1, key2}}, model, T(measuredBearing, measuredRange)) {
59 this->initialize(expression({{key1, key2}}));
62 ~BearingRangeFactor()
override {}
65 gtsam::NonlinearFactor::shared_ptr
clone()
const override {
66 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
67 gtsam::NonlinearFactor::shared_ptr(
new This(*
this)));
76 Vector evaluateError(
const A1& a1,
const A2& a2,
77 boost::optional<Matrix&> H1 = boost::none,
78 boost::optional<Matrix&> H2 = boost::none)
const
80 std::vector<Matrix> Hs(2);
90 void print(
const std::string& s =
"",
91 const KeyFormatter& kf = DefaultKeyFormatter)
const override {
92 std::cout << s <<
"BearingRangeFactor" << std::endl;
98 friend class boost::serialization::access;
99 template <
class ARCHIVE>
100 void serialize(ARCHIVE& ar,
const unsigned int ) {
101 ar& boost::serialization::make_nvp(
102 "Base", boost::serialization::base_object<Base>(*
this));
107template <
typename A1,
typename A2,
typename B,
typename R>
109 :
public Testable<BearingRangeFactor<A1, A2, B, R> > {};
Global functions in a separate testing namespace.
Definition chartTesting.h:28
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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
GenericValue< T > genericValue(const T &v)
Functional constructor of GenericValue<T> so T can be automatically deduced.
Definition GenericValue.h:212
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition Testable.h:151
Bearing-Range product for a particular A1,A2 combination will use the functors above to create a simi...
Definition BearingRange.h:51
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition Factor.h:140
virtual void print(const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition Factor.cpp:29
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
Error function without the NoiseModel, .
Definition ExpressionFactor.h:99
Expression class that supports automatic differentiation.
Definition Expression.h:48
N-ary variadic template for ExpressionFactor meant as a base class for N-ary factors.
Definition ExpressionFactor.h:242
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
Binary factor for a bearing/range measurement.
Definition BearingRangeFactor.h:36
BearingRangeFactor()
Default constructor.
Definition BearingRangeFactor.h:46
BearingRangeFactor(Key key1, Key key2, const B &measuredBearing, const R &measuredRange, const SharedNoiseModel &model)
Construct from separate bearing and range.
Definition BearingRangeFactor.h:56
BearingRangeFactor(Key key1, Key key2, const T &bearingRange, const SharedNoiseModel &model)
Construct from BearingRange instance.
Definition BearingRangeFactor.h:49
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition BearingRangeFactor.h:65
void print(const std::string &s="", const KeyFormatter &kf=DefaultKeyFormatter) const override
print
Definition BearingRangeFactor.h:90