Cal3Unified.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 
24 #pragma once
25 
27 
28 namespace gtsam {
29 
45 class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
46  using This = Cal3Unified;
47  using Base = Cal3DS2_Base;
48 
49  private:
50  double xi_ = 0.0f;
51 
52  public:
53  enum { dimension = 10 };
54 
56  using shared_ptr = std::shared_ptr<Cal3Unified>;
57 
60 
62  Cal3Unified() = default;
63 
64  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1,
65  double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0)
66  : Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
67 
68  ~Cal3Unified() override {}
69 
73 
74  Cal3Unified(const Vector10& v)
75  : Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {}
76 
80 
82  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
83  const Cal3Unified& cal);
84 
86  void print(const std::string& s = "") const override;
87 
89  bool equals(const Cal3Unified& K, double tol = 10e-9) const;
90 
94 
96  inline double xi() const { return xi_; }
97 
99  Vector10 vector() const;
100 
108  Point2 uncalibrate(const Point2& p,
109  OptionalJacobian<2, 10> Dcal = {},
110  OptionalJacobian<2, 2> Dp = {}) const;
111 
113  Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = {},
114  OptionalJacobian<2, 2> Dp = {}) const;
115 
117  Point2 spaceToNPlane(const Point2& p) const;
118 
120  Point2 nPlaneToSpace(const Point2& p) const;
121 
125 
127  Cal3Unified retract(const Vector& d) const;
128 
130  Vector localCoordinates(const Cal3Unified& T2) const;
131 
133  size_t dim() const override { return Dim(); }
134 
136  inline static size_t Dim() { return dimension; }
137 
139 
140  private:
141 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
142 
143  friend class boost::serialization::access;
144  template <class Archive>
145  void serialize(Archive& ar, const unsigned int /*version*/) {
146  ar& boost::serialization::make_nvp(
147  "Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
148  ar& BOOST_SERIALIZATION_NVP(xi_);
149  }
150 #endif
151 };
152 
153 template <>
154 struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
155 
156 template <>
157 struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
158 }
gtsam::Cal3Unified::xi
double xi() const
mirror parameter
Definition: Cal3Unified.h:96
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.cal
cal
Definition: test_ProjectionFactorRollingShutter.py:27
gtsam::Cal3Unified::Cal3Unified
Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1=0.0, double p2=0.0, double xi=0.0)
Definition: Cal3Unified.h:64
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
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
fx
const double fx
Definition: testSmartStereoFactor_iSAM2.cpp:47
gtsam::Cal3DS2_Base
Calibration of a camera with radial distortion.
Definition: Cal3DS2_Base.h:42
gtsam::utils.numerical_derivative.retract
def retract(a, np.ndarray xi)
Definition: numerical_derivative.py:44
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
gtsam::Cal3Unified::Cal3Unified
Cal3Unified(const Vector10 &v)
Definition: Cal3Unified.h:74
simple::p2
static Point3 p2
Definition: testInitializePose3.cpp:51
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
k1
double k1(double x)
Definition: k1.c:133
gtsam::Cal3Unified::dim
size_t dim() const override
Return dimensions of calibration manifold object.
Definition: Cal3Unified.h:133
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
p1
Vector3f p1
Definition: MatrixBase_all.cpp:2
gtsam::equals
Definition: Testable.h:112
u0
static const double u0
Definition: testCal3DFisheye.cpp:31
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::Cal3Unified
Calibration of a omni-directional camera with mirror + lens radial distortion.
Definition: Cal3Unified.h:45
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Cal3::shared_ptr
std::shared_ptr< Cal3 > shared_ptr
Definition: Cal3.h:78
Cal3DS2_Base.h
gtsam::Cal3Unified::~Cal3Unified
~Cal3Unified() override
Definition: Cal3Unified.h:68
gtsam::Cal3Unified::Dim
static size_t Dim()
Return dimensions of calibration manifold object.
Definition: Cal3Unified.h:136
fy
const double fy
Definition: testSmartStereoFactor_iSAM2.cpp:48


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:03