Cal3_S2Stereo.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 
20 #include <iostream>
21 
22 namespace gtsam {
23 
24 /* ************************************************************************* */
25 std::ostream& operator<<(std::ostream& os, const Cal3_S2Stereo& cal) {
26  os << (Cal3_S2&)cal;
27  os << ", b: " << cal.baseline();
28  return os;
29 }
30 
31 /* ************************************************************************* */
32 void Cal3_S2Stereo::print(const std::string& s) const {
33  std::cout << s << (s != "" ? " " : "");
34  std::cout << "K: " << (Matrix)K() << std::endl;
35  std::cout << "Baseline: " << b_ << std::endl;
36 }
37 
38 /* ************************************************************************* */
39 bool Cal3_S2Stereo::equals(const Cal3_S2Stereo& other, double tol) const {
40  const Cal3_S2* base = dynamic_cast<const Cal3_S2*>(&other);
41  return (Cal3_S2::equals(*base, tol) &&
42  std::fabs(b_ - other.baseline()) < tol);
43 }
44 
45 /* ************************************************************************* */
47  OptionalJacobian<2, 2> Dp) const {
48  const double x = p.x(), y = p.y();
49  if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, 0.0, y, 0.0, 0.0, 1.0, 0.0;
50  if (Dp) *Dp << fx_, s_, 0.0, fy_;
51  return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
52 }
53 
54 /* ************************************************************************* */
56  OptionalJacobian<2, 2> Dp) const {
57  const double u = p.x(), v = p.y();
58  double delta_u = u - u0_, delta_v = v - v0_;
59  double inv_fx = 1 / fx_, inv_fy = 1 / fy_;
60  double inv_fy_delta_v = inv_fy * delta_v;
61  double inv_fx_s_inv_fy = inv_fx * s_ * inv_fy;
62 
63  Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), inv_fy_delta_v);
64  if (Dcal) {
65  *Dcal << -inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v,
66  -inv_fx * point.y(), -inv_fx, inv_fx_s_inv_fy, 0, 0,
67  -inv_fy * point.y(), 0, 0, -inv_fy, 0;
68  }
69  if (Dp) *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy;
70  return point;
71 }
72 
73 /* ************************************************************************* */
74 
75 } // namespace gtsam
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
Vector2 Point2
Definition: Point2.h:32
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
double u0_
Definition: Cal3.h:73
Matrix3 K() const override
return calibration matrix K, same for left and right
Real fabs(const Real &a)
double b_
Stereo baseline.
Definition: Cal3_S2Stereo.h:32
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
double fy_
focal length
Definition: Cal3.h:71
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
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 6 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
double v0_
principal point
Definition: Cal3.h:73
traits
Definition: chartTesting.h:28
The most common 5DOF 3D->2D calibration + Stereo baseline.
ofstream os("timeSchurFactors.csv")
float * p
void print(const std::string &s="") const override
print with optional string
Annotation indicating that a class derives from another given type.
Definition: attr.h:61
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
double baseline() const
return baseline
bool equals(const Cal3_S2Stereo &other, double tol=10e-9) const
Check if equal up to specified tolerance.
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 6 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const


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