gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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
26#include <gtsam/geometry/Cal3DS2_Base.h>
27
28namespace gtsam {
29
45class 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 = boost::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 = boost::none,
110 OptionalJacobian<2, 2> Dp = boost::none) const;
111
113 Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
114 OptionalJacobian<2, 2> Dp = boost::none) 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:
142 friend class boost::serialization::access;
143 template <class Archive>
144 void serialize(Archive& ar, const unsigned int /*version*/) {
145 ar& boost::serialization::make_nvp(
146 "Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
147 ar& BOOST_SERIALIZATION_NVP(xi_);
148 }
149};
150
151template <>
152struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
153
154template <>
155struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
156}
Global functions in a separate testing namespace.
Definition chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
Both ManifoldTraits and Testable.
Definition Manifold.h:120
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
Calibration of a camera with radial distortion.
Definition Cal3DS2_Base.h:42
Calibration of a omni-directional camera with mirror + lens radial distortion.
Definition Cal3Unified.h:45
size_t dim() const override
Return dimensions of calibration manifold object.
Definition Cal3Unified.h:133
Cal3Unified()=default
Default Constructor with only unit focal length.
double xi() const
mirror parameter
Definition Cal3Unified.h:96
static size_t Dim()
Return dimensions of calibration manifold object.
Definition Cal3Unified.h:136