gtsam 4.2.0
gtsam
GPSFactor.h
Go to the documentation of this file.
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
18#pragma once
19
23
24namespace gtsam {
25
35class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
36
37private:
38
40
41 Point3 nT_;
42
43public:
44
46 typedef boost::shared_ptr<GPSFactor> shared_ptr;
47
49 typedef GPSFactor This;
50
52 GPSFactor(): nT_(0, 0, 0) {}
53
54 ~GPSFactor() override {}
55
63 GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
64 Base(model, key), nT_(gpsIn) {
65 }
66
68 gtsam::NonlinearFactor::shared_ptr clone() const override {
69 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
70 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
71 }
72
74 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
75 DefaultKeyFormatter) const override;
76
78 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
79
81 Vector evaluateError(const Pose3& p,
82 boost::optional<Matrix&> H = boost::none) const override;
83
84 inline const Point3 & measurementIn() const {
85 return nT_;
86 }
87
93 static std::pair<Pose3, Vector3> EstimateState(double t1, const Point3& NED1,
94 double t2, const Point3& NED2, double timestamp);
95
96private:
97
99 friend class boost::serialization::access;
100 template<class ARCHIVE>
101 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
102 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
103 ar
104 & boost::serialization::make_nvp("NoiseModelFactor1",
105 boost::serialization::base_object<Base>(*this));
106 ar & BOOST_SERIALIZATION_NVP(nT_);
107 }
108};
109
114class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
115
116private:
117
119
120 Point3 nT_;
121
122public:
123
125 typedef boost::shared_ptr<GPSFactor2> shared_ptr;
126
129
131 GPSFactor2():nT_(0, 0, 0) {}
132
133 ~GPSFactor2() override {}
134
136 GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
137 Base(model, key), nT_(gpsIn) {
138 }
139
141 gtsam::NonlinearFactor::shared_ptr clone() const override {
142 return boost::static_pointer_cast<gtsam::NonlinearFactor>(
143 gtsam::NonlinearFactor::shared_ptr(new This(*this)));
144 }
145
147 void print(const std::string& s = "", const KeyFormatter& keyFormatter =
148 DefaultKeyFormatter) const override;
149
151 bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
152
154 Vector evaluateError(const NavState& p,
155 boost::optional<Matrix&> H = boost::none) const override;
156
157 inline const Point3 & measurementIn() const {
158 return nT_;
159 }
160
161private:
162
164 friend class boost::serialization::access;
165 template<class ARCHIVE>
166 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
167 // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
168 ar
169 & boost::serialization::make_nvp("NoiseModelFactor1",
170 boost::serialization::base_object<Base>(*this));
171 ar & BOOST_SERIALIZATION_NVP(nT_);
172 }
173};
174
175}
3D Pose
Navigation state composing of attitude, position, and velocity.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:156
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
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Definition: Key.h:35
Template to create a binary predicate.
Definition: Testable.h:111
A 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
Definition: Factor.h:68
Prior on position in a Cartesian frame.
Definition: GPSFactor.h:35
GPSFactor This
Typedef to this class.
Definition: GPSFactor.h:49
GPSFactor()
default constructor - only use for serialization
Definition: GPSFactor.h:52
GPSFactor(Key key, const Point3 &gpsIn, const SharedNoiseModel &model)
Constructor from a measurement in a Cartesian frame.
Definition: GPSFactor.h:63
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: GPSFactor.h:68
boost::shared_ptr< GPSFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: GPSFactor.h:46
Version of GPSFactor for NavState.
Definition: GPSFactor.h:114
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: GPSFactor.h:141
GPSFactor2 This
Typedef to this class.
Definition: GPSFactor.h:128
GPSFactor2(Key key, const Point3 &gpsIn, const SharedNoiseModel &model)
Constructor from a measurement in a Cartesian frame.
Definition: GPSFactor.h:136
boost::shared_ptr< GPSFactor2 > shared_ptr
shorthand for a smart pointer to a factor
Definition: GPSFactor.h:125
GPSFactor2()
default constructor - only use for serialization
Definition: GPSFactor.h:131
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition: NavState.h:34
Nonlinear factor base class.
Definition: NonlinearFactor.h:42
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition: NonlinearFactor.h:400