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 NoiseModelFactorN<Pose3, Vector3> {
26 protected:
27 
28  // Keep a copy of measurement and calibration for I/O
31 
32 public:
33 
35  typedef NoiseModelFactor2<Pose3, Vector3> Base;
36 
37  // Provide access to the Matrix& version of evaluateError:
38  using Base::evaluateError;
39 
42 
44  typedef std::shared_ptr<This> shared_ptr;
45 
48  measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
49  }
50 
61  InvDepthFactorVariant3a(const Key poseKey, const Key landmarkKey,
62  const Point2& measured, const Cal3_S2::shared_ptr& K, const SharedNoiseModel& model) :
63  Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {}
64 
67 
73  void print(const std::string& s = "InvDepthFactorVariant3a",
74  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
75  Base::print(s, keyFormatter);
77  }
78 
80  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
81  const This *e = dynamic_cast<const This*>(&p);
82  return e
83  && Base::equals(p, tol)
84  && traits<Point2>::Equals(this->measured_, e->measured_, tol)
85  && this->K_->equals(*e->K_, tol);
86  }
87 
89  try {
90  // Calculate the 3D coordinates of the landmark in the Pose frame
91  double theta = landmark(0), phi = landmark(1), rho = landmark(2);
92  Point3 pose_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho);
93  // Convert the landmark to world coordinates
94  Point3 world_P_landmark = pose.transformFrom(pose_P_landmark);
95  // Project landmark into Pose2
97  return camera.project(world_P_landmark) - measured_;
98  } catch( CheiralityException& e) {
99  std::cout << e.what()
100  << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<2>()) << "]"
101  << " moved behind camera [" << DefaultKeyFormatter(this->key<1>()) << "]"
102  << std::endl;
103  return Vector::Ones(2) * 2.0 * K_->fx();
104  }
105  return (Vector(1) << 0.0).finished();
106  }
107 
110  OptionalMatrixType H1, OptionalMatrixType H2) const override {
111 
112  if(H1) {
113  (*H1) = numericalDerivative11<Vector, Pose3>(
115  std::placeholders::_1, landmark),
116  pose);
117  }
118  if(H2) {
119  (*H2) = numericalDerivative11<Vector, Vector3>(
121  std::placeholders::_1),
122  landmark);
123  }
124 
126  }
127 
129  const Point2& imagePoint() const {
130  return measured_;
131  }
132 
135  return K_;
136  }
137 
138 private:
139 
140 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
141  friend class boost::serialization::access;
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  }
149 #endif
150 };
151 
155 class InvDepthFactorVariant3b: public NoiseModelFactorN<Pose3, Pose3, Vector3> {
156 protected:
157 
158  // Keep a copy of measurement and calibration for I/O
161 
162 public:
163 
166 
169 
171  typedef std::shared_ptr<This> shared_ptr;
172 
175  measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
176  }
177 
188  InvDepthFactorVariant3b(const Key poseKey1, const Key poseKey2, const Key landmarkKey,
189  const Point2& measured, const Cal3_S2::shared_ptr& K, const SharedNoiseModel& model) :
190  Base(model, poseKey1, poseKey2, landmarkKey), measured_(measured), K_(K) {}
191 
194 
200  void print(const std::string& s = "InvDepthFactorVariant3",
201  const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
202  Base::print(s, keyFormatter);
204  }
205 
207  bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
208  const This *e = dynamic_cast<const This*>(&p);
209  return e
210  && Base::equals(p, tol)
211  && traits<Point2>::Equals(this->measured_, e->measured_, tol)
212  && this->K_->equals(*e->K_, tol);
213  }
214 
215  Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark) const {
216  try {
217  // Calculate the 3D coordinates of the landmark in the Pose1 frame
218  double theta = landmark(0), phi = landmark(1), rho = landmark(2);
219  Point3 pose1_P_landmark(cos(phi)*sin(theta)/rho, sin(phi)/rho, cos(phi)*cos(theta)/rho);
220  // Convert the landmark to world coordinates
221  Point3 world_P_landmark = pose1.transformFrom(pose1_P_landmark);
222  // Project landmark into Pose2
224  return camera.project(world_P_landmark) - measured_;
225  } catch( CheiralityException& e) {
226  std::cout << e.what()
227  << ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key<1>()) << "," << DefaultKeyFormatter(this->key<3>()) << "]"
228  << " moved behind camera " << DefaultKeyFormatter(this->key<2>())
229  << std::endl;
230  return Vector::Ones(2) * 2.0 * K_->fx();
231  }
232  return (Vector(1) << 0.0).finished();
233  }
234 
237  OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override {
238 
239  if(H1)
240  (*H1) = numericalDerivative11<Vector, Pose3>(
242  std::placeholders::_1, pose2, landmark),
243  pose1);
244 
245  if(H2)
246  (*H2) = numericalDerivative11<Vector, Pose3>(
248  std::placeholders::_1, landmark),
249  pose2);
250 
251  if(H3)
252  (*H3) = numericalDerivative11<Vector, Vector3>(
254  pose2, std::placeholders::_1),
255  landmark);
256 
258  }
259 
261  const Point2& imagePoint() const {
262  return measured_;
263  }
264 
267  return K_;
268  }
269 
270 private:
271 
272 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
273  friend class boost::serialization::access;
275  template<class ARCHIVE>
276  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
277  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
278  ar & BOOST_SERIALIZATION_NVP(measured_);
279  ar & BOOST_SERIALIZATION_NVP(K_);
280  }
281 #endif
282 };
283 
284 } // \ namespace gtsam
Eigen::internal::print
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
Definition: NEON/PacketMath.h:3115
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
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)
Definition: InvDepthFactorVariant3.h:188
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::InvDepthFactorVariant3b::print
void print(const std::string &s="InvDepthFactorVariant3", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: InvDepthFactorVariant3.h:200
ceres::sin
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::InvDepthFactorVariant3a::InvDepthFactorVariant3a
InvDepthFactorVariant3a(const Key poseKey, const Key landmarkKey, const Point2 &measured, const Cal3_S2::shared_ptr &K, const SharedNoiseModel &model)
Definition: InvDepthFactorVariant3.h:61
gtsam::InvDepthFactorVariant3b::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: InvDepthFactorVariant3.h:171
measured
Point2 measured(-17, 30)
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
gtsam::Factor
Definition: Factor.h:70
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:44
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::InvDepthFactorVariant3b::evaluateError
Vector evaluateError(const Pose3 &pose1, const Pose3 &pose2, const Vector3 &landmark, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: InvDepthFactorVariant3.h:236
poseKey
const gtsam::Key poseKey
Definition: testPoseRotationPrior.cpp:29
ceres::cos
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Point2.h
2D Point
gtsam::DefaultKeyFormatter
KeyFormatter DefaultKeyFormatter
Assign default key formatter.
Definition: Key.cpp:30
gtsam::CheiralityException
Definition: CalibratedCamera.h:34
numericalDerivative.h
Some functions to compute numerical derivatives.
gtsam::InvDepthFactorVariant3a::imagePoint
const Point2 & imagePoint() const
Definition: InvDepthFactorVariant3.h:129
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::Pose3
Definition: Pose3.h:37
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:432
gtsam::NoiseModelFactor::print
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactor.cpp:74
landmark
static Point3 landmark(0, 0, 5)
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::InvDepthFactorVariant3a
Definition: InvDepthFactorVariant3.h:25
gtsam::NoiseModelFactor::equals
bool equals(const NonlinearFactor &f, double tol=1e-9) const override
Definition: NonlinearFactor.cpp:82
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::InvDepthFactorVariant3b::calibration
const Cal3_S2::shared_ptr calibration() const
Definition: InvDepthFactorVariant3.h:266
gtsam::InvDepthFactorVariant3b::inverseDepthError
Vector inverseDepthError(const Pose3 &pose1, const Pose3 &pose2, const Vector3 &landmark) const
Definition: InvDepthFactorVariant3.h:215
gtsam::InvDepthFactorVariant3a::shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
Definition: InvDepthFactorVariant3.h:44
gtsam::InvDepthFactorVariant3b::Base
NoiseModelFactorN< Pose3, Pose3, Vector3 > Base
shorthand for base class type
Definition: InvDepthFactorVariant3.h:165
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::InvDepthFactorVariant3b::This
InvDepthFactorVariant3b This
shorthand for this class
Definition: InvDepthFactorVariant3.h:168
gtsam::InvDepthFactorVariant3a::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: InvDepthFactorVariant3.h:80
gtsam::InvDepthFactorVariant3a::evaluateError
Vector evaluateError(const Pose3 &pose, const Vector3 &landmark, OptionalMatrixType H1, OptionalMatrixType H2) const override
Evaluate error h(x)-z and optionally derivatives.
Definition: InvDepthFactorVariant3.h:109
gtsam::InvDepthFactorVariant3b::equals
bool equals(const NonlinearFactor &p, double tol=1e-9) const override
equals
Definition: InvDepthFactorVariant3.h:207
NonlinearFactor.h
Non-linear factor base classes.
gtsam::InvDepthFactorVariant3b::InvDepthFactorVariant3b
InvDepthFactorVariant3b()
Default constructor.
Definition: InvDepthFactorVariant3.h:174
gtsam::InvDepthFactorVariant3a::This
InvDepthFactorVariant3a This
shorthand for this class
Definition: InvDepthFactorVariant3.h:41
gtsam
traits
Definition: SFMdata.h:40
poseKey2
static Key poseKey2(X(2))
pose1
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
gtsam::traits
Definition: Group.h:36
K
#define K
Definition: igam.h:8
gtsam::InvDepthFactorVariant3b::measured_
Point2 measured_
2D measurement
Definition: InvDepthFactorVariant3.h:159
gtsam::InvDepthFactorVariant3b::~InvDepthFactorVariant3b
~InvDepthFactorVariant3b() override
Definition: InvDepthFactorVariant3.h:193
gtsam::NonlinearFactor
Definition: NonlinearFactor.h:69
gtsam::InvDepthFactorVariant3b
Definition: InvDepthFactorVariant3.h:155
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::InvDepthFactorVariant3a::~InvDepthFactorVariant3a
~InvDepthFactorVariant3a() override
Definition: InvDepthFactorVariant3.h:66
gtsam::InvDepthFactorVariant3a::InvDepthFactorVariant3a
InvDepthFactorVariant3a()
Default constructor.
Definition: InvDepthFactorVariant3.h:47
gtsam::InvDepthFactorVariant3a::Base
NoiseModelFactor2< Pose3, Vector3 > Base
shorthand for base class type
Definition: InvDepthFactorVariant3.h:35
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::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::InvDepthFactorVariant3b::imagePoint
const Point2 & imagePoint() const
Definition: InvDepthFactorVariant3.h:261
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:56
poseKey1
static Key poseKey1(X(1))
camera
static const CalibratedCamera camera(kDefaultPose)
gtsam::InvDepthFactorVariant3a::print
void print(const std::string &s="InvDepthFactorVariant3a", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: InvDepthFactorVariant3.h:73
Base
Definition: test_virtual_functions.cpp:156
gtsam::InvDepthFactorVariant3a::K_
Cal3_S2::shared_ptr K_
shared pointer to calibration object
Definition: InvDepthFactorVariant3.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::InvDepthFactorVariant3a::measured_
Point2 measured_
2D measurement
Definition: InvDepthFactorVariant3.h:29
gtsam::InvDepthFactorVariant3a::inverseDepthError
Vector inverseDepthError(const Pose3 &pose, const Vector3 &landmark) const
Definition: InvDepthFactorVariant3.h:88
Pose3.h
3D Pose manifold SO(3) x R^3 and group SE(3)
gtsam::InvDepthFactorVariant3b::K_
Cal3_S2::shared_ptr K_
shared pointer to calibration object
Definition: InvDepthFactorVariant3.h:160
gtsam::InvDepthFactorVariant3a::calibration
const Cal3_S2::shared_ptr calibration() const
Definition: InvDepthFactorVariant3.h:134


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:42