gtsam  4.0.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 {
73  Base::print(s, keyFormatter);
75  }
76 
78  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
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 {
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
This is the base class for all factor types.
Definition: Factor.h:54
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:345
The most common 5DOF 3D->2D calibration.
Cal3_S2::shared_ptr K_
shared pointer to calibration object
Definition: InvDepthFactorVariant2.h:31
Definition: PinholeCamera.h:33
Definition: Point3.h:45
virtual void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Print.
Definition: NonlinearFactor.cpp:63
Binary factor representing a visual measurement using an inverse-depth parameterization.
Definition: InvDepthFactorVariant2.h:26
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: InvDepthFactorVariant2.h:43
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
NoiseModelFactor2< Pose3, Vector3 > Base
shorthand for base class type
Definition: InvDepthFactorVariant2.h:37
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
const Point3 & referencePoint() const
return the calibration object
Definition: InvDepthFactorVariant2.h:135
boost::shared_ptr< Cal3_S2 > shared_ptr
shared pointer to calibration object
Definition: Cal3_S2.h:39
2D Point
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:33
virtual bool equals(const NonlinearFactor &p, double tol=1e-9) const
equals
Definition: InvDepthFactorVariant2.h:78
Base class for all pinhole cameras.
Nonlinear factor base class.
Definition: NonlinearFactor.h:50
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:377
void print(const std::string &s="InvDepthFactorVariant2", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition: InvDepthFactorVariant2.h:71
friend class boost::serialization::access
Serialization function.
Definition: InvDepthFactorVariant2.h:142
InvDepthFactorVariant2()
Default constructor.
Definition: InvDepthFactorVariant2.h:46
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
Point3 referencePoint_
the reference point/origin for this landmark
Definition: InvDepthFactorVariant2.h:32
Non-linear factor base classes.
3D Pose
Point2 measured_
2D measurement
Definition: InvDepthFactorVariant2.h:30
Definition: Point2.h:40
const Cal3_S2::shared_ptr calibration() const
return the calibration object
Definition: InvDepthFactorVariant2.h:130
Definition: Cal3_S2.h:33
InvDepthFactorVariant2 This
shorthand for this class
Definition: InvDepthFactorVariant2.h:40
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:1072
virtual ~InvDepthFactorVariant2()
Virtual destructor.
Definition: InvDepthFactorVariant2.h:64
Vector evaluateError(const Pose3 &pose, const Vector3 &landmark, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const
Evaluate error h(x)-z and optionally derivatives.
Definition: InvDepthFactorVariant2.h:106
Some functions to compute numerical derivatives.
Definition: Pose3.h:37
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
Check if two factors are equal.
Definition: NonlinearFactor.cpp:71
const Point2 & imagePoint() const
return the measurement
Definition: InvDepthFactorVariant2.h:125