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
12/**
13 * @file BiasedGPSFactor.h
14 * @author Luca Carlone
15 **/
16#pragma once
17
18#include <ostream>
19
20#include <gtsam/geometry/Pose3.h>
21#include <gtsam/nonlinear/NonlinearFactor.h>
22
23namespace gtsam {
24
25 /**
26 * A class to model GPS measurements, including a bias term which models
27 * common-mode errors and that can be partially corrected if other sensors are used
28 * @ingroup slam
29 */
30 class BiasedGPSFactor: public NoiseModelFactorN<Pose3, Point3> {
31
32 private:
33
34 typedef BiasedGPSFactor This;
35 typedef NoiseModelFactorN<Pose3, Point3> Base;
36
37 Point3 measured_; /** The measurement */
38
39 public:
40
41 // Provide access to the Matrix& version of evaluateError:
42 using Base::evaluateError;
43
44 // shorthand for a smart pointer to a factor
45 typedef std::shared_ptr<BiasedGPSFactor> shared_ptr;
46
47 /** default constructor - only use for serialization */
48 BiasedGPSFactor() {}
49
50 /** Constructor */
51 BiasedGPSFactor(Key posekey, Key biaskey, const Point3 measured,
52 const SharedNoiseModel& model) :
53 Base(model, posekey, biaskey), measured_(measured) {
54 }
55
56 ~BiasedGPSFactor() override {}
57
58 /** implement functions needed for Testable */
59
60 /** print */
61 void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
62 std::cout << s << "BiasedGPSFactor("
63 << keyFormatter(this->key<1>()) << ","
64 << keyFormatter(this->key<2>()) << ")\n"
65 << " measured: " << measured_.transpose() << std::endl;
66 this->noiseModel_->print(name: " noise model: ");
67 }
68
69 /** equals */
70 bool equals(const NonlinearFactor& expected, double tol=1e-9) const override {
71 const This *e = dynamic_cast<const This*> (&expected);
72 return e != nullptr && Base::equals(f: *e, tol) && traits<Point3>::Equals(v1: this->measured_, v2: e->measured_, tol);
73 }
74
75 /** implement functions needed to derive from Factor */
76
77 /** vector of errors */
78 Vector evaluateError(const Pose3& pose, const Point3& bias,
79 OptionalMatrixType H1, OptionalMatrixType H2) const override {
80
81 if (H1 || H2){
82 H1->resize(rows: 3,cols: 6); // jacobian wrt pose
83 (*H1) << Z_3x3, pose.rotation().matrix();
84 H2->resize(rows: 3,cols: 3); // jacobian wrt bias
85 (*H2) << I_3x3;
86 }
87 return pose.translation() + bias - measured_;
88 }
89
90 /** return the measured */
91 const Point3 measured() const {
92 return measured_;
93 }
94
95 private:
96
97#if GTSAM_ENABLE_BOOST_SERIALIZATION
98 /** Serialization function */
99 friend class boost::serialization::access;
100 template<class ARCHIVE>
101 void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
102 // NoiseModelFactor2 instead of NoiseModelFactorN for backward compatibility
103 ar & boost::serialization::make_nvp(n: "NoiseModelFactor2",
104 v&: boost::serialization::base_object<Base>(d&: *this));
105 ar & BOOST_SERIALIZATION_NVP(measured_);
106 }
107#endif
108 }; // \class BiasedGPSFactor
109
110} /// namespace gtsam
111