gtsam  4.1.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 
22 #include <gtsam/geometry/Pose3.h>
23 
24 namespace gtsam {
25 
35 class GTSAM_EXPORT GPSFactor: public NoiseModelFactor1<Pose3> {
36 
37 private:
38 
40 
41  Point3 nT_;
42 
43 public:
44 
46  typedef boost::shared_ptr<GPSFactor> shared_ptr;
47 
49  typedef GPSFactor This;
50 
52  GPSFactor(): nT_(0, 0, 0) {}
53 
54  virtual ~GPSFactor() {}
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 
96 private:
97 
99  friend class boost::serialization::access;
100  template<class ARCHIVE>
101  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
102  ar
103  & boost::serialization::make_nvp("NoiseModelFactor1",
104  boost::serialization::base_object<Base>(*this));
105  ar & BOOST_SERIALIZATION_NVP(nT_);
106  }
107 };
108 
113 class GTSAM_EXPORT GPSFactor2: public NoiseModelFactor1<NavState> {
114 
115 private:
116 
118 
119  Point3 nT_;
120 
121 public:
122 
124  typedef boost::shared_ptr<GPSFactor2> shared_ptr;
125 
127  typedef GPSFactor2 This;
128 
130  GPSFactor2():nT_(0, 0, 0) {}
131 
132  virtual ~GPSFactor2() {}
133 
135  GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
136  Base(model, key), nT_(gpsIn) {
137  }
138 
140  gtsam::NonlinearFactor::shared_ptr clone() const override {
141  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
142  gtsam::NonlinearFactor::shared_ptr(new This(*this)));
143  }
144 
146  void print(const std::string& s, const KeyFormatter& keyFormatter =
147  DefaultKeyFormatter) const override;
148 
150  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
151 
153  Vector evaluateError(const NavState& p,
154  boost::optional<Matrix&> H = boost::none) const override;
155 
156  inline const Point3 & measurementIn() const {
157  return nT_;
158  }
159 
160 private:
161 
163  friend class boost::serialization::access;
164  template<class ARCHIVE>
165  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
166  ar
167  & boost::serialization::make_nvp("NoiseModelFactor1",
168  boost::serialization::base_object<Base>(*this));
169  ar & BOOST_SERIALIZATION_NVP(nT_);
170  }
171 };
172 
173 }
gtsam::GPSFactor2::This
GPSFactor2 This
Typedef to this class.
Definition: GPSFactor.h:127
gtsam::equals
Template to create a binary predicate.
Definition: Testable.h:110
gtsam::GPSFactor::GPSFactor
GPSFactor(Key key, const Point3 &gpsIn, const SharedNoiseModel &model)
Constructor from a measurement in a Cartesian frame.
Definition: GPSFactor.h:63
gtsam::GPSFactor::This
GPSFactor This
Typedef to this class.
Definition: GPSFactor.h:49
gtsam::GPSFactor2::shared_ptr
boost::shared_ptr< GPSFactor2 > shared_ptr
shorthand for a smart pointer to a factor
Definition: GPSFactor.h:124
NavState.h
Navigation state composing of attitude, position, and velocity.
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition: NavState.h:34
gtsam::GPSFactor::GPSFactor
GPSFactor()
default constructor - only use for serialization
Definition: GPSFactor.h:52
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
Pose3.h
3D Pose
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition: Matrix.cpp:155
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::GPSFactor::shared_ptr
boost::shared_ptr< GPSFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: GPSFactor.h:46
gtsam::GPSFactor2
Definition: GPSFactor.h:113
gtsam::GPSFactor2::GPSFactor2
GPSFactor2(Key key, const Point3 &gpsIn, const SharedNoiseModel &model)
Constructor from a measurement in a Cartesian frame.
Definition: GPSFactor.h:135
gtsam::GPSFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: GPSFactor.h:140
gtsam::Pose3
Definition: Pose3.h:37
gtsam::KeyFormatter
boost::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
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::Point3
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:35
gtsam::GPSFactor
Definition: GPSFactor.h:35
gtsam::GPSFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: GPSFactor.h:68
gtsam::GPSFactor2::GPSFactor2
GPSFactor2()
default constructor - only use for serialization
Definition: GPSFactor.h:130