Go to the documentation of this file.
48 using NoiseModelFactor2<Point3, Point3>::evaluateError;
71 const Point3 dir = Tb - Ta;
72 Matrix33 H_predicted_dir;
74 normalize(dir, H1 || H2 ? &H_predicted_dir :
nullptr);
75 if (H1) *H1 = -H_predicted_dir;
76 if (H2) *H2 = H_predicted_dir;
81 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
82 friend class boost::serialization::access;
83 template <
class ARCHIVE>
84 void serialize(ARCHIVE& ar,
const unsigned int ) {
85 ar& boost::serialization::make_nvp(
86 "Base", boost::serialization::base_object<Base>(*
this));
120 using NoiseModelFactor2<Point3, Point3, Vector1>::evaluateError;
137 const double abs_scale =
abs(
scale[0]);
138 const Point3 predicted = (Tb - Ta) * abs_scale;
140 *H1 = -Matrix3::Identity() * abs_scale;
143 *H2 = Matrix3::Identity() * abs_scale;
146 *H3 =
scale[0] >= 0 ? (Tb - Ta) : (Ta - Tb);
152 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
153 friend class boost::serialization::access;
154 template <
class ARCHIVE>
155 void serialize(ARCHIVE& ar,
const unsigned int ) {
156 ar& boost::serialization::make_nvp(
157 "Base", boost::serialization::base_object<Base>(*
this));
NoiseModelFactorN< Point3, Point3 > Base
typedef and functions to augment Eigen's VectorXd
Vector evaluateError(const Point3 &Ta, const Point3 &Tb, OptionalMatrixType H1, OptionalMatrixType H2) const override
Calculate error: (norm(Tb - Ta) - measurement) where Tb and Ta are Point3 translations and measuremen...
Vector evaluateError(const Point3 &Ta, const Point3 &Tb, const Vector1 &scale, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
Calculate error: (scale * (Tb - Ta) - measurement) where Tb and Ta are Point3 translations and measur...
Point3_ point3(const Unit3_ &v)
NoiseModelFactorN< Point3, Point3, Vector1 > Base
BilinearAngleTranslationFactor()
default constructor
static void normalize(Signature::Row &row)
const SharedNoiseModel & noiseModel() const
access to the noise model
TranslationFactor()
default constructor
BilinearAngleTranslationFactor(Key a, Key b, Key scale_key, const Unit3 &w_aZb, const SharedNoiseModel &noiseModel)
noiseModel::Base::shared_ptr SharedNoiseModel
Non-linear factor base classes.
TranslationFactor(Key a, Key b, const Unit3 &w_aZb, const SharedNoiseModel &noiseModel)
static double scale(double x, double a, double b, double t1, double t2)
Scale x from [a, b] to [t1, t2].
Matrix * OptionalMatrixType
The matrix class, also used for vectors and row-vectors.
Represents a 3D point on a unit sphere.
std::uint64_t Key
Integer nonlinear key type.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:32