Go to the documentation of this file.
26 template <
typename A1,
typename A2>
34 template <
typename A1,
typename A2 = A1,
typename T =
double>
51 return std::static_pointer_cast<gtsam::NonlinearFactor>(
65 std::vector<Matrix> Hs(2);
76 void print(
const std::string&
s =
"",
78 std::cout <<
s <<
"RangeFactor" << std::endl;
83 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
84 friend class boost::serialization::access;
85 template <
class ARCHIVE>
86 void serialize(ARCHIVE& ar,
const unsigned int ) {
87 ar& boost::serialization::make_nvp(
88 "Base", boost::serialization::base_object<Base>(*
this));
94 template <
typename A1,
typename A2,
typename T>
96 :
public Testable<RangeFactor<A1, A2, T> > {};
102 template <
typename A1,
typename A2 =
A1,
117 const A1& body_T_sensor)
126 return std::static_pointer_cast<gtsam::NonlinearFactor>(
142 std::vector<Matrix> Hs(2);
161 std::cout <<
s <<
"RangeFactorWithTransform" << std::endl;
162 this->body_T_sensor_.print(
" sensor pose in body frame: ");
167 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
168 friend class boost::serialization::access;
170 template <
typename ARCHIVE>
171 void serialize(ARCHIVE& ar,
const unsigned int ) {
176 ar& boost::serialization::make_nvp(
177 "Base", boost::serialization::base_object<Base>(*
this));
183 template <
typename A1,
typename A2,
typename T>
185 :
public Testable<RangeFactorWithTransform<A1, A2, T> > {};
const Symbol key1('v', 1)
Vector evaluateError(const A1 &a1, const A2 &a2, OptionalMatrixType H1=OptionalNone, OptionalMatrixType H2=OptionalNone) const
std::shared_ptr< This > shared_ptr
RangeFactor< A1, A2 > This
Expression< T > expression(const typename Base::ArrayNKeys &keys) const override
GenericValue< T > genericValue(const T &v)
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
RangeFactor()
default constructor
const Symbol key2('v', 2)
RangeFactor(Key key1, Key key2, T measured, const SharedNoiseModel &model)
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
void initialize(const Expression< double > &expression)
Initialize with constructor arguments.
Expression< double > expression() const override
Return an expression that predicts the measurement given Values.
noiseModel::Base::shared_ptr SharedNoiseModel
std::array< Key, NARY_EXPRESSION_SIZE > ArrayNKeys
noiseModel::Diagonal::shared_ptr model
const double & measured() const
pair< size_t, size_t > Range
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
const KeyVector & keys() const
Access the factor's involved variable keys.
void print(const std::string &s="", const KeyFormatter &kf=DefaultKeyFormatter) const override
print
Matrix * OptionalMatrixType
gtsam::NonlinearFactor::shared_ptr clone() const override
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print relies on Testable traits being defined for T
ExpressionFactorN< T, A1, A2 > Base
std::uint64_t Key
Integer nonlinear key type.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:51