InvDepthFactor3.h
Go to the documentation of this file.
1 
15 #pragma once
16 
17 #include <gtsam/geometry/Cal3_S2.h>
20 
21 namespace gtsam {
22 
26 template<class POSE, class LANDMARK, class INVDEPTH>
27 class InvDepthFactor3: public NoiseModelFactorN<POSE, LANDMARK, INVDEPTH> {
28 protected:
29 
30  // Keep a copy of measurement and calibration for I/O
32  std::shared_ptr<Cal3_S2> K_;
33 
34 public:
35 
37  typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
38 
39  // Provide access to the Matrix& version of evaluateError:
40  using Base::evaluateError;
41 
44 
46  typedef std::shared_ptr<This> shared_ptr;
47 
50  measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
51  }
52 
64  const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr& K) :
65  Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {}
66 
68  ~InvDepthFactor3() override {}
69 
75  void print(const std::string& s = "InvDepthFactor3",
76  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
77  Base::print(s, keyFormatter);
79  }
80 
82  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
83  const This *e = dynamic_cast<const This*>(&p);
84  return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol);
85  }
86 
88  Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
89  OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
90  try {
92  return camera.project(point, invDepth, H1, H2, H3) - measured_;
93  } catch( CheiralityException& e) {
94  if (H1) *H1 = Matrix::Zero(2,6);
95  if (H2) *H2 = Matrix::Zero(2,5);
96  if (H3) *H3 = Matrix::Zero(2,1);
97  std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
98  " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
99  return Vector::Ones(2) * 2.0 * K_->fx();
100  }
101  return (Vector(1) << 0.0).finished();
102  }
103 
105  const Point2& imagePoint() const {
106  return measured_;
107  }
108 
110  inline const Cal3_S2::shared_ptr calibration() const {
111  return K_;
112  }
113 
114 private:
115 
116 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
117  friend class boost::serialization::access;
119  template<class ARCHIVE>
120  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
121  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
122  ar & BOOST_SERIALIZATION_NVP(measured_);
123  ar & BOOST_SERIALIZATION_NVP(K_);
124  }
125 #endif
126 };
127 } // \ namespace gtsam
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
gtsam::InvDepthFactor3::calibration
const Cal3_S2::shared_ptr calibration() const
Definition: InvDepthFactor3.h:110
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
pointKey
const gtsam::Key pointKey
Definition: testRelativeElevationFactor.cpp:25
gtsam::InvDepthFactor3::evaluateError
Vector evaluateError(const POSE &pose, const Vector5 &point, const INVDEPTH &invDepth, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: InvDepthFactor3.h:88
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::InvDepthFactor3
Definition: InvDepthFactor3.h:27
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::InvDepthFactor3::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: InvDepthFactor3.h:82
measured
Point2 measured(-17, 30)
gtsam::InvDepthFactor3::This
InvDepthFactor3< POSE, LANDMARK, INVDEPTH > This
shorthand for this class
Definition: InvDepthFactor3.h:43
gtsam::Factor
Definition: Factor.h:70
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::InvDepthFactor3::Base
NoiseModelFactor3< POSE, LANDMARK, INVDEPTH > Base
shorthand for base class type
Definition: InvDepthFactor3.h:37
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::CheiralityException
Definition: CalibratedCamera.h:34
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::InvDepthFactor3::~InvDepthFactor3
~InvDepthFactor3() override
Definition: InvDepthFactor3.h:68
gtsam::KeyFormatter
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
gtsam::InvDepthCamera3
Definition: InvDepthCamera3.h:34
InvDepthCamera3.h
Inverse Depth Camera based on Civera09tro, Montiel06rss. Landmarks are initialized from the first cam...
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::Cal3_S2::shared_ptr
std::shared_ptr< Cal3_S2 > shared_ptr
Definition: Cal3_S2.h:39
gtsam::NoiseModelFactorN< POSE, LANDMARK, INVDEPTH >::key1
Key key1() const
Definition: NonlinearFactor.h:731
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
NonlinearFactor.h
Non-linear factor base classes.
gtsam::NoiseModelFactorN< POSE, LANDMARK, INVDEPTH >::key2
Key key2() const
Definition: NonlinearFactor.h:735
gtsam
traits
Definition: SFMdata.h:40
gtsam::traits
Definition: Group.h:36
K
#define K
Definition: igam.h:8
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:68
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::Print
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
Definition: Key.cpp:65
gtsam::InvDepthFactor3::InvDepthFactor3
InvDepthFactor3()
Default constructor.
Definition: InvDepthFactor3.h:49
gtsam::InvDepthFactor3::imagePoint
const Point2 & imagePoint() const
Definition: InvDepthFactor3.h:105
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
camera
static const CalibratedCamera camera(kDefaultPose)
Base
Definition: test_virtual_functions.cpp:156
gtsam::InvDepthFactor3::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: InvDepthFactor3.h:46
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::InvDepthFactor3::K_
std::shared_ptr< Cal3_S2 > K_
shared pointer to calibration object
Definition: InvDepthFactor3.h:32
gtsam::InvDepthFactor3::measured_
Point2 measured_
2D measurement
Definition: InvDepthFactor3.h:31
gtsam::InvDepthFactor3::print
void print(const std::string &s="InvDepthFactor3", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: InvDepthFactor3.h:75
gtsam::InvDepthFactor3::InvDepthFactor3
InvDepthFactor3(const Point2 &measured, const SharedNoiseModel &model, const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr &K)
Definition: InvDepthFactor3.h:63


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:02:28