44 Base(model, i, j), measured_(measured) {
49 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
50 boost::none)
const override {
73 Base(model, b1, i, b2, j), measured_(measured) {
79 boost::optional<Matrix&> H1 = boost::none,
80 boost::optional<Matrix&> H2 = boost::none,
81 boost::optional<Matrix&> H3 = boost::none,
82 boost::optional<Matrix&> H4 = boost::none)
const override {
83 if (H1 || H2 || H3 || H4) {
85 Matrix D_pose_g_base1, D_pose_g_pose;
86 Pose2 pose_g = base1.
compose(pose, D_pose_g_base1, D_pose_g_pose);
87 Matrix D_point_g_base2, D_point_g_point;
90 Matrix D_e_pose_g, D_e_point_g;
93 *H1 = D_e_pose_g * D_pose_g_base1;
95 *H2 = D_e_pose_g * D_pose_g_pose;
97 *H3 = D_e_point_g * D_point_g_base2;
99 *H4 = D_e_point_g * D_point_g_point;
128 Base(model, b1, i, b2, j), measured_(measured) {
134 boost::optional<Matrix&> H1 = boost::none,
135 boost::optional<Matrix&> H2 = boost::none,
136 boost::optional<Matrix&> H3 = boost::none,
137 boost::optional<Matrix&> H4 = boost::none)
const override {
138 if (H1 || H2 || H3 || H4) {
140 Matrix D_pose1_g_base1, D_pose1_g_pose1;
141 Pose2 pose1_g = base1.
compose(pose1, D_pose1_g_base1, D_pose1_g_pose1);
142 Matrix D_pose2_g_base2, D_pose2_g_pose2;
143 Pose2 pose2_g = base2.
compose(pose2, D_pose2_g_base2, D_pose2_g_pose2);
144 Matrix D_e_pose1_g, D_e_pose2_g;
145 Pose2 d = pose1_g.
between(pose2_g, D_e_pose1_g, D_e_pose2_g);
147 *H1 = D_e_pose1_g * D_pose1_g_base1;
149 *H2 = D_e_pose1_g * D_pose1_g_pose1;
151 *H3 = D_e_pose2_g * D_pose2_g_base2;
153 *H4 = D_e_pose2_g * D_pose2_g_pose2;
NoiseModelFactor2< Pose2, Point2 > Base
NoiseModelFactor4< Pose2, Pose2, Pose2, Pose2 > Base
Vector evaluateError(const Pose2 &base1, const Pose2 &pose, const Pose2 &base2, const Point2 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none) const override
Evaluate measurement error h(x)-z.
noiseModel::Diagonal::shared_ptr model
boost::shared_ptr< This > shared_ptr
Point2 measured_
the measurement
Class compose(const Class &g) const
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
Vector evaluateError(const Pose2 &pose, const Point2 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Evaluate measurement error h(x)-z.
NoiseModelFactor4< Pose2, Pose2, Pose2, Point2 > Base
boost::shared_ptr< This > shared_ptr
Vector evaluateError(const Pose2 &base1, const Pose2 &pose1, const Pose2 &base2, const Pose2 &pose2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none) const override
Evaluate measurement error h(x)-z.
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
OdometryFactorBase(Key b1, Key i, Key b2, Key j, const Pose2 &measured, const SharedNoiseModel &model)
Constructor.
Non-linear factor base classes.
DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2 &measured, const SharedNoiseModel &model)
Constructor.
Class between(const Class &g) const
DeltaFactor(Key i, Key j, const Point2 &measured, const SharedNoiseModel &model)
Constructor.
std::uint64_t Key
Integer nonlinear key type.
Pose2 measured_
the measurement
noiseModel::Base::shared_ptr SharedNoiseModel
GTSAM_EXPORT Point2 transformTo(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
boost::shared_ptr< This > shared_ptr
Point2 measured_
the measurement
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...