gtsam  4.1.0
gtsam
InvDepthFactorVariant1.h
Go to the documentation of this file.
1 
11 #pragma once
12 
15 #include <gtsam/geometry/Cal3_S2.h>
16 #include <gtsam/geometry/Pose3.h>
17 #include <gtsam/geometry/Point2.h>
19 
20 namespace gtsam {
21 
25 class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, Vector6> {
26 protected:
27 
28  // Keep a copy of measurement and calibration for I/O
31 
32 public:
33 
36 
39 
41  typedef boost::shared_ptr<This> shared_ptr;
42 
45  measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
46  }
47 
56  InvDepthFactorVariant1(const Key poseKey, const Key landmarkKey,
57  const Point2& measured, const Cal3_S2::shared_ptr& K, const SharedNoiseModel& model) :
58  Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {}
59 
62 
68  void print(const std::string& s = "InvDepthFactorVariant1",
69  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
70  Base::print(s, keyFormatter);
72  }
73 
75  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
76  const This *e = dynamic_cast<const This*>(&p);
77  return e
78  && Base::equals(p, tol)
79  && traits<Point2>::Equals(this->measured_, e->measured_, tol)
80  && this->K_->equals(*e->K_, tol);
81  }
82 
83  Vector inverseDepthError(const Pose3& pose, const Vector6& landmark) const {
84  try {
85  // Calculate the 3D coordinates of the landmark in the world frame
86  double x = landmark(0), y = landmark(1), z = landmark(2);
87  double theta = landmark(3), phi = landmark(4), rho = landmark(5);
88  Point3 world_P_landmark = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
89  // Project landmark into Pose2
90  PinholeCamera<Cal3_S2> camera(pose, *K_);
91  return camera.project(world_P_landmark) - measured_;
92  } catch( CheiralityException& e) {
93  std::cout << e.what()
94  << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
95  << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
96  << std::endl;
97  return Vector::Ones(2) * 2.0 * K_->fx();
98  }
99  return (Vector(1) << 0.0).finished();
100  }
101 
103  Vector evaluateError(const Pose3& pose, const Vector6& landmark,
104  boost::optional<Matrix&> H1=boost::none,
105  boost::optional<Matrix&> H2=boost::none) const override {
106 
107  if (H1) {
108  (*H1) = numericalDerivative11<Vector, Pose3>(
109  boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, _1,
110  landmark), pose);
111  }
112  if (H2) {
113  (*H2) = numericalDerivative11<Vector, Vector6>(
114  boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose,
115  _1), landmark);
116  }
117 
118  return inverseDepthError(pose, landmark);
119  }
120 
122  const Point2& imagePoint() const {
123  return measured_;
124  }
125 
128  return K_;
129  }
130 
131 private:
132 
135  template<class ARCHIVE>
136  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
137  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
138  ar & BOOST_SERIALIZATION_NVP(measured_);
139  ar & BOOST_SERIALIZATION_NVP(K_);
140  }
141 };
142 
143 } // \ namespace gtsam
gtsam::InvDepthFactorVariant1::measured_
Point2 measured_
2D measurement
Definition: InvDepthFactorVariant1.h:29
gtsam::Cal3_S2::shared_ptr
boost::shared_ptr< Cal3_S2 > shared_ptr
shared pointer to calibration object
Definition: Cal3_S2.h:39
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Print.
Definition: NonlinearFactor.cpp:63
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
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::InvDepthFactorVariant1::print
void print(const std::string &s="InvDepthFactorVariant1", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: InvDepthFactorVariant1.h:68
gtsam::NoiseModelFactor2< Pose3, Vector6 >::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
gtsam::Cal3_S2
Definition: Cal3_S2.h:33
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::InvDepthFactorVariant1::calibration
const Cal3_S2::shared_ptr calibration() const
return the calibration object
Definition: InvDepthFactorVariant1.h:127
gtsam::InvDepthFactorVariant1::Base
NoiseModelFactor2< Pose3, Vector6 > Base
shorthand for base class type
Definition: InvDepthFactorVariant1.h:35
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::InvDepthFactorVariant1::This
InvDepthFactorVariant1 This
shorthand for this class
Definition: InvDepthFactorVariant1.h:38
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition: Point2.h:27
gtsam::InvDepthFactorVariant1::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: InvDepthFactorVariant1.h:41
gtsam::InvDepthFactorVariant1::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: InvDepthFactorVariant1.h:75
gtsam::InvDepthFactorVariant1::evaluateError
Vector evaluateError(const Pose3 &pose, const Vector6 &landmark, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: InvDepthFactorVariant1.h:103
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::Pose3
Definition: Pose3.h:37
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::InvDepthFactorVariant1::access
friend class boost::serialization::access
Serialization function.
Definition: InvDepthFactorVariant1.h:134
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
Point2.h
2D Point
gtsam::InvDepthFactorVariant1::InvDepthFactorVariant1
InvDepthFactorVariant1(const Key poseKey, const Key landmarkKey, const Point2 &measured, const Cal3_S2::shared_ptr &K, const SharedNoiseModel &model)
Constructor.
Definition: InvDepthFactorVariant1.h:56
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::InvDepthFactorVariant1
Binary factor representing a visual measurement using an inverse-depth parameterization.
Definition: InvDepthFactorVariant1.h:25
gtsam::InvDepthFactorVariant1::imagePoint
const Point2 & imagePoint() const
return the measurement
Definition: InvDepthFactorVariant1.h:122
gtsam::InvDepthFactorVariant1::~InvDepthFactorVariant1
virtual ~InvDepthFactorVariant1()
Virtual destructor.
Definition: InvDepthFactorVariant1.h:61
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::InvDepthFactorVariant1::InvDepthFactorVariant1
InvDepthFactorVariant1()
Default constructor.
Definition: InvDepthFactorVariant1.h:44
gtsam::InvDepthFactorVariant1::K_
Cal3_S2::shared_ptr K_
shared pointer to calibration object
Definition: InvDepthFactorVariant1.h:30