PinholePose.h
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 
20 #pragma once
21 
23 #include <gtsam/geometry/Point2.h>
24 
25 namespace gtsam {
26 
32 template<typename CALIBRATION>
33 class PinholeBaseK: public PinholeBase {
34 
35 private:
36 
37  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
38 
39  // Get dimensions of calibration type at compile time
40  static const int DimK = FixedDimension<CALIBRATION>::value;
41 
42 public:
43 
44  typedef CALIBRATION CalibrationType;
45 
48 
51  }
52 
54  explicit PinholeBaseK(const Pose3& pose) :
55  PinholeBase(pose) {
56  }
57 
61 
62  explicit PinholeBaseK(const Vector& v) : PinholeBase(v) {}
63 
67 
69  virtual const CALIBRATION& calibration() const = 0;
70 
74 
76  std::pair<Point2, bool> projectSafe(const Point3& pw) const {
77  std::pair<Point2, bool> pn = PinholeBase::projectSafe(pw);
78  pn.first = calibration().uncalibrate(pn.first);
79  return pn;
80  }
81 
82 
89  template <class POINT>
92  OptionalJacobian<2, DimK> Dcal) const {
93 
94  // project to normalized coordinates
95  const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
96 
97  // uncalibrate to pixel coordinates
98  Matrix2 Dpi_pn;
99  const Point2 pi = calibration().uncalibrate(pn, Dcal,
100  Dpose || Dpoint ? &Dpi_pn : 0);
101 
102  // If needed, apply chain rule
103  if (Dpose)
104  *Dpose = Dpi_pn * *Dpose;
105  if (Dpoint)
106  *Dpoint = Dpi_pn * *Dpoint;
107 
108  return pi;
109  }
110 
114  OptionalJacobian<2, DimK> Dcal = {}) const {
115  return _project(pw, Dpose, Dpoint, Dcal);
116  }
117 
121  OptionalJacobian<2, DimK> Dcal = {}) const {
122  return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured);
123  }
124 
128  OptionalJacobian<2, DimK> Dcal = {}) const {
129  return _project(pw, Dpose, Dpoint, Dcal);
130  }
131 
133  Point3 backproject(const Point2& p, double depth,
134  OptionalJacobian<3, 6> Dresult_dpose = {},
135  OptionalJacobian<3, 2> Dresult_dp = {},
136  OptionalJacobian<3, 1> Dresult_ddepth = {},
137  OptionalJacobian<3, DimK> Dresult_dcal = {}) const {
138  typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
139  Matrix2K Dpn_dcal;
140  Matrix22 Dpn_dp;
141  const Point2 pn = calibration().calibrate(p, Dresult_dcal ? &Dpn_dcal : 0,
142  Dresult_dp ? &Dpn_dp : 0);
143  Matrix32 Dpoint_dpn;
144  Matrix31 Dpoint_ddepth;
146  (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0,
147  Dresult_ddepth ? &Dpoint_ddepth : 0);
148  Matrix33 Dresult_dpoint;
149  const Point3 result = pose().transformFrom(point, Dresult_dpose,
150  (Dresult_ddepth ||
151  Dresult_dp ||
152  Dresult_dcal) ? &Dresult_dpoint : 0);
153  if (Dresult_dcal)
154  *Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal; // (3x3)*(3x2)*(2xDimK)
155  if (Dresult_dp)
156  *Dresult_dp = Dresult_dpoint * Dpoint_dpn * Dpn_dp; // (3x3)*(3x2)*(2x2)
157  if (Dresult_ddepth)
158  *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; // (3x3)*(3x1)
159 
160  return result;
161  }
162 
165  const Point2 pn = calibration().calibrate(p);
166  const Unit3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
167  return pose().rotation().rotate(pc);
168  }
169 
175  double range(const Point3& point,
176  OptionalJacobian<1, 6> Dcamera = {},
177  OptionalJacobian<1, 3> Dpoint = {}) const {
178  return pose().range(point, Dcamera, Dpoint);
179  }
180 
186  double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = {},
187  OptionalJacobian<1, 6> Dpose = {}) const {
188  return this->pose().range(pose, Dcamera, Dpose);
189  }
190 
197  {}, OptionalJacobian<1, 6> Dother = {}) const {
198  return pose().range(camera.pose(), Dcamera, Dother);
199  }
200 
206  template<class CalibrationB>
208  OptionalJacobian<1, 6> Dcamera = {},
209  OptionalJacobian<1, 6> Dother = {}) const {
210  return pose().range(camera.pose(), Dcamera, Dother);
211  }
212 
213 private:
214 
215 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
216 
217  friend class boost::serialization::access;
218  template<class Archive>
219  void serialize(Archive & ar, const unsigned int /*version*/) {
220  ar
221  & boost::serialization::make_nvp("PinholeBase",
222  boost::serialization::base_object<PinholeBase>(*this));
223  }
224 #endif
225 
226 public:
228 };
229 // end of class PinholeBaseK
230 
238 template<typename CALIBRATION>
239 class PinholePose: public PinholeBaseK<CALIBRATION> {
240 
241 private:
242 
244  std::shared_ptr<CALIBRATION> K_;
245 
246 public:
247 
248  enum {
250  };
251 
254 
257  }
258 
260  explicit PinholePose(const Pose3& pose) :
261  Base(pose), K_(new CALIBRATION()) {
262  }
263 
265  PinholePose(const Pose3& pose, const std::shared_ptr<CALIBRATION>& K) :
266  Base(pose), K_(K) {
267  }
268 
272 
280  static PinholePose Level(const std::shared_ptr<CALIBRATION>& K,
281  const Pose2& pose2, double height) {
282  return PinholePose(Base::LevelPose(pose2, height), K);
283  }
284 
286  static PinholePose Level(const Pose2& pose2, double height) {
287  return PinholePose::Level(std::make_shared<CALIBRATION>(), pose2, height);
288  }
289 
299  static PinholePose Lookat(const Point3& eye, const Point3& target,
300  const Point3& upVector, const std::shared_ptr<CALIBRATION>& K =
301  std::make_shared<CALIBRATION>()) {
302  return PinholePose(Base::LookatPose(eye, target, upVector), K);
303  }
304 
308 
310  explicit PinholePose(const Vector &v) :
311  Base(v), K_(new CALIBRATION()) {
312  }
313 
315  PinholePose(const Vector &v, const Vector &K) :
316  Base(v), K_(new CALIBRATION(K)) {
317  }
318 
319  // Init from Pose3 and calibration
320  PinholePose(const Pose3 &pose, const Vector &K) :
321  Base(pose), K_(new CALIBRATION(K)) {
322  }
323 
327 
329  bool equals(const Base &camera, double tol = 1e-9) const {
330  const PinholePose* e = dynamic_cast<const PinholePose*>(&camera);
331  return Base::equals(camera, tol) && K_->equals(e->calibration(), tol);
332  }
333 
335  GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
336  const PinholePose& camera) {
337  os << "{R: " << camera.pose().rotation().rpy().transpose();
338  os << ", t: " << camera.pose().translation().transpose();
339  if (!camera.K_) os << ", K: none";
340  else os << ", K: " << *camera.K_;
341  os << "}";
342  return os;
343  }
344 
346  void print(const std::string& s = "PinholePose") const override {
347  Base::print(s);
348  if (!K_)
349  std::cout << "s No calibration given" << std::endl;
350  else
351  K_->print(s + ".calibration");
352  }
353 
357 
358  ~PinholePose() override {
359  }
360 
362  const std::shared_ptr<CALIBRATION>& sharedCalibration() const {
363  return K_;
364  }
365 
367  const CALIBRATION& calibration() const override {
368  return *K_;
369  }
370 
377  OptionalJacobian<2, 3> Dpoint = {}) const {
378  return Base::project(pw, Dpose, Dpoint);
379  }
380 
383  OptionalJacobian<2, 2> Dpoint = {}) const {
384  return Base::project(pw, Dpose, Dpoint);
385  }
386 
390 
392  size_t dim() const {
393  return 6;
394  }
395 
397  static size_t Dim() {
398  return 6;
399  }
400 
402  PinholePose retract(const Vector6& d) const {
403  return PinholePose(Base::pose().retract(d), K_);
404  }
405 
407  Vector6 localCoordinates(const PinholePose& p) const {
408  return Base::pose().localCoordinates(p.Base::pose());
409  }
410 
413  return PinholePose(); // assumes that the default constructor is valid
414  }
415 
417  Matrix34 cameraProjectionMatrix() const {
418  Matrix34 P = Matrix34(PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4));
419  return K_->K() * P;
420  }
421 
424  return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());
425  }
427 
428 private:
429 
430 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
431 
432  friend class boost::serialization::access;
433  template<class Archive>
434  void serialize(Archive & ar, const unsigned int /*version*/) {
435  ar
436  & boost::serialization::make_nvp("PinholeBaseK",
437  boost::serialization::base_object<Base>(*this));
438  ar & BOOST_SERIALIZATION_NVP(K_);
439  }
440 #endif
441 
442 public:
444 };
445 // end of class PinholePose
446 
447 template<typename CALIBRATION>
448 struct traits<PinholePose<CALIBRATION> > : public internal::Manifold<
449  PinholePose<CALIBRATION> > {
450 };
451 
452 template<typename CALIBRATION>
453 struct traits<const PinholePose<CALIBRATION> > : public internal::Manifold<
454  PinholePose<CALIBRATION> > {
455 };
456 
457 } // \ gtsam
gtsam::PinholePose::Level
static PinholePose Level(const std::shared_ptr< CALIBRATION > &K, const Pose2 &pose2, double height)
Definition: PinholePose.h:280
gtsam::PinholePose::cameraProjectionMatrix
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition: PinholePose.h:417
gtsam::PinholeBaseK
Definition: PinholePose.h:33
gtsam::PinholeBase::Dpoint
static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt)
Definition: CalibratedCamera.cpp:37
gtsam::PinholePose::operator<<
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const PinholePose &camera)
stream operator
Definition: PinholePose.h:335
gtsam::PinholeBase::LookatPose
static Pose3 LookatPose(const Point3 &eye, const Point3 &target, const Point3 &upVector)
Definition: CalibratedCamera.cpp:58
gtsam::PinholeBaseK::backprojectPointAtInfinity
Unit3 backprojectPointAtInfinity(const Point2 &p) const
backproject a 2-dimensional point to a 3-dimensional point at infinity
Definition: PinholePose.h:164
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
gtsam::PinholeBase::BackprojectFromCamera
static Point3 BackprojectFromCamera(const Point2 &p, const double depth, OptionalJacobian< 3, 2 > Dpoint={}, OptionalJacobian< 3, 1 > Ddepth={})
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition: CalibratedCamera.cpp:167
d
static const double d[K][N]
Definition: igam.h:11
measured
Point2 measured(-17, 30)
gtsam::PinholeBase::print
virtual void print(const std::string &s="PinholeBase") const
print
Definition: CalibratedCamera.cpp:74
gtsam::PinholePose::Identity
static PinholePose Identity()
for Canonical
Definition: PinholePose.h:412
simple::pose2
static Pose3 pose2
Definition: testInitializePose3.cpp:58
gtsam::PinholeBaseK::PinholeBaseK
PinholeBaseK(const Vector &v)
Definition: PinholePose.h:62
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
gtsam::PinholePose::PinholePose
PinholePose(const Pose3 &pose, const Vector &K)
Definition: PinholePose.h:320
os
ofstream os("timeSchurFactors.csv")
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
gtsam::PinholePose::PinholePose
PinholePose(const Pose3 &pose, const std::shared_ptr< CALIBRATION > &K)
Definition: PinholePose.h:265
gtsam::PinholeBaseK::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}, OptionalJacobian< 2, DimK > Dcal={}) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:112
block
m m block(1, 0, 2, 2)<< 4
gtsam::Pose3::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself={}, OptionalJacobian< 1, 3 > Hpoint={}) const
Definition: Pose3.cpp:399
Point2.h
2D Point
gtsam::PinholePose::PinholePose
PinholePose(const Vector &v, const Vector &K)
Init from Vector and calibration.
Definition: PinholePose.h:315
gtsam::PinholePose::retract
PinholePose retract(const Vector6 &d) const
move a cameras according to d
Definition: PinholePose.h:402
gtsam::PinholeBase::equals
bool equals(const PinholeBase &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition: CalibratedCamera.cpp:69
gtsam::PinholePose::calibration
const CALIBRATION & calibration() const override
return calibration
Definition: PinholePose.h:367
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::PinholeBaseK::projectSafe
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
Definition: PinholePose.h:76
gtsam::PinholePose::PinholePose
PinholePose()
Definition: PinholePose.h:256
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: Rot3M.cpp:148
gtsam::Pose3
Definition: Pose3.h:37
gtsam::PinholePose::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition: PinholePose.h:423
gtsam::PinholeBaseK::backproject
Point3 backproject(const Point2 &p, double depth, OptionalJacobian< 3, 6 > Dresult_dpose={}, OptionalJacobian< 3, 2 > Dresult_dp={}, OptionalJacobian< 3, 1 > Dresult_ddepth={}, OptionalJacobian< 3, DimK > Dresult_dcal={}) const
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition: PinholePose.h:133
gtsam::PinholePose::PinholePose
PinholePose(const Pose3 &pose)
Definition: PinholePose.h:260
gtsam::PinholeBaseK::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 3 > Dpoint={}) const
Definition: PinholePose.h:175
gtsam::PinholePose::~PinholePose
~PinholePose() override
Definition: PinholePose.h:358
gtsam::Cal3Bundler
Calibration used by Bundler.
Definition: Cal3Bundler.h:32
gtsam::PinholeBase
Definition: CalibratedCamera.h:54
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::PinholePose::K_
std::shared_ptr< CALIBRATION > K_
shared pointer to fixed calibration
Definition: PinholePose.h:244
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition: Lie.h:136
gtsam::PinholePose::print
void print(const std::string &s="PinholePose") const override
print
Definition: PinholePose.h:346
gtsam::CalibratedCamera
Definition: CalibratedCamera.h:251
pc
int RealScalar int RealScalar int RealScalar * pc
Definition: level1_cplx_impl.h:119
gtsam::PinholeBaseK::calibration
virtual const CALIBRATION & calibration() const =0
return calibration
gtsam::PinholePose::project2
Point2 project2(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: PinholePose.h:376
gtsam::PinholeBaseK::_project
Point2 _project(const POINT &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint, OptionalJacobian< 2, DimK > Dcal) const
Definition: PinholePose.h:90
gtsam::PinholeBase::projectSafe
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
Definition: CalibratedCamera.cpp:109
matrix
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Definition: gtsam/3rdparty/Eigen/blas/common.h:110
gtsam::PinholePose::PinholePose
PinholePose(const Vector &v)
Init from 6D vector.
Definition: PinholePose.h:310
gtsam::PinholeBaseK::range
double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 6 > Dother={}) const
Definition: PinholePose.h:196
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
gtsam
traits
Definition: chartTesting.h:28
gtsam::PinholeBaseK::project
Point2 project(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}, OptionalJacobian< 2, DimK > Dcal={}) const
project a point at infinity from world coordinates into the image
Definition: PinholePose.h:126
gtsam::traits
Definition: Group.h:36
K
#define K
Definition: igam.h:8
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::PinholePose::Base
PinholeBaseK< CALIBRATION > Base
base class has 3D pose as private member
Definition: PinholePose.h:243
gtsam::PinholeBaseK::range
double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 6 > Dpose={}) const
Definition: PinholePose.h:186
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
gtsam::PinholeBase::project2
Point2 project2(const Point3 &point, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: CalibratedCamera.cpp:116
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::PinholePose::project2
Point2 project2(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
project2 version for point at infinity
Definition: PinholePose.h:382
gtsam::PinholeBaseK::range
double range(const PinholeBaseK< CalibrationB > &camera, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 6 > Dother={}) const
Definition: PinholePose.h:207
gtsam::PinholePose::dim
size_t dim() const
Definition: PinholePose.h:392
gtsam::PinholePose::sharedCalibration
const std::shared_ptr< CALIBRATION > & sharedCalibration() const
return shared pointer to calibration
Definition: PinholePose.h:362
gtsam::PinholeBaseK::PinholeBaseK
PinholeBaseK(const Pose3 &pose)
Definition: PinholePose.h:54
gtsam::PinholePose::equals
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition: PinholePose.h:329
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition: CalibratedCamera.h:154
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
P
static double P[]
Definition: ellpe.c:68
gtsam::PinholeBaseK::reprojectionError
Point2 reprojectionError(const Point3 &pw, const Point2 &measured, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}, OptionalJacobian< 2, DimK > Dcal={}) const
project a 3D point from world coordinates into the image
Definition: PinholePose.h:119
gtsam::tol
const G double tol
Definition: Group.h:79
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
Eigen::Matrix
The matrix class, also used for vectors and row-vectors.
Definition: 3rdparty/Eigen/Eigen/src/Core/Matrix.h:178
gtsam::FixedDimension
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:161
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
camera
static const CalibratedCamera camera(kDefaultPose)
gtsam::PinholePose::Level
static PinholePose Level(const Pose2 &pose2, double height)
PinholePose::level with default calibration.
Definition: PinholePose.h:286
gtsam::PinholePose::dimension
@ dimension
Definition: PinholePose.h:249
depth
static double depth
Definition: testSphericalCamera.cpp:64
gtsam::PinholePose
Definition: PinholePose.h:239
gtsam::PinholeBase::Dpose
static Matrix26 Dpose(const Point2 &pn, double d)
Definition: CalibratedCamera.cpp:27
gtsam::PinholePose::Lookat
static PinholePose Lookat(const Point3 &eye, const Point3 &target, const Point3 &upVector, const std::shared_ptr< CALIBRATION > &K=std::make_shared< CALIBRATION >())
Definition: PinholePose.h:299
GTSAM_CONCEPT_MANIFOLD_TYPE
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T)
Definition: Manifold.h:178
gtsam::PinholeBase::LevelPose
static Pose3 LevelPose(const Pose2 &pose2, double height)
Definition: CalibratedCamera.cpp:49
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279
test_callbacks.value
value
Definition: test_callbacks.py:158
gtsam::PinholePose::localCoordinates
Vector6 localCoordinates(const PinholePose &p) const
return canonical coordinate
Definition: PinholePose.h:407
gtsam::PinholePose::Dim
static size_t Dim()
Definition: PinholePose.h:397
gtsam::PinholeBaseK::DimK
static const int DimK
Definition: PinholePose.h:40
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Thu Jun 13 2024 03:04:19