gtsam  4.1.0
gtsam
InvDepthFactorVariant2.h
Go to the documentation of this file.
1 
12 #pragma once
13 
16 #include <gtsam/geometry/Cal3_S2.h>
17 #include <gtsam/geometry/Pose3.h>
18 #include <gtsam/geometry/Point2.h>
20 
21 namespace gtsam {
22 
26 class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, Vector3> {
27 protected:
28 
29  // Keep a copy of measurement and calibration for I/O
33 
34 public:
35 
38 
41 
43  typedef boost::shared_ptr<This> shared_ptr;
44 
47  measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
48  }
49 
58  InvDepthFactorVariant2(const Key poseKey, const Key landmarkKey,
59  const Point2& measured, const Cal3_S2::shared_ptr& K, const Point3 referencePoint,
60  const SharedNoiseModel& model) :
61  Base(model, poseKey, landmarkKey), measured_(measured), K_(K), referencePoint_(referencePoint) {}
62 
65 
71  void print(const std::string& s = "InvDepthFactorVariant2",
72  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
73  Base::print(s, keyFormatter);
75  }
76 
78  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
79  const This *e = dynamic_cast<const This*>(&p);
80  return e
81  && Base::equals(p, tol)
82  && traits<Point2>::Equals(this->measured_, e->measured_, tol)
83  && this->K_->equals(*e->K_, tol)
84  && traits<Point3>::Equals(this->referencePoint_, e->referencePoint_, tol);
85  }
86 
87  Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {
88  try {
89  // Calculate the 3D coordinates of the landmark in the world frame
90  double theta = landmark(0), phi = landmark(1), rho = landmark(2);
91  Point3 world_P_landmark = referencePoint_ + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
92  // Project landmark into Pose2
93  PinholeCamera<Cal3_S2> camera(pose, *K_);
94  return camera.project(world_P_landmark) - measured_;
95  } catch( CheiralityException& e) {
96  std::cout << e.what()
97  << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
98  << " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
99  << std::endl;
100  return Vector::Ones(2) * 2.0 * K_->fx();
101  }
102  return (Vector(1) << 0.0).finished();
103  }
104 
106  Vector evaluateError(const Pose3& pose, const Vector3& landmark,
107  boost::optional<Matrix&> H1=boost::none,
108  boost::optional<Matrix&> H2=boost::none) const override {
109 
110  if (H1) {
111  (*H1) = numericalDerivative11<Vector, Pose3>(
112  boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, _1,
113  landmark), pose);
114  }
115  if (H2) {
116  (*H2) = numericalDerivative11<Vector, Vector3>(
117  boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose,
118  _1), landmark);
119  }
120 
121  return inverseDepthError(pose, landmark);
122  }
123 
125  const Point2& imagePoint() const {
126  return measured_;
127  }
128 
131  return K_;
132  }
133 
135  const Point3& referencePoint() const {
136  return referencePoint_;
137  }
138 
139 private:
140 
143  template<class ARCHIVE>
144  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
145  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
146  ar & BOOST_SERIALIZATION_NVP(measured_);
147  ar & BOOST_SERIALIZATION_NVP(K_);
148  ar & BOOST_SERIALIZATION_NVP(referencePoint_);
149  }
150 };
151 
152 } // \ namespace gtsam
gtsam::Cal3_S2::shared_ptr
boost::shared_ptr< Cal3_S2 > shared_ptr
shared pointer to calibration object
Definition: Cal3_S2.h:39
gtsam::InvDepthFactorVariant2::evaluateError
Vector evaluateError(const Pose3 &pose, const Vector3 &landmark, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: InvDepthFactorVariant2.h:106
gtsam::InvDepthFactorVariant2::imagePoint
const Point2 & imagePoint() const
return the measurement
Definition: InvDepthFactorVariant2.h:125
gtsam::InvDepthFactorVariant2::~InvDepthFactorVariant2
virtual ~InvDepthFactorVariant2()
Virtual destructor.
Definition: InvDepthFactorVariant2.h:64
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::InvDepthFactorVariant2::InvDepthFactorVariant2
InvDepthFactorVariant2()
Default constructor.
Definition: InvDepthFactorVariant2.h:46
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::InvDepthFactorVariant2::This
InvDepthFactorVariant2 This
shorthand for this class
Definition: InvDepthFactorVariant2.h:40
gtsam::InvDepthFactorVariant2::K_
Cal3_S2::shared_ptr K_
shared pointer to calibration object
Definition: InvDepthFactorVariant2.h:31
gtsam::NoiseModelFactor2< Pose3, Vector3 >::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::InvDepthFactorVariant2::calibration
const Cal3_S2::shared_ptr calibration() const
return the calibration object
Definition: InvDepthFactorVariant2.h:130
gtsam::InvDepthFactorVariant2::InvDepthFactorVariant2
InvDepthFactorVariant2(const Key poseKey, const Key landmarkKey, const Point2 &measured, const Cal3_S2::shared_ptr &K, const Point3 referencePoint, const SharedNoiseModel &model)
Constructor.
Definition: InvDepthFactorVariant2.h:58
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
gtsam::InvDepthFactorVariant2::Base
NoiseModelFactor2< Pose3, Vector3 > Base
shorthand for base class type
Definition: InvDepthFactorVariant2.h:37
NonlinearFactor.h
Non-linear factor base classes.
gtsam::InvDepthFactorVariant2
Binary factor representing a visual measurement using an inverse-depth parameterization.
Definition: InvDepthFactorVariant2.h:26
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::InvDepthFactorVariant2::print
void print(const std::string &s="InvDepthFactorVariant2", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: InvDepthFactorVariant2.h:71
gtsam::InvDepthFactorVariant2::referencePoint_
Point3 referencePoint_
the reference point/origin for this landmark
Definition: InvDepthFactorVariant2.h:32
gtsam::InvDepthFactorVariant2::referencePoint
const Point3 & referencePoint() const
return the calibration object
Definition: InvDepthFactorVariant2.h:135
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::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::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::InvDepthFactorVariant2::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: InvDepthFactorVariant2.h:43
gtsam::InvDepthFactorVariant2::access
friend class boost::serialization::access
Serialization function.
Definition: InvDepthFactorVariant2.h:142
gtsam::InvDepthFactorVariant2::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: InvDepthFactorVariant2.h:78
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::InvDepthFactorVariant2::measured_
Point2 measured_
2D measurement
Definition: InvDepthFactorVariant2.h:30