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 NoiseModelFactorN<Pose3> {
36 
37 private:
38 
40 
42 
43 public:
44 
45  // Provide access to the Matrix& version of evaluateError:
46  using Base::evaluateError;
47 
49  typedef std::shared_ptr<GPSFactor> shared_ptr;
50 
52  typedef GPSFactor This;
53 
55  GPSFactor(): nT_(0, 0, 0) {}
56 
57  ~GPSFactor() override {}
58 
66  GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
67  Base(model, key), nT_(gpsIn) {
68  }
69 
72  return std::static_pointer_cast<gtsam::NonlinearFactor>(
74  }
75 
77  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
78  DefaultKeyFormatter) const override;
79 
81  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
82 
84  Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;
85 
86  inline const Point3 & measurementIn() const {
87  return nT_;
88  }
89 
95  static std::pair<Pose3, Vector3> EstimateState(double t1, const Point3& NED1,
96  double t2, const Point3& NED2, double timestamp);
97 
98 private:
99 
100 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
101  friend class boost::serialization::access;
103  template<class ARCHIVE>
104  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
105  // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
106  ar
107  & boost::serialization::make_nvp("NoiseModelFactor1",
108  boost::serialization::base_object<Base>(*this));
109  ar & BOOST_SERIALIZATION_NVP(nT_);
110  }
111 #endif
112 };
113 
118 class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
119 
120 private:
121 
123 
125 
126 public:
127 
128  // Provide access to the Matrix& version of evaluateError:
129  using Base::evaluateError;
130 
132  typedef std::shared_ptr<GPSFactor2> shared_ptr;
133 
135  typedef GPSFactor2 This;
136 
138  GPSFactor2():nT_(0, 0, 0) {}
139 
140  ~GPSFactor2() override {}
141 
143  GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
144  Base(model, key), nT_(gpsIn) {
145  }
146 
149  return std::static_pointer_cast<gtsam::NonlinearFactor>(
151  }
152 
154  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
155  DefaultKeyFormatter) const override;
156 
158  bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
159 
161  Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;
162 
163  inline const Point3 & measurementIn() const {
164  return nT_;
165  }
166 
167 private:
168 
169 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
170  friend class boost::serialization::access;
172  template<class ARCHIVE>
173  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
174  // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
175  ar
176  & boost::serialization::make_nvp("NoiseModelFactor1",
177  boost::serialization::base_object<Base>(*this));
178  ar & BOOST_SERIALIZATION_NVP(nT_);
179  }
180 #endif
181 };
182 
183 }
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
gtsam::GPSFactor2::measurementIn
const Point3 & measurementIn() const
Definition: GPSFactor.h:163
gtsam::GPSFactor::This
GPSFactor This
Typedef to this class.
Definition: GPSFactor.h:52
gtsam::GPSFactor2::Base
NoiseModelFactorN< NavState > Base
Definition: GPSFactor.h:122
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::GPSFactor::shared_ptr
std::shared_ptr< GPSFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: GPSFactor.h:49
gtsam::GPSFactor2::nT_
Point3 nT_
Position measurement in cartesian coordinates.
Definition: GPSFactor.h:124
gtsam::GPSFactor::GPSFactor
GPSFactor(Key key, const Point3 &gpsIn, const SharedNoiseModel &model)
Constructor from a measurement in a Cartesian frame. Use GeographicLib to convert from geographic (la...
Definition: GPSFactor.h:66
gtsam::NavState
Definition: NavState.h:34
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::GPSFactor2::GPSFactor2
GPSFactor2()
default constructor - only use for serialization
Definition: GPSFactor.h:138
gtsam::GPSFactor2::~GPSFactor2
~GPSFactor2() override
Definition: GPSFactor.h:140
gtsam::GPSFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: GPSFactor.h:71
gtsam::GPSFactor2::shared_ptr
std::shared_ptr< GPSFactor2 > shared_ptr
shorthand for a smart pointer to a factor
Definition: GPSFactor.h:132
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::GPSFactor::nT_
Point3 nT_
Position measurement in cartesian coordinates.
Definition: GPSFactor.h:41
gtsam::KeyFormatter
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
gtsam::Pose3
Definition: Pose3.h:37
NavState.h
Navigation state composing of attitude, position, and velocity.
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
cholesky::expected
Matrix expected
Definition: testMatrix.cpp:971
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::GPSFactor::Base
NoiseModelFactorN< Pose3 > Base
Definition: GPSFactor.h:39
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::equals
Definition: Testable.h:112
key
const gtsam::Symbol key('X', 0)
gtsam::GPSFactor::~GPSFactor
~GPSFactor() override
Definition: GPSFactor.h:57
NonlinearFactor.h
Non-linear factor base classes.
gtsam::GPSFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: GPSFactor.h:148
gtsam
traits
Definition: SFMdata.h:40
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::GPSFactor2::GPSFactor2
GPSFactor2(Key key, const Point3 &gpsIn, const SharedNoiseModel &model)
Constructor from a measurement in a Cartesian frame.
Definition: GPSFactor.h:143
This
#define This
Definition: ActiveSetSolver-inl.h:27
gtsam::GPSFactor::GPSFactor
GPSFactor()
Definition: GPSFactor.h:55
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::GPSFactor2
Definition: GPSFactor.h:118
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::GPSFactor::measurementIn
const Point3 & measurementIn() const
Definition: GPSFactor.h:86
gtsam::GPSFactor
Definition: GPSFactor.h:35
Pose3.h
3D Pose
gtsam::GPSFactor2::This
GPSFactor2 This
Typedef to this class.
Definition: GPSFactor.h:135


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:40