Cal3_S2.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 
18 #include <gtsam/geometry/Cal3_S2.h>
19 
20 #include <cmath>
21 #include <fstream>
22 #include <iostream>
23 
24 namespace gtsam {
25 
26 /* ************************************************************************* */
27 std::ostream& operator<<(std::ostream& os, const Cal3_S2& cal) {
28  // Use the base class version since it is identical.
29  os << (Cal3&)cal;
30  return os;
31 }
32 
33 /* ************************************************************************* */
34 void Cal3_S2::print(const std::string& s) const {
35  gtsam::print((Matrix)K(), s);
36 }
37 
38 /* ************************************************************************* */
39 bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
40  return Cal3::equals(K, tol);
41 }
42 
43 /* ************************************************************************* */
45  OptionalJacobian<2, 2> Dp) const {
46  const double x = p.x(), y = p.y();
47  if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
48  if (Dp) *Dp << fx_, s_, 0.0, fy_;
49  return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
50 }
51 
52 /* ************************************************************************* */
54  OptionalJacobian<2, 2> Dp) const {
55  const double u = p.x(), v = p.y();
56  double delta_u = u - u0_, delta_v = v - v0_;
57  double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
58  double inv_fy_delta_v = inv_fy * delta_v;
59  double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
60 
61  Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
62  if (Dcal) {
63  *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
64  -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, -inv_fy * point.y(),
65  0, 0, -inv_fy;
66  }
67  if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
68  return point;
69 }
70 
71 /* ************************************************************************* */
72 Vector3 Cal3_S2::calibrate(const Vector3& p) const { return inverse() * p; }
73 
74 /* ************************************************************************* */
75 
76 } // namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
Scalar * y
bool equals(const Cal3_S2 &K, double tol=10e-9) const
Check if equal up to specified tolerance.
Definition: Cal3_S2.cpp:39
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector2 Point2
Definition: Point2.h:32
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
double u0_
Definition: Cal3.h:73
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3_S2.cpp:53
Common base class for all calibration models.
Definition: Cal3.h:69
Matrix3 inverse() const
Return inverted calibration matrix inv(K)
Definition: Cal3.cpp:65
double fy_
focal length
Definition: Cal3.h:71
bool equals(const Cal3 &K, double tol=10e-9) const
Check if equal up to specified tolerance.
Definition: Cal3.cpp:59
Array< int, Dynamic, 1 > v
double s_
skew
Definition: Cal3.h:72
RealScalar s
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
double v0_
principal point
Definition: Cal3.h:73
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
traits
Definition: chartTesting.h:28
ofstream os("timeSchurFactors.csv")
float * p
const G double tol
Definition: Group.h:86
double fx_
Definition: Cal3.h:71
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
void print(const std::string &s="Cal3_S2") const override
print with optional string
Definition: Cal3_S2.cpp:34
The most common 5DOF 3D->2D calibration.
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3_S2.cpp:44


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:33:59