Cal3Unified.cpp
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 
19 #include <gtsam/base/Matrix.h>
20 #include <gtsam/base/Vector.h>
22 #include <gtsam/geometry/Point2.h>
23 
24 #include <cmath>
25 
26 namespace gtsam {
27 
28 /* ************************************************************************* */
29 Vector10 Cal3Unified::vector() const {
30  Vector10 v;
31  v << Base::vector(), xi_;
32  return v;
33 }
34 
35 /* ************************************************************************* */
36 std::ostream& operator<<(std::ostream& os, const Cal3Unified& cal) {
37  os << (Cal3DS2_Base&)cal;
38  os << ", xi: " << cal.xi();
39  return os;
40 }
41 
42 /* ************************************************************************* */
43 void Cal3Unified::print(const std::string& s) const {
44  Base::print(s);
45  gtsam::print((Vector)(Vector(1) << xi_).finished(), s + ".xi");
46 }
47 
48 /* ************************************************************************* */
49 bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
50  const Cal3DS2_Base* base = dynamic_cast<const Cal3DS2_Base*>(&K);
51  return Cal3DS2_Base::equals(*base, tol) && std::fabs(xi_ - K.xi_) < tol;
52 }
53 
54 /* ************************************************************************* */
55 // todo: make a fixed sized jacobian version of this
57  OptionalJacobian<2, 2> Dp) const {
58  // this part of code is modified from Cal3DS2,
59  // since the second part of this model (after project to normalized plane)
60  // is same as Cal3DS2
61 
62  // parameters
63  const double xi = xi_;
64 
65  // Part1: project 3D space to NPlane
66  const double xs = p.x(), ys = p.y(); // normalized points in 3D space
67  const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0);
68  const double xi_sqrt_nx = 1.0 / (1 + xi * sqrt_nx);
69  const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx;
70  const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane
71 
72  // Part2: project NPlane point to pixel plane: use Cal3DS2
73  Point2 m(x, y);
74  Matrix29 H1base;
75  Matrix2 H2base; // jacobians from Base class
76  Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
77 
78  // Inlined derivative for calibration
79  if (Dcal) {
80  // part1
81  Vector2 DU;
82  DU << -xs * sqrt_nx * xi_sqrt_nx2, //
83  -ys * sqrt_nx * xi_sqrt_nx2;
84  *Dcal << H1base, H2base * DU;
85  }
86 
87  // Inlined derivative for points
88  if (Dp) {
89  // part1
90  const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
91  const double mid = -(xi * xs * ys) * denom;
92  Matrix2 DU;
93  DU << (sqrt_nx + xi * (ys * ys + 1)) * denom, mid, //
94  mid, (sqrt_nx + xi * (xs * xs + 1)) * denom;
95 
96  *Dp << H2base * DU;
97  }
98 
99  return puncalib;
100 }
101 
102 /* ************************************************************************* */
104  OptionalJacobian<2, 2> Dp) const {
105  // calibrate point to Nplane use base class::calibrate()
106  Point2 pnplane = Base::calibrate(pi);
107 
108  // call nplane to space
109  Point2 pn = this->nPlaneToSpace(pnplane);
110 
111  calibrateJacobians<Cal3Unified, dimension>(*this, pn, Dcal, Dp);
112 
113  return pn;
114 }
115 /* ************************************************************************* */
117  const double x = p.x(), y = p.y();
118  const double xy2 = x * x + y * y;
119  const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1);
120 
121  return Point2((sq_xy * x / (sq_xy - xi_)), (sq_xy * y / (sq_xy - xi_)));
122 }
123 
124 /* ************************************************************************* */
126  const double x = p.x(), y = p.y();
127  const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1);
128 
129  return Point2((x / sq_xy), (y / sq_xy));
130 }
131 
132 /* ************************************************************************* */
134  return Cal3Unified(vector() + d);
135 }
136 
137 /* ************************************************************************* */
139  return T2.vector() - vector();
140 }
141 
142 /* ************************************************************************* */
143 
144 } // \ namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
Matrix3f m
bool equals(const Cal3Unified &K, double tol=10e-9) const
assert equality up to a tolerance
Definition: Cal3Unified.cpp:49
Scalar * y
Vector2 Point2
Definition: Point2.h:32
void print(const std::string &s="") const override
print with optional string
Definition: Cal3Unified.cpp:43
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 10 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3Unified.cpp:56
Calibration of a camera with radial distortion.
Definition: Cal3DS2_Base.h:42
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
Cal3Unified retract(const Vector &d) const
Given delta vector, update calibration.
Real fabs(const Real &a)
Point2 nPlaneToSpace(const Point2 &p) const
Convert a normalized unit plane point to 3D space.
Eigen::VectorXd Vector
Definition: Vector.h:38
Array< int, Dynamic, 1 > v
double xi() const
mirror parameter
Definition: Cal3Unified.h:96
Vector localCoordinates(const Cal3Unified &T2) const
Given a different calibration, calculate update to obtain it.
Cal3Unified()=default
Default Constructor with only unit focal length.
RealScalar s
Vector9 vector() const
Return all parameters as a vector.
double xi_
mirror parameter
Definition: Cal3Unified.h:50
void print(const std::string &s="") const override
print with optional string
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Convert (distorted) image coordinates uv to intrinsic coordinates xy.
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
traits
Definition: chartTesting.h:28
typedef and functions to augment Eigen&#39;s VectorXd
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 9 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Calibration of a omni-directional camera with mirror + lens radial distortion.
Definition: Cal3Unified.h:45
ofstream os("timeSchurFactors.csv")
bool equals(const Cal3DS2_Base &K, double tol=1e-8) const
assert equality up to a tolerance
float * p
Point2 spaceToNPlane(const Point2 &p) const
Convert a 3D point to normalized unit plane.
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
Annotation indicating that a class derives from another given type.
Definition: attr.h:61
const G double tol
Definition: Group.h:86
Unified Calibration Model, see Mei07icra for details.
Vector10 vector() const
Return all parameters as a vector.
Definition: Cal3Unified.cpp:29
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
2D Point
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 10 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Conver a pixel coordinate to ideal coordinate.
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Cal3Unified &cal)
Output stream operator.
Definition: Cal3Unified.cpp:36


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:00