CalibratedCamera.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 #include <gtsam/geometry/Pose2.h>
21 
22 using namespace std;
23 
24 namespace gtsam {
25 
26 /* ************************************************************************* */
27 Matrix26 PinholeBase::Dpose(const Point2& pn, double d) {
28  // optimized version of derivatives, see CalibratedCamera.nb
29  const double u = pn.x(), v = pn.y();
30  double uv = u * v, uu = u * u, vv = v * v;
31  Matrix26 Dpn_pose;
32  Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
33  return Dpn_pose;
34 }
35 
36 /* ************************************************************************* */
37 Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& Rt) {
38  // optimized version of derivatives, see CalibratedCamera.nb
39  const double u = pn.x(), v = pn.y();
40  Matrix23 Dpn_point;
41  Dpn_point << //
42  Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), //
43  Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2);
44  Dpn_point *= d;
45  return Dpn_point;
46 }
47 
48 /* ************************************************************************* */
49 Pose3 PinholeBase::LevelPose(const Pose2& pose2, double height) {
50  const double st = sin(pose2.theta()), ct = cos(pose2.theta());
51  const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
52  const Rot3 wRc(x, y, z);
53  const Point3 t(pose2.x(), pose2.y(), height);
54  return Pose3(wRc, t);
55 }
56 
57 /* ************************************************************************* */
58 Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target,
59  const Point3& upVector) {
60  Point3 zc = target - eye;
61  zc = zc / zc.norm();
62  Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
63  xc = xc / xc.norm();
64  Point3 yc = zc.cross(xc);
65  return Pose3(Rot3(xc, yc, zc), eye);
66 }
67 
68 /* ************************************************************************* */
69 bool PinholeBase::equals(const PinholeBase &camera, double tol) const {
70  return pose_.equals(camera.pose(), tol);
71 }
72 
73 /* ************************************************************************* */
74 void PinholeBase::print(const string& s) const {
75  pose_.print(s + ".pose");
76 }
77 
78 /* ************************************************************************* */
80  if (H) {
81  H->setZero();
82  H->block(0, 0, 6, 6) = I_6x6;
83  }
84  return pose_;
85 }
86 
87 /* ************************************************************************* */
89  double d = 1.0 / pc.z();
90  const double u = pc.x() * d, v = pc.y() * d;
91  if (Dpoint)
92  *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
93  return Point2(u, v);
94 }
95 
96 /* ************************************************************************* */
98  if (Dpoint) {
99  Matrix32 Dpoint3_pc;
100  Matrix23 Duv_point3;
101  Point2 uv = Project(pc.point3(Dpoint3_pc), Duv_point3);
102  *Dpoint = Duv_point3 * Dpoint3_pc;
103  return uv;
104  } else
105  return Project(pc.point3());
106 }
107 
108 /* ************************************************************************* */
109 pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const {
110  const Point3 pc = pose().transformTo(pw);
111  const Point2 pn = Project(pc);
112  return make_pair(pn, pc.z() > 0);
113 }
114 
115 /* ************************************************************************* */
117  OptionalJacobian<2, 3> Dpoint) const {
118 
119  Matrix3 Rt; // calculated by transformTo if needed
120  const Point3 q = pose().transformTo(point, {}, Dpoint ? &Rt : 0);
121 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
122  if (q.z() <= 0)
123  throw CheiralityException();
124 #endif
125  const Point2 pn = Project(q);
126 
127  if (Dpose || Dpoint) {
128  const double d = 1.0 / q.z();
129  if (Dpose)
130  *Dpose = PinholeBase::Dpose(pn, d);
131  if (Dpoint)
132  *Dpoint = PinholeBase::Dpoint(pn, d, Rt);
133  }
134  return pn;
135 }
136 
137 /* ************************************************************************* */
139  OptionalJacobian<2, 2> Dpoint) const {
140 
141  // world to camera coordinate
142  Matrix23 Dpc_rot;
143  Matrix2 Dpc_point;
144  const Unit3 pc = pose().rotation().unrotate(pw, Dpose ? &Dpc_rot : 0,
145  Dpoint ? &Dpc_point : 0);
146 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
147  if (pc.unitVector().z() <= 0)
148  throw CheiralityException();
149 #endif
150  // camera to normalized image coordinate
151  Matrix2 Dpn_pc;
152  const Point2 pn = Project(pc, Dpose || Dpoint ? &Dpn_pc : 0);
153 
154  // chain the Jacobian matrices
155  if (Dpose) {
156  // only rotation is important
157  Matrix26 Dpc_pose;
158  Dpc_pose.setZero();
159  Dpc_pose.leftCols<3>() = Dpc_rot;
160  *Dpose = Dpn_pc * Dpc_pose; // 2x2 * 2x6
161  }
162  if (Dpoint)
163  *Dpoint = Dpn_pc * Dpc_point; // 2x2 * 2*2
164  return pn;
165 }
166 /* ************************************************************************* */
168  const double depth, OptionalJacobian<3, 2> Dpoint, OptionalJacobian<3, 1> Ddepth) {
169  if (Dpoint)
170  *Dpoint << depth, 0, 0, depth, 0, 0;
171  if (Ddepth)
172  *Ddepth << p.x(), p.y(), 1;
173  return Point3(p.x() * depth, p.y() * depth, depth);
174 }
175 
176 /* ************************************************************************* */
177 CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
178  return CalibratedCamera(LevelPose(pose2, height));
179 }
180 
181 /* ************************************************************************* */
182 CalibratedCamera CalibratedCamera::Lookat(const Point3& eye,
183  const Point3& target, const Point3& upVector) {
184  return CalibratedCamera(LookatPose(eye, target, upVector));
185 }
186 
187 /* ************************************************************************* */
189  OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
190  return project2(point, Dcamera, Dpoint);
191 }
192 
193 /* ************************************************************************* */
194 CalibratedCamera CalibratedCamera::retract(const Vector& d) const {
195  return CalibratedCamera(pose().retract(d));
196 }
197 
198 /* ************************************************************************* */
199 Vector CalibratedCamera::localCoordinates(const CalibratedCamera& T2) const {
200  return pose().localCoordinates(T2.pose());
201 }
202 
203 /* ************************************************************************* */
204 }
static Point2 project2(const CalibratedCamera &camera, const Point3 &point)
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Pose3_ getPose(const Expression< PinholeCamera< CALIBRATION > > &cam)
Scalar * y
static Point3 BackprojectFromCamera(const CalibratedCamera &camera, const Point2 &point, const double &depth)
int RealScalar int RealScalar int RealScalar * pc
static double depth
Vector2 Point2
Definition: Point2.h:32
Vector3 unitVector(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Vector.
Definition: Unit3.cpp:151
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Definition: BFloat16.h:88
static const Pose3 T2(Rot3::Rodrigues(0.3, 0.2, 0.1), P2)
EIGEN_STRONG_INLINE Packet4f print(const Packet4f &a)
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 y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
double y() const
get y
Definition: Pose2.h:249
static const VectorValues vv
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Eigen::VectorXd Vector
Definition: Vector.h:38
Point3 cross(const Point3 &p, const Point3 &q, OptionalJacobian< 3, 3 > H1, OptionalJacobian< 3, 3 > H2)
cross product
Definition: Point3.cpp:64
const Pose3 & pose() const
return pose, constant version
double theta() const
get theta
Definition: Pose2.h:252
Array< int, Dynamic, 1 > v
RealScalar s
EIGEN_DEVICE_FUNC const Scalar & q
Calibrated camera for which only pose is unknown.
traits
Definition: chartTesting.h:28
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
float * p
Point2(* Project)(const Point3 &, OptionalJacobian< 2, 3 >)
static Pose3 pose2
Point3 point3(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Point3.
Definition: Unit3.cpp:144
const G double tol
Definition: Group.h:86
Vector3 Point3
Definition: Point3.h:38
2D Pose
static const CalibratedCamera camera(kDefaultPose)
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 x() const
get x
Definition: Pose2.h:246
Point2 t(10, 10)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:00