Cal3.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.h>
19 
20 #include <cmath>
21 #include <fstream>
22 #include <iostream>
23 
24 namespace gtsam {
25 
26 /* ************************************************************************* */
27 Cal3::Cal3(double fov, int w, int h)
28  : s_(0), u0_((double)w / 2.0), v0_((double)h / 2.0) {
29  double a = fov * M_PI / 360.0; // fov/2 in radians
30  fx_ = double(w) / (2.0 * tan(a));
31  fy_ = fx_;
32 }
33 
34 /* ************************************************************************* */
35 Cal3::Cal3(const std::string& path) {
36  const auto buffer = path + std::string("/calibration_info.txt");
37  std::ifstream infile(buffer, std::ios::in);
38 
39  if (infile && !infile.eof()) {
40  infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_;
41  } else {
42  throw std::runtime_error("Cal3: Unable to load the calibration");
43  }
44 
45  infile.close();
46 }
47 
48 /* ************************************************************************* */
49 std::ostream& operator<<(std::ostream& os, const Cal3& cal) {
50  os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew()
51  << ", px: " << cal.px() << ", py: " << cal.py();
52  return os;
53 }
54 
55 /* ************************************************************************* */
56 void Cal3::print(const std::string& s) const { gtsam::print((Matrix)K(), s); }
57 
58 /* ************************************************************************* */
59 bool Cal3::equals(const Cal3& K, double tol) const {
60  return (std::fabs(fx_ - K.fx_) < tol && std::fabs(fy_ - K.fy_) < tol &&
61  std::fabs(s_ - K.s_) < tol && std::fabs(u0_ - K.u0_) < tol &&
62  std::fabs(v0_ - K.v0_) < tol);
63 }
64 
65 Matrix3 Cal3::inverse() const {
66  const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
67  Matrix3 K_inverse;
68  K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, 1.0 / fy_,
69  -v0_ / fy_, 0.0, 0.0, 1.0;
70  return K_inverse;
71 }
72 
73 /* ************************************************************************* */
74 
75 } // \ namespace gtsam
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
virtual void print(const std::string &s="") const
print with optional string
Definition: Cal3.cpp:56
double skew() const
skew
Definition: Cal3.h:148
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
#define M_PI
Definition: main.h:106
double u0_
Definition: Cal3.h:73
double py() const
image center in y
Definition: Cal3.h:154
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const Cal3 &cal)
Output stream operator.
Definition: Cal3.cpp:49
Real fabs(const Real &a)
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
double fx() const
focal length x
Definition: Cal3.h:139
double s_
skew
Definition: Cal3.h:72
RealScalar s
Common code for all Calibration models.
Cal3()=default
Create a default calibration that leaves coordinates unchanged.
RowVector3d w
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
const double h
ofstream os("timeSchurFactors.csv")
EIGEN_DEVICE_FUNC const TanReturnType tan() const
const G double tol
Definition: Group.h:86
double fx_
Definition: Cal3.h:71
static double fov
double fy() const
focal length y
Definition: Cal3.h:142
double px() const
image center in x
Definition: Cal3.h:151


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