gtsam  4.1.0
gtsam
InvDepthFactorVariant3.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 InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, Vector3> {
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 
58  InvDepthFactorVariant3a(const Key poseKey, const Key landmarkKey,
59  const Point2& measured, const Cal3_S2::shared_ptr& K, const SharedNoiseModel& model) :
60  Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {}
61 
64 
70  void print(const std::string& s = "InvDepthFactorVariant3a",
71  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
72  Base::print(s, keyFormatter);
74  }
75 
77  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
78  const This *e = dynamic_cast<const This*>(&p);
79  return e
80  && Base::equals(p, tol)
81  && traits<Point2>::Equals(this->measured_, e->measured_, tol)
82  && this->K_->equals(*e->K_, tol);
83  }
84 
85  Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {
86  try {
87  // Calculate the 3D coordinates of the landmark in the Pose frame
88  double theta = landmark(0), phi = landmark(1), rho = landmark(2);
89  Point3 pose_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho);
90  // Convert the landmark to world coordinates
91  Point3 world_P_landmark = pose.transformFrom(pose_P_landmark);
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->key1()) << "," << 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>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose);
112  }
113  if(H2) {
114  (*H2) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark);
115  }
116 
117  return inverseDepthError(pose, landmark);
118  }
119 
121  const Point2& imagePoint() const {
122  return measured_;
123  }
124 
127  return K_;
128  }
129 
130 private:
131 
134  template<class ARCHIVE>
135  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
136  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
137  ar & BOOST_SERIALIZATION_NVP(measured_);
138  ar & BOOST_SERIALIZATION_NVP(K_);
139  }
140 };
141 
145 class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, Vector3> {
146 protected:
147 
148  // Keep a copy of measurement and calibration for I/O
151 
152 public:
153 
156 
159 
161  typedef boost::shared_ptr<This> shared_ptr;
162 
165  measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
166  }
167 
178  InvDepthFactorVariant3b(const Key poseKey1, const Key poseKey2, const Key landmarkKey,
179  const Point2& measured, const Cal3_S2::shared_ptr& K, const SharedNoiseModel& model) :
180  Base(model, poseKey1, poseKey2, landmarkKey), measured_(measured), K_(K) {}
181 
184 
190  void print(const std::string& s = "InvDepthFactorVariant3",
191  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
192  Base::print(s, keyFormatter);
193  traits<Point2>::Print(measured_, s + ".z");
194  }
195 
197  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
198  const This *e = dynamic_cast<const This*>(&p);
199  return e
200  && Base::equals(p, tol)
201  && traits<Point2>::Equals(this->measured_, e->measured_, tol)
202  && this->K_->equals(*e->K_, tol);
203  }
204 
205  Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark) const {
206  try {
207  // Calculate the 3D coordinates of the landmark in the Pose1 frame
208  double theta = landmark(0), phi = landmark(1), rho = landmark(2);
209  Point3 pose1_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho);
210  // Convert the landmark to world coordinates
211  Point3 world_P_landmark = pose1.transformFrom(pose1_P_landmark);
212  // Project landmark into Pose2
213  PinholeCamera<Cal3_S2> camera(pose2, *K_);
214  return camera.project(world_P_landmark) - measured_;
215  } catch( CheiralityException& e) {
216  std::cout << e.what()
217  << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]"
218  << " moved behind camera " << DefaultKeyFormatter(this->key2())
219  << std::endl;
220  return Vector::Ones(2) * 2.0 * K_->fx();
221  }
222  return (Vector(1) << 0.0).finished();
223  }
224 
226  Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark,
227  boost::optional<Matrix&> H1=boost::none,
228  boost::optional<Matrix&> H2=boost::none,
229  boost::optional<Matrix&> H3=boost::none) const override {
230 
231  if(H1)
232  (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1);
233 
234  if(H2)
235  (*H2) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2);
236 
237  if(H3)
238  (*H3) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark);
239 
240  return inverseDepthError(pose1, pose2, landmark);
241  }
242 
244  const Point2& imagePoint() const {
245  return measured_;
246  }
247 
250  return K_;
251  }
252 
253 private:
254 
257  template<class ARCHIVE>
258  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
259  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
260  ar & BOOST_SERIALIZATION_NVP(measured_);
261  ar & BOOST_SERIALIZATION_NVP(K_);
262  }
263 };
264 
265 } // \ 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::InvDepthFactorVariant3b::This
InvDepthFactorVariant3b This
shorthand for this class
Definition: InvDepthFactorVariant3.h:158
gtsam::Pose3::transformFrom
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in Pose coordinates and transforms it to world coordinates
Definition: Pose3.cpp:298
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::NoiseModelFactor3
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:430
gtsam::InvDepthFactorVariant3a::InvDepthFactorVariant3a
InvDepthFactorVariant3a(const Key poseKey, const Key landmarkKey, const Point2 &measured, const Cal3_S2::shared_ptr &K, const SharedNoiseModel &model)
Constructor TODO: Mark argument order standard (keys, measurement, parameters)
Definition: InvDepthFactorVariant3.h:58
gtsam::InvDepthFactorVariant3a::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: InvDepthFactorVariant3.h:41
gtsam::InvDepthFactorVariant3a::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: InvDepthFactorVariant3.h:106
gtsam::InvDepthFactorVariant3a::access
friend class boost::serialization::access
Serialization function.
Definition: InvDepthFactorVariant3.h:133
gtsam::InvDepthFactorVariant3b::calibration
const Cal3_S2::shared_ptr calibration() const
return the calibration object
Definition: InvDepthFactorVariant3.h:249
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::InvDepthFactorVariant3b::imagePoint
const Point2 & imagePoint() const
return the measurement
Definition: InvDepthFactorVariant3.h:244
gtsam::InvDepthFactorVariant3b::shared_ptr
boost::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: InvDepthFactorVariant3.h:161
gtsam::InvDepthFactorVariant3b::evaluateError
Vector evaluateError(const Pose3 &pose1, const Pose3 &pose2, const Vector3 &landmark, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: InvDepthFactorVariant3.h:226
gtsam::InvDepthFactorVariant3b::Base
NoiseModelFactor3< Pose3, Pose3, Vector3 > Base
shorthand for base class type
Definition: InvDepthFactorVariant3.h:155
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
gtsam::InvDepthFactorVariant3a::~InvDepthFactorVariant3a
virtual ~InvDepthFactorVariant3a()
Virtual destructor.
Definition: InvDepthFactorVariant3.h:63
Pose3.h
3D Pose
gtsam::NoiseModelFactor3< Pose3, Pose3, Vector3 >::key1
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:464
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::InvDepthFactorVariant3b::InvDepthFactorVariant3b
InvDepthFactorVariant3b()
Default constructor.
Definition: InvDepthFactorVariant3.h:164
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::InvDepthFactorVariant3a::K_
Cal3_S2::shared_ptr K_
shared pointer to calibration object
Definition: InvDepthFactorVariant3.h:30
gtsam::InvDepthFactorVariant3a::Base
NoiseModelFactor2< Pose3, Vector3 > Base
shorthand for base class type
Definition: InvDepthFactorVariant3.h:35
gtsam::InvDepthFactorVariant3a::print
void print(const std::string &s="InvDepthFactorVariant3a", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: InvDepthFactorVariant3.h:70
gtsam::InvDepthFactorVariant3a::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: InvDepthFactorVariant3.h:77
gtsam::InvDepthFactorVariant3b::measured_
Point2 measured_
2D measurement
Definition: InvDepthFactorVariant3.h:149
gtsam::InvDepthFactorVariant3b::K_
Cal3_S2::shared_ptr K_
shared pointer to calibration object
Definition: InvDepthFactorVariant3.h:150
gtsam::NonlinearFactor
Nonlinear factor base class.
Definition: NonlinearFactor.h:43
NonlinearFactor.h
Non-linear factor base classes.
gtsam::InvDepthFactorVariant3b::access
friend class boost::serialization::access
Serialization function.
Definition: InvDepthFactorVariant3.h:256
gtsam::InvDepthFactorVariant3a::calibration
const Cal3_S2::shared_ptr calibration() const
return the calibration object
Definition: InvDepthFactorVariant3.h:126
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::InvDepthFactorVariant3a::InvDepthFactorVariant3a
InvDepthFactorVariant3a()
Default constructor.
Definition: InvDepthFactorVariant3.h:44
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
gtsam::InvDepthFactorVariant3a::This
InvDepthFactorVariant3a This
shorthand for this class
Definition: InvDepthFactorVariant3.h:38
Point2.h
2D Point
gtsam::InvDepthFactorVariant3b::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: InvDepthFactorVariant3.h:197
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::InvDepthFactorVariant3b::print
void print(const std::string &s="InvDepthFactorVariant3", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: InvDepthFactorVariant3.h:190
gtsam::InvDepthFactorVariant3b::~InvDepthFactorVariant3b
virtual ~InvDepthFactorVariant3b()
Virtual destructor.
Definition: InvDepthFactorVariant3.h:183
gtsam::InvDepthFactorVariant3b
Ternary factor representing a visual measurement using an inverse-depth parameterization.
Definition: InvDepthFactorVariant3.h:145
gtsam::InvDepthFactorVariant3a::measured_
Point2 measured_
2D measurement
Definition: InvDepthFactorVariant3.h:29
gtsam::InvDepthFactorVariant3b::InvDepthFactorVariant3b
InvDepthFactorVariant3b(const Key poseKey1, const Key poseKey2, const Key landmarkKey, const Point2 &measured, const Cal3_S2::shared_ptr &K, const SharedNoiseModel &model)
Constructor TODO: Mark argument order standard (keys, measurement, parameters)
Definition: InvDepthFactorVariant3.h:178
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::InvDepthFactorVariant3a
Binary factor representing the first visual measurement using an inverse-depth parameterization.
Definition: InvDepthFactorVariant3.h:25
gtsam::InvDepthFactorVariant3a::imagePoint
const Point2 & imagePoint() const
return the measurement
Definition: InvDepthFactorVariant3.h:121