22#include <gtsam/geometry/Unit3.h>
53 : Base(noiseModel, a, b), measured_w_aZb_(w_aZb.point3()) {}
68 boost::optional<Matrix&> H1 = boost::none,
69 boost::optional<Matrix&> H2 = boost::none)
const override {
70 const Point3 dir = Tb - Ta;
71 Matrix33 H_predicted_dir;
72 const Point3 predicted =
normalize(dir, H1 || H2 ? &H_predicted_dir :
nullptr);
73 if (H1) *H1 = -H_predicted_dir;
74 if (H2) *H2 = H_predicted_dir;
75 return predicted - measured_w_aZb_;
79 friend class boost::serialization::access;
80 template <
class ARCHIVE>
81 void serialize(ARCHIVE& ar,
const unsigned int ) {
82 ar& boost::serialization::make_nvp(
83 "Base", boost::serialization::base_object<Base>(*
this));
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
Point3 normalize(const Point3 &p, OptionalJacobian< 3, 3 > H)
normalize, with optional Jacobian
Definition Point3.cpp:52
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400
Binary factor for a relative translation direction measurement w_aZb.
Definition TranslationFactor.h:42
TranslationFactor()
default constructor
Definition TranslationFactor.h:49
Vector evaluateError(const Point3 &Ta, const Point3 &Tb, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Caclulate error: (norm(Tb - Ta) - measurement) where Tb and Ta are Point3 translations and measuremen...
Definition TranslationFactor.h:66