1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file TSAMFactors.h
14 * @brief TSAM 1 Factors, simpler than the hierarchical TSAM 2
15 * @author Frank Dellaert
16 * @date May 2014
17 */
18
19#pragma once
20
21#include <gtsam/geometry/Pose2.h>
22#include <gtsam/nonlinear/NonlinearFactor.h>
23
24namespace gtsam {
25
26/**
27 * DeltaFactor: relative 2D measurement between Pose2 and Point2
28 */
29class DeltaFactor: public NoiseModelFactorN<Pose2, Point2> {
30
31public:
32 typedef DeltaFactor This;
33 typedef NoiseModelFactorN<Pose2, Point2> Base;
34 typedef std::shared_ptr<This> shared_ptr;
35
36private:
37 Point2 measured_; ///< the measurement
38
39public:
40
41 // Provide access to the Matrix& version of evaluateError:
42 using Base::evaluateError;
43
44 /// Constructor
45 DeltaFactor(Key i, Key j, const Point2& measured,
46 const SharedNoiseModel& model) :
47 Base(model, i, j), measured_(measured) {
48 }
49
50 /// Evaluate measurement error h(x)-z
51 Vector evaluateError(const Pose2& pose, const Point2& point,
52 OptionalMatrixType H1, OptionalMatrixType H2) const override {
53 return pose.transformTo(point, Dpose: H1, Dpoint: H2) - measured_;
54 }
55};
56
57/**
58 * DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes
59 */
60class DeltaFactorBase: public NoiseModelFactorN<Pose2, Pose2, Pose2, Point2> {
61
62public:
63 typedef DeltaFactorBase This;
64 typedef NoiseModelFactorN<Pose2, Pose2, Pose2, Point2> Base;
65 typedef std::shared_ptr<This> shared_ptr;
66
67private:
68 Point2 measured_; ///< the measurement
69
70public:
71
72 // Provide access to the Matrix& version of evaluateError:
73 using Base::evaluateError;
74
75 /// Constructor
76 DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2& measured,
77 const SharedNoiseModel& model) :
78 Base(model, b1, i, b2, j), measured_(measured) {
79 }
80
81 /// Evaluate measurement error h(x)-z
82 Vector evaluateError(const Pose2& base1, const Pose2& pose,
83 const Pose2& base2, const Point2& point, //
84 OptionalMatrixType H1, OptionalMatrixType H2, //
85 OptionalMatrixType H3, OptionalMatrixType H4) const override {
86 if (H1 || H2 || H3 || H4) {
87 // TODO use fixed-size matrices
88 Matrix D_pose_g_base1, D_pose_g_pose;
89 Pose2 pose_g = base1.compose(g: pose, H1: D_pose_g_base1, H2: D_pose_g_pose);
90 Matrix D_point_g_base2, D_point_g_point;
91 Point2 point_g = base2.transformFrom(point, Dpose: D_point_g_base2,
92 Dpoint: D_point_g_point);
93 Matrix D_e_pose_g, D_e_point_g;
94 Point2 d = pose_g.transformTo(point: point_g, Dpose: D_e_pose_g, Dpoint: D_e_point_g);
95 if (H1)
96 *H1 = D_e_pose_g * D_pose_g_base1;
97 if (H2)
98 *H2 = D_e_pose_g * D_pose_g_pose;
99 if (H3)
100 *H3 = D_e_point_g * D_point_g_base2;
101 if (H4)
102 *H4 = D_e_point_g * D_point_g_point;
103 return d - measured_;
104 } else {
105 Pose2 pose_g = base1.compose(g: pose);
106 Point2 point_g = base2.transformFrom(point);
107 Point2 d = pose_g.transformTo(point: point_g);
108 return d - measured_;
109 }
110 }
111};
112
113/**
114 * OdometryFactorBase: Pose2 odometry, with Basenodes
115 */
116class OdometryFactorBase: public NoiseModelFactorN<Pose2, Pose2, Pose2, Pose2> {
117
118public:
119 typedef OdometryFactorBase This;
120 typedef NoiseModelFactorN<Pose2, Pose2, Pose2, Pose2> Base;
121 typedef std::shared_ptr<This> shared_ptr;
122
123private:
124 Pose2 measured_; ///< the measurement
125
126public:
127
128 // Provide access to the Matrix& version of evaluateError:
129 using Base::evaluateError;
130
131 /// Constructor
132 OdometryFactorBase(Key b1, Key i, Key b2, Key j, const Pose2& measured,
133 const SharedNoiseModel& model) :
134 Base(model, b1, i, b2, j), measured_(measured) {
135 }
136
137 /// Evaluate measurement error h(x)-z
138 Vector evaluateError(const Pose2& base1, const Pose2& pose1,
139 const Pose2& base2, const Pose2& pose2,
140 OptionalMatrixType H1, OptionalMatrixType H2,
141 OptionalMatrixType H3, OptionalMatrixType H4) const override {
142 if (H1 || H2 || H3 || H4) {
143 // TODO use fixed-size matrices
144 Matrix D_pose1_g_base1, D_pose1_g_pose1;
145 Pose2 pose1_g = base1.compose(g: pose1, H1: D_pose1_g_base1, H2: D_pose1_g_pose1);
146 Matrix D_pose2_g_base2, D_pose2_g_pose2;
147 Pose2 pose2_g = base2.compose(g: pose2, H1: D_pose2_g_base2, H2: D_pose2_g_pose2);
148 Matrix D_e_pose1_g, D_e_pose2_g;
149 Pose2 d = pose1_g.between(g: pose2_g, H1: D_e_pose1_g, H2: D_e_pose2_g);
150 if (H1)
151 *H1 = D_e_pose1_g * D_pose1_g_base1;
152 if (H2)
153 *H2 = D_e_pose1_g * D_pose1_g_pose1;
154 if (H3)
155 *H3 = D_e_pose2_g * D_pose2_g_base2;
156 if (H4)
157 *H4 = D_e_pose2_g * D_pose2_g_pose2;
158 return measured_.localCoordinates(g: d);
159 } else {
160 Pose2 pose1_g = base1.compose(g: pose1);
161 Pose2 pose2_g = base2.compose(g: pose2);
162 Pose2 d = pose1_g.between(g: pose2_g);
163 return measured_.localCoordinates(g: d);
164 }
165 }
166};
167
168}
169
170