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
gtsam::Cal3::fx_
double fx_
Definition: Cal3.h:71
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.cal
cal
Definition: test_ProjectionFactorRollingShutter.py:27
s
RealScalar s
Definition: level1_cplx_impl.h:126
gtsam::operator<<
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
Definition: testGroup.cpp:109
gtsam::Cal3_S2::calibrate
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3_S2.cpp:53
gtsam::Cal3_S2::uncalibrate
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Definition: Cal3_S2.cpp:44
gtsam::Cal3::inverse
Matrix3 inverse() const
Return inverted calibration matrix inv(K)
Definition: Cal3.cpp:65
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
x
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
Definition: gnuplot_common_settings.hh:12
gtsam::Cal3_S2::equals
bool equals(const Cal3_S2 &K, double tol=10e-9) const
Check if equal up to specified tolerance.
Definition: Cal3_S2.cpp:39
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
os
ofstream os("timeSchurFactors.csv")
gtsam::Cal3::u0_
double u0_
Definition: Cal3.h:73
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
gtsam::Cal3
Common base class for all calibration models.
Definition: Cal3.h:69
gtsam::Cal3::fy_
double fy_
focal length
Definition: Cal3.h:71
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::Cal3::K
virtual Matrix3 K() const
return calibration matrix K
Definition: Cal3.h:167
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam::Cal3::equals
bool equals(const Cal3 &K, double tol=10e-9) const
Check if equal up to specified tolerance.
Definition: Cal3.cpp:59
gtsam
traits
Definition: SFMdata.h:40
K
#define K
Definition: igam.h:8
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Cal3_S2::print
void print(const std::string &s="Cal3_S2") const override
print with optional string
Definition: Cal3_S2.cpp:34
gtsam::Cal3::s_
double s_
skew
Definition: Cal3.h:72
gtsam::Cal3::v0_
double v0_
principal point
Definition: Cal3.h:73


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