PoseToPointFactor.h
Go to the documentation of this file.
1 
7 #pragma once
8 
10 #include <gtsam/geometry/Pose3.h>
12 #include <ostream>
13 
14 namespace gtsam {
15 
20 class PoseToPointFactor : public NoiseModelFactor2<Pose3, Point3> {
21  private:
24 
27  public:
28  // shorthand for a smart pointer to a factor
29  typedef boost::shared_ptr<PoseToPointFactor> shared_ptr;
30 
33 
36  const SharedNoiseModel& model)
37  : Base(model, key1, key2), measured_(measured) {}
38 
39  virtual ~PoseToPointFactor() {}
40 
44  virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
45  DefaultKeyFormatter) const {
46  std::cout << s << "PoseToPointFactor(" << keyFormatter(this->key1()) << ","
47  << keyFormatter(this->key2()) << ")\n"
48  << " measured: " << measured_.transpose() << std::endl;
49  this->noiseModel_->print(" noise model: ");
50  }
51 
53  virtual bool equals(const NonlinearFactor& expected,
54  double tol = 1e-9) const {
55  const This* e = dynamic_cast<const This*>(&expected);
56  return e != nullptr && Base::equals(*e, tol) &&
57  traits<Point3>::Equals(this->measured_, e->measured_, tol);
58  }
59 
69  Vector evaluateError(const Pose3& wTwi, const Point3& wPwp,
70  boost::optional<Matrix&> H1 = boost::none,
71  boost::optional<Matrix&> H2 = boost::none) const {
72  return wTwi.transformTo(wPwp, H1, H2) - measured_;
73  }
74 
76  const Point3& measured() const { return measured_; }
77 
78  private:
81  template <class ARCHIVE>
82  void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
83  ar& boost::serialization::make_nvp(
84  "NoiseModelFactor2", boost::serialization::base_object<Base>(*this));
85  ar& BOOST_SERIALIZATION_NVP(measured_);
86  }
87 
88 }; // \class PoseToPointFactor
89 
90 } // namespace gtsam
boost::shared_ptr< PoseToPointFactor > shared_ptr
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
noiseModel::Diagonal::shared_ptr model
Matrix expected
Definition: testMatrix.cpp:974
virtual bool equals(const NonlinearFactor &expected, double tol=1e-9) const
NoiseModelFactor2< Pose3, Point3 > Base
void serialize(ARCHIVE &ar, const unsigned int)
static const KeyFormatter DefaultKeyFormatter
Definition: Key.h:43
virtual void print(const std::string &s, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in world coordinates and transforms it to Pose coordinates
Definition: Pose3.cpp:314
Eigen::VectorXd Vector
Definition: Vector.h:38
Vector evaluateError(const Pose3 &wTwi, const Point3 &wPwp, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
Error = wTwi.inverse()*wPwp - measured_.
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
friend class boost::serialization::access
traits
Definition: chartTesting.h:28
SharedNoiseModel noiseModel_
const Point3 & measured() const
Non-linear factor base classes.
3D Point
const G double tol
Definition: Group.h:83
Vector3 Point3
Definition: Point3.h:35
PoseToPointFactor(Key key1, Key key2, const Point3 &measured, const SharedNoiseModel &model)
3D Pose
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:43:27