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 
36 
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  ~BiasedGPSFactor() override {}
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 
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 }
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
noiseModel::Diagonal::shared_ptr model
Matrix expected
Definition: testMatrix.cpp:974
boost::shared_ptr< BiasedGPSFactor > shared_ptr
Matrix3 matrix() const
Definition: Rot3M.cpp:219
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition: Pose3.cpp:259
friend class boost::serialization::access
BiasedGPSFactor(Key posekey, Key biaskey, const Point3 measured, const SharedNoiseModel &model)
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
NoiseModelFactor2< Pose3, Point3 > Base
void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Eigen::VectorXd Vector
Definition: Vector.h:38
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
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
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
const Point3 measured() const
Point3 bias(10,-10, 50)
traits
Definition: chartTesting.h:28
SharedNoiseModel noiseModel_
Vector evaluateError(const Pose3 &pose, const Point3 &bias, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Non-linear factor base classes.
BiasedGPSFactor This
const G double tol
Definition: Group.h:83
Vector3 Point3
Definition: Point3.h:35
3D Pose
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
void serialize(ARCHIVE &ar, const unsigned int)
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition: Pose3.cpp:266


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:41:42