Cal3Bundler.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #pragma once
21 
22 #include <gtsam/geometry/Cal3f.h>
23 #include <gtsam/geometry/Point2.h>
24 
25 namespace gtsam {
26 
32 class GTSAM_EXPORT Cal3Bundler : public Cal3f {
33  private:
34  double k1_ = 0.0, k2_ = 0.0;
35  double tol_ = 1e-5;
36 
37  // Note: u0 and v0 are constants and not optimized.
38 
39  public:
40  enum { dimension = 3 };
41  using shared_ptr = std::shared_ptr<Cal3Bundler>;
42 
45 
47  Cal3Bundler() = default;
48 
58  Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
59  double tol = 1e-5)
60  : Cal3f(f, u0, v0), k1_(k1), k2_(k2), tol_(tol) {}
61 
62  ~Cal3Bundler() override = default;
63 
67 
69  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
70  const Cal3Bundler& cal);
71 
73  void print(const std::string& s = "") const override;
74 
76  bool equals(const Cal3Bundler& K, double tol = 1e-9) const;
77 
81 
83  inline double k1() const { return k1_; }
84 
86  inline double k2() const { return k2_; }
87 
88  Matrix3 K() const override;
89  Vector4 k() const;
90 
91  Vector3 vector() const;
92 
102  OptionalJacobian<2, 2> Dp = {}) const;
103 
111  Point2 calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal = {},
112  OptionalJacobian<2, 2> Dp = {}) const;
113 
115  Matrix2 D2d_intrinsic(const Point2& p) const;
116 
118  Matrix23 D2d_calibration(const Point2& p) const;
119 
121  Matrix25 D2d_intrinsic_calibration(const Point2& p) const;
122 
126 
128  size_t dim() const override { return Dim(); }
129 
131  inline static size_t Dim() { return dimension; }
132 
134  Cal3Bundler retract(const Vector& d) const {
135  return Cal3Bundler(fx_ + d(0), k1_ + d(1), k2_ + d(2), u0_, v0_);
136  }
137 
140  return T2.vector() - vector();
141  }
142 
143  private:
147 
148 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
149 
150  friend class boost::serialization::access;
151  template <class Archive>
152  void serialize(Archive& ar, const unsigned int /*version*/) {
153  ar& boost::serialization::make_nvp(
154  "Cal3Bundler", boost::serialization::base_object<Cal3>(*this));
155  ar& BOOST_SERIALIZATION_NVP(k1_);
156  ar& BOOST_SERIALIZATION_NVP(k2_);
157  ar& BOOST_SERIALIZATION_NVP(tol_);
158  }
159 #endif
160 
162 };
163 
164 template <>
165 struct traits<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
166 
167 template <>
168 struct traits<const Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
169 
170 } // namespace gtsam
gtsam::Cal3Bundler::retract
Cal3Bundler retract(const Vector &d) const
Update calibration with tangent space delta.
Definition: Cal3Bundler.h:134
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
v0
static const double v0
Definition: testCal3DFisheye.cpp:31
gtsam::Cal3f
Calibration model with a single focal length and zero skew.
Definition: Cal3f.h:35
gtsam::Cal3Bundler::localCoordinates
Vector3 localCoordinates(const Cal3Bundler &T2) const
Calculate local coordinates to another calibration.
Definition: Cal3Bundler.h:139
gtsam::Cal3Bundler::Cal3Bundler
Cal3Bundler(double f, double k1, double k2, double u0=0, double v0=0, double tol=1e-5)
Definition: Cal3Bundler.h:58
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
fixture::cal
const Cal3_S2 cal(focalLength, focalLength, 0.0, principalPoint.x(), principalPoint.y())
gtsam::Cal3Bundler::dim
size_t dim() const override
Return DOF, dimensionality of tangent space.
Definition: Cal3Bundler.h:128
Point2.h
2D Point
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::Cal3Bundler::k1
double k1() const
distortion parameter k1
Definition: Cal3Bundler.h:83
k1
double k1(double x)
Definition: k1.c:133
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
Cal3f.h
Calibration model with a single focal length and zero skew.
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
T2
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
gtsam::uncalibrate
Point2_ uncalibrate(const Expression< CALIBRATION > &K, const Point2_ &xy_hat)
Definition: slam/expressions.h:172
gtsam::equals
Definition: Testable.h:112
gtsam::Cal3Bundler::k2
double k2() const
distortion parameter k2
Definition: Cal3Bundler.h:86
u0
static const double u0
Definition: testCal3DFisheye.cpp:31
tree::f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Definition: testExpression.cpp:218
gtsam
traits
Definition: SFMdata.h:40
gtsam::traits
Definition: Group.h:36
K
#define K
Definition: igam.h:8
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::Cal3Bundler::Dim
static size_t Dim()
Return DOF, dimensionality of tangent space.
Definition: Cal3Bundler.h:131
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Cal3::shared_ptr
std::shared_ptr< Cal3 > shared_ptr
Definition: Cal3.h:78


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:01:56