StereoCamera.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 using namespace std;
21 using namespace gtsam;
22 
23 namespace gtsam {
24 
25  /* ************************************************************************* */
26  StereoCamera::StereoCamera(const Pose3& leftCamPose,
28  leftCamPose_(leftCamPose), K_(K) {
29  }
30 
31  /* ************************************************************************* */
33  return project2(point);
34  }
35 
36  /* ************************************************************************* */
39 
41 
42 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
43  if (q.z() <= 0)
45 #endif
46 
47  // get calibration
48  const Cal3_S2Stereo& K = *K_;
49  const double fx = K.fx(), fy = K.fy(), b = K.baseline();
50 
51  // calculate scaled but not translated image coordinates
52  const double d = 1.0 / q.z();
53  const double x = q.x(), y = q.y();
54  const double dfx = d*fx, dfy = d*fy;
55  const double uL = dfx*x;
56  const double uR = dfx*(x - b);
57  const double v = dfy*y;
58 
59  // check if derivatives need to be computed
60  if (H1 || H2) {
61  // optimized version, see StereoCamera.nb
62  if (H1) {
63  const double v1 = v/fy, v2 = fx*v1, dx=d*x;
64  *H1 << uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL,
65  uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR,
66  fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v;
67  }
68  if (H2) {
69  const Matrix3 R(leftCamPose_.rotation().matrix());
70  *H2 << fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL,
71  fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR,
72  fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v;
73  *H2 << d * (*H2);
74  }
75  }
76 
77  // finally translate
78  return StereoPoint2(K.px() + uL, K.px() + uR, K.py() + v);
79  }
80 
81  /* ************************************************************************* */
84  OptionalJacobian<3,0> H3) const {
85  if (H3)
86  throw std::runtime_error(
87  "StereoCamera::project does not support third derivative - BTW use project2");
88  return project2(point,H1,H2);
89  }
90 
91  /* ************************************************************************* */
93  Vector measured = z.vector();
94  double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]);
95  double X = Z * (measured[0] - K_->px()) / K_->fx();
96  double Y = Z * (measured[2] - K_->py()) / K_->fy();
98  return point;
99  }
100 
101  /* ************************************************************************* */
103  OptionalJacobian<3, 3> H2) const {
104  const Cal3_S2Stereo& K = *K_;
105  const double fx = K.fx(), fy = K.fy(), cx = K.px(), cy = K.py(), b = K.baseline();
106 
107  double uL = z.uL(), uR = z.uR(), v = z.v();
108  double disparity = uL - uR;
109 
110  double local_z = b * fx / disparity;
111  const Point3 local(local_z * (uL - cx)/ fx, local_z * (v - cy) / fy, local_z);
112 
113  if(H1 || H2) {
114  double z_partial_uR = local_z/disparity;
115  double x_partial_uR = local.x()/disparity;
116  double y_partial_uR = local.y()/disparity;
117  Matrix3 D_local_z;
118  D_local_z << -x_partial_uR + local.z()/fx, x_partial_uR, 0,
119  -y_partial_uR, y_partial_uR, local.z() / fy,
120  -z_partial_uR, z_partial_uR, 0;
121 
122  Matrix3 D_point_local;
123  const Point3 point = leftCamPose_.transformFrom(local, H1, D_point_local);
124 
125  if(H2) {
126  *H2 = D_point_local * D_local_z;
127  }
128 
129  return point;
130  }
131 
132  return leftCamPose_.transformFrom(local);
133  }
134 
135 }
cy
const double cy
Definition: testSmartStereoFactor_iSAM2.cpp:50
gtsam::Pose3::transformTo
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in world coordinates and transforms it to Pose coordinates
Definition: Pose3.cpp:371
d
static const double d[K][N]
Definition: igam.h:11
gtsam::StereoCamera::K_
Cal3_S2Stereo::shared_ptr K_
Definition: StereoCamera.h:60
fx
const double fx
Definition: testSmartStereoFactor_iSAM2.cpp:47
gtsam::Y
GaussianFactorGraphValuePair Y
Definition: HybridGaussianProductFactor.cpp:29
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
measured
Point2 measured(-17, 30)
X
#define X
Definition: icosphere.cpp:20
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::StereoCamera::leftCamPose_
Pose3 leftCamPose_
Definition: StereoCamera.h:59
gtsam::StereoCamera::project2
StereoPoint2 project2(const Point3 &point, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: StereoCamera.cpp:37
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam::Pose3
Definition: Pose3.h:37
gtsam::StereoCamera::backproject
Point3 backproject(const StereoPoint2 &z) const
back-project a measurement
Definition: StereoCamera.cpp:92
gtsam::Cal3_S2Stereo::shared_ptr
std::shared_ptr< Cal3_S2Stereo > shared_ptr
Definition: Cal3_S2Stereo.h:38
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
y
Scalar * y
Definition: level1_cplx_impl.h:124
gtsam::b
const G & b
Definition: Group.h:79
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
gtsam::StereoCamera::backproject2
Point3 backproject2(const StereoPoint2 &z, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: StereoCamera.cpp:102
gtsam
traits
Definition: SFMdata.h:40
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
K
#define K
Definition: igam.h:8
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::StereoPoint2
Definition: StereoPoint2.h:34
std
Definition: BFloat16.h:88
gtsam::Pose3::transformFrom
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in Pose coordinates and transforms it to world coordinates
Definition: Pose3.cpp:347
v2
Vector v2
Definition: testSerializationBase.cpp:39
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::StereoCamera::project
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v)
Definition: StereoCamera.cpp:32
gtsam::StereoCheiralityException
Definition: StereoCamera.h:26
cx
const double cx
Definition: testSmartStereoFactor_iSAM2.cpp:49
Z
#define Z
Definition: icosphere.cpp:21
R
Rot2 R(Rot2::fromAngle(0.1))
v1
Vector v1
Definition: testSerializationBase.cpp:38
fy
const double fy
Definition: testSmartStereoFactor_iSAM2.cpp:48
StereoCamera.h
A Stereo Camera based on two Simple Cameras.


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