Go to the documentation of this file.
21 using namespace gtsam;
26 StereoCamera::StereoCamera(
const Pose3& leftCamPose,
28 leftCamPose_(leftCamPose), K_(
K) {
42 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
49 const double fx =
K.fx(),
fy =
K.fy(),
b =
K.baseline();
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;
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;
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;
86 throw std::runtime_error(
87 "StereoCamera::project does not support third derivative - BTW use project2");
105 const double fx =
K.fx(),
fy =
K.fy(),
cx =
K.px(),
cy =
K.py(),
b =
K.baseline();
107 double uL =
z.uL(), uR =
z.uR(),
v =
z.v();
108 double disparity = uL - uR;
110 double local_z =
b *
fx / disparity;
111 const Point3 local(local_z * (uL -
cx)/
fx, local_z * (
v -
cy) /
fy, local_z);
114 double z_partial_uR = local_z/disparity;
115 double x_partial_uR = local.x()/disparity;
116 double y_partial_uR = local.y()/disparity;
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;
122 Matrix3 D_point_local;
126 *H2 = D_point_local * D_local_z;
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
static const double d[K][N]
Cal3_S2Stereo::shared_ptr K_
GaussianFactorGraphValuePair Y
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
StereoPoint2 project2(const Point3 &point, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
EIGEN_DEVICE_FUNC const Scalar & q
Point3 backproject(const StereoPoint2 &z) const
back-project a measurement
std::shared_ptr< Cal3_S2Stereo > shared_ptr
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Point3 backproject2(const StereoPoint2 &z, OptionalJacobian< 3, 6 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
The most common 5DOF 3D->2D calibration, stereo version.
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
Array< int, Dynamic, 1 > v
StereoPoint2 project(const Point3 &point) const
Project 3D point to StereoPoint2 (uL,uR,v)
Rot2 R(Rot2::fromAngle(0.1))
A Stereo Camera based on two Simple Cameras.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:36:25