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 
57 
59  Cal3Unified() = default;
60 
61  Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1,
62  double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0)
63  : Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
64 
65  ~Cal3Unified() override {}
66 
70 
71  Cal3Unified(const Vector10& v)
72  : Base(v(0), v(1), v(2), v(3), v(4), v(5), v(6), v(7), v(8)), xi_(v(9)) {}
73 
77 
79  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
80  const Cal3Unified& cal);
81 
83  void print(const std::string& s = "") const override;
84 
86  bool equals(const Cal3Unified& K, double tol = 10e-9) const;
87 
91 
93  inline double xi() const { return xi_; }
94 
96  Vector10 vector() const;
97 
105  Point2 uncalibrate(const Point2& p,
106  OptionalJacobian<2, 10> Dcal = boost::none,
107  OptionalJacobian<2, 2> Dp = boost::none) const;
108 
110  Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
111  OptionalJacobian<2, 2> Dp = boost::none) const;
112 
114  Point2 spaceToNPlane(const Point2& p) const;
115 
117  Point2 nPlaneToSpace(const Point2& p) const;
118 
122 
124  Cal3Unified retract(const Vector& d) const;
125 
127  Vector localCoordinates(const Cal3Unified& T2) const;
128 
130  size_t dim() const override { return Dim(); }
131 
133  inline static size_t Dim() { return dimension; }
134 
136 
137  private:
139  friend class boost::serialization::access;
140  template <class Archive>
141  void serialize(Archive& ar, const unsigned int /*version*/) {
142  ar& boost::serialization::make_nvp(
143  "Cal3Unified", boost::serialization::base_object<Cal3DS2_Base>(*this));
144  ar& BOOST_SERIALIZATION_NVP(xi_);
145  }
146 };
147 
148 template <>
149 struct traits<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
150 
151 template <>
152 struct traits<const Cal3Unified> : public internal::Manifold<Cal3Unified> {};
153 }
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Both ManifoldTraits and Testable.
Definition: Manifold.h:120
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
void serialize(Archive &ar, const unsigned int)
Definition: Cal3Unified.h:141
Vector3f p1
Vector2 Point2
Definition: Point2.h:27
ArrayXcf v
Definition: Cwise_arg.cpp:1
size_t dim() const override
Return dimensions of calibration manifold object.
Definition: Cal3Unified.h:130
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:61
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Pose2 T2(M_PI/2.0, Point2(0.0, 2.0))
~Cal3Unified() override
Definition: Cal3Unified.h:65
const double fy
double xi() const
mirror parameter
Definition: Cal3Unified.h:93
Eigen::VectorXd Vector
Definition: Vector.h:38
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
static const double u0
Vector xi
Definition: testPose2.cpp:150
traits
Definition: chartTesting.h:28
ofstream os("timeSchurFactors.csv")
static size_t Dim()
Return dimensions of calibration manifold object.
Definition: Cal3Unified.h:133
static const double v0
Point2_ uncalibrate(const Expression< CALIBRATION > &K, const Point2_ &xy_hat)
float * p
Cal3Unified(const Vector10 &v)
Definition: Cal3Unified.h:71
static Point3 p2
const G double tol
Definition: Group.h:83
const double fx


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:41:44