gtsam  4.1.0
gtsam
BiasedGPSFactor.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 
16 #pragma once
17 
18 #include <ostream>
19 
20 #include <gtsam/geometry/Pose3.h>
22 
23 namespace gtsam {
24 
30  class BiasedGPSFactor: public NoiseModelFactor2<Pose3, Point3> {
31 
32  private:
33 
34  typedef BiasedGPSFactor This;
36 
37  Point3 measured_;
39  public:
40 
41  // shorthand for a smart pointer to a factor
42  typedef boost::shared_ptr<BiasedGPSFactor> shared_ptr;
43 
46 
48  BiasedGPSFactor(Key posekey, Key biaskey, const Point3 measured,
49  const SharedNoiseModel& model) :
50  Base(model, posekey, biaskey), measured_(measured) {
51  }
52 
53  virtual ~BiasedGPSFactor() {}
54 
58  void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
59  std::cout << s << "BiasedGPSFactor("
60  << keyFormatter(this->key1()) << ","
61  << keyFormatter(this->key2()) << ")\n"
62  << " measured: " << measured_.transpose() << std::endl;
63  this->noiseModel_->print(" noise model: ");
64  }
65 
67  bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
68  const This *e = dynamic_cast<const This*> (&expected);
69  return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(this->measured_, e->measured_, tol);
70  }
71 
75  Vector evaluateError(const Pose3& pose, const Point3& bias,
76  boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
77  boost::none) const override {
78 
79  if (H1 || H2){
80  H1->resize(3,6); // jacobian wrt pose
81  (*H1) << Matrix3::Zero(), pose.rotation().matrix();
82  H2->resize(3,3); // jacobian wrt bias
83  (*H2) << Matrix3::Identity();
84  }
85  return pose.translation() + bias - measured_;
86  }
87 
89  const Point3 measured() const {
90  return measured_;
91  }
92 
93  private:
94 
97  template<class ARCHIVE>
98  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
99  ar & boost::serialization::make_nvp("NoiseModelFactor2",
100  boost::serialization::base_object<Base>(*this));
101  ar & BOOST_SERIALIZATION_NVP(measured_);
102  }
103  }; // \class BiasedGPSFactor
104 
105 }
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition: Pose3.cpp:259
gtsam::Rot3::matrix
Matrix3 matrix() const
return 3*3 rotation matrix
Definition: Rot3M.cpp:219
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::NoiseModelFactor2< Pose3, Point3 >::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
gtsam::BiasedGPSFactor
Definition: BiasedGPSFactor.h:30
gtsam::BiasedGPSFactor::measured
const Point3 measured() const
return the measured
Definition: BiasedGPSFactor.h:89
Pose3.h
3D Pose
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::BiasedGPSFactor::equals
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
equals
Definition: BiasedGPSFactor.h:67
gtsam::BiasedGPSFactor::print
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
implement functions needed for Testable
Definition: BiasedGPSFactor.h:58
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::Pose3
Definition: Pose3.h:37
gtsam::BiasedGPSFactor::shared_ptr
boost::shared_ptr< BiasedGPSFactor > shared_ptr
The measurement.
Definition: BiasedGPSFactor.h:42
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::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
gtsam::BiasedGPSFactor::BiasedGPSFactor
BiasedGPSFactor()
default constructor - only use for serialization
Definition: BiasedGPSFactor.h:45
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::BiasedGPSFactor::access
friend class boost::serialization::access
Serialization function.
Definition: BiasedGPSFactor.h:96
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition: Pose3.cpp:266
gtsam::BiasedGPSFactor::BiasedGPSFactor
BiasedGPSFactor(Key posekey, Key biaskey, const Point3 measured, const SharedNoiseModel &model)
Constructor.
Definition: BiasedGPSFactor.h:48
gtsam::BiasedGPSFactor::evaluateError
Vector evaluateError(const Pose3 &pose, const Point3 &bias, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
implement functions needed to derive from Factor
Definition: BiasedGPSFactor.h:75