Loading...
Searching...
No Matches
GPSFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/geometry/Pose3.h>
namespace gtsam {
class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
private:
typedef NoiseModelFactorN<Pose3> Base;
Point3 nT_;
public:
typedef boost::shared_ptr<GPSFactor> shared_ptr;
typedef GPSFactor This;
GPSFactor(): nT_(0, 0, 0) {}
~GPSFactor() override {}
GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
Base(model, key), nT_(gpsIn) {
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
Vector evaluateError(const Pose3& p,
boost::optional<Matrix&> H = boost::none) const override;
inline const Point3 & measurementIn() const {
return nT_;
}
static std::pair<Pose3, Vector3> EstimateState(double t1, const Point3& NED1,
double t2, const Point3& NED2, double timestamp);
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar
& boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(nT_);
}
};
class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
private:
typedef NoiseModelFactorN<NavState> Base;
Point3 nT_;
public:
typedef boost::shared_ptr<GPSFactor2> shared_ptr;
typedef GPSFactor2 This;
GPSFactor2():nT_(0, 0, 0) {}
~GPSFactor2() override {}
GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
Base(model, key), nT_(gpsIn) {
}
gtsam::NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;
Vector evaluateError(const NavState& p,
boost::optional<Matrix&> H = boost::none) const override;
inline const Point3 & measurementIn() const {
return nT_;
}
private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
// NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility
ar
& boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(nT_);
}
};
}
Pose3.h
3D Pose
NavState.h
Navigation state composing of attitude, position, and velocity.
NonlinearFactor.h
Non-linear factor base classes.
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:156
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::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:36
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::GPSFactor
Prior on position in a Cartesian frame.
Definition GPSFactor.h:35
gtsam::GPSFactor::This
GPSFactor This
Typedef to this class.
Definition GPSFactor.h:49
gtsam::GPSFactor::GPSFactor
GPSFactor()
default constructor - only use for serialization
Definition GPSFactor.h:52
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::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GPSFactor.h:68
gtsam::GPSFactor::shared_ptr
boost::shared_ptr< GPSFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition GPSFactor.h:46
gtsam::GPSFactor2
Version of GPSFactor for NavState.
Definition GPSFactor.h:114
gtsam::GPSFactor2::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition GPSFactor.h:141
gtsam::GPSFactor2::This
GPSFactor2 This
Typedef to this class.
Definition GPSFactor.h:128
gtsam::GPSFactor2::GPSFactor2
GPSFactor2(Key key, const Point3 &gpsIn, const SharedNoiseModel &model)
Constructor from a measurement in a Cartesian frame.
Definition GPSFactor.h:136
gtsam::GPSFactor2::shared_ptr
boost::shared_ptr< GPSFactor2 > shared_ptr
shorthand for a smart pointer to a factor
Definition GPSFactor.h:125
gtsam::GPSFactor2::GPSFactor2
GPSFactor2()
default constructor - only use for serialization
Definition GPSFactor.h:131
gtsam::NavState
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition NonlinearFactor.h:42
gtsam::NoiseModelFactorN< Pose3 >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::NoiseModelFactorN< Pose3 >::key
Key key() const
Definition NonlinearFactor.h:518