SphericalCamera.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 
19 #pragma once
20 
21 #include <gtsam/base/Manifold.h>
23 #include <gtsam/base/concepts.h>
24 #include <gtsam/dllexport.h>
26 #include <gtsam/geometry/Pose3.h>
27 #include <gtsam/geometry/Unit3.h>
28 
29 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
30 #include <boost/serialization/nvp.hpp>
31 #endif
32 
33 namespace gtsam {
34 
42 class GTSAM_EXPORT EmptyCal {
43  public:
44  enum { dimension = 0 };
45  EmptyCal() {}
46  virtual ~EmptyCal() = default;
47  using shared_ptr = std::shared_ptr<EmptyCal>;
48 
50  inline static size_t Dim() { return dimension; }
51 
52  void print(const std::string& s) const {
53  std::cout << "empty calibration: " << s << std::endl;
54  }
55 
56  private:
57 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
58  friend class boost::serialization::access;
60  template <class Archive>
61  void serialize(Archive& ar, const unsigned int /*version*/) {
62  ar& boost::serialization::make_nvp(
63  "EmptyCal", boost::serialization::base_object<EmptyCal>(*this));
64  }
65 #endif
66 };
67 
74 class GTSAM_EXPORT SphericalCamera {
75  public:
76  enum { dimension = 6 };
77 
78  using Measurement = Unit3;
79  using MeasurementVector = std::vector<Unit3>;
81 
82  private:
84 
85  protected:
87 
88  public:
91 
94  : pose_(Pose3()), emptyCal_(std::make_shared<EmptyCal>()) {}
95 
97  explicit SphericalCamera(const Pose3& pose)
98  : pose_(pose), emptyCal_(std::make_shared<EmptyCal>()) {}
99 
101  explicit SphericalCamera(const Pose3& pose,
102  const EmptyCal::shared_ptr& cal)
103  : pose_(pose), emptyCal_(cal) {}
104 
108  explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {}
109 
111  virtual ~SphericalCamera() = default;
112 
115  return emptyCal_;
116  }
117 
119  const EmptyCal& calibration() const { return *emptyCal_; }
120 
124 
126  bool equals(const SphericalCamera& camera, double tol = 1e-9) const;
127 
129  virtual void print(const std::string& s = "SphericalCamera") const;
130 
134 
136  const Pose3& pose() const { return pose_; }
137 
139  const Rot3& rotation() const { return pose_.rotation(); }
140 
142  const Point3& translation() const { return pose_.translation(); }
143 
144  // /// return pose, with derivative
145  // const Pose3& getPose(OptionalJacobian<6, 6> H) const;
146 
150 
152  std::pair<Unit3, bool> projectSafe(const Point3& pw) const;
153 
159  Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = {},
160  OptionalJacobian<2, 3> Dpoint = {}) const;
161 
167  Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = {},
168  OptionalJacobian<2, 2> Dpoint = {}) const;
169 
171  Point3 backproject(const Unit3& p, const double depth) const;
172 
174  Unit3 backprojectPointAtInfinity(const Unit3& p) const;
175 
181  Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = {},
182  OptionalJacobian<2, 3> Dpoint = {}) const;
183 
188  Vector2 reprojectionError(const Point3& point, const Unit3& measured,
189  OptionalJacobian<2, 6> Dpose = {},
190  OptionalJacobian<2, 3> Dpoint = {}) const;
192 
194  SphericalCamera retract(const Vector6& d) const {
195  return SphericalCamera(pose().retract(d));
196  }
197 
199  Vector6 localCoordinates(const SphericalCamera& p) const {
200  return pose().localCoordinates(p.pose());
201  }
202 
205  return SphericalCamera(
206  Pose3::Identity()); // assumes that the default constructor is valid
207  }
208 
210  Matrix34 cameraProjectionMatrix() const {
211  return Matrix34(pose_.inverse().matrix().block(0, 0, 3, 4));
212  }
213 
216  return Eigen::Matrix<double, traits<Point2>::dimension, 1>::Constant(0.0);
217  }
218 
220  size_t dim() const { return 6; }
221 
223  static size_t Dim() { return 6; }
224 
225  private:
226 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
227 
228  friend class boost::serialization::access;
229  template <class Archive>
230  void serialize(Archive& ar, const unsigned int /*version*/) {
231  ar& BOOST_SERIALIZATION_NVP(pose_);
232  }
233 #endif
234 
235  public:
237 };
238 // end of class SphericalCamera
239 
240 template <>
241 struct traits<SphericalCamera> : public internal::Manifold<SphericalCamera> {};
242 
243 template <>
244 struct traits<const SphericalCamera> : public internal::Manifold<SphericalCamera> {};
245 
246 } // namespace gtsam
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.cal
cal
Definition: test_ProjectionFactorRollingShutter.py:27
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
gtsam::EmptyCal::EmptyCal
EmptyCal()
Definition: SphericalCamera.h:45
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
d
static const double d[K][N]
Definition: igam.h:11
gtsam::SphericalCamera::retract
SphericalCamera retract(const Vector6 &d) const
move a cameras according to d
Definition: SphericalCamera.h:194
gtsam::EmptyCal
Definition: SphericalCamera.h:42
gtsam::SphericalCamera::translation
const Point3 & translation() const
get translation
Definition: SphericalCamera.h:142
gtsam::SphericalCamera::calibration
const EmptyCal & calibration() const
return calibration
Definition: SphericalCamera.h:119
gtsam::SphericalCamera
Definition: SphericalCamera.h:74
measured
Point2 measured(-17, 30)
gtsam::utils.numerical_derivative.retract
def retract(a, np.ndarray xi)
Definition: numerical_derivative.py:44
gtsam::SphericalCamera::cameraProjectionMatrix
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition: SphericalCamera.h:210
concepts.h
gtsam::SphericalCamera::sharedCalibration
const EmptyCal::shared_ptr & sharedCalibration() const
return shared pointer to calibration
Definition: SphericalCamera.h:114
gtsam::internal::Manifold
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
backproject
static Point3 backproject(const Pose3 &pose, const Point2 &point, const double &depth)
Definition: testCalibratedCamera.cpp:207
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::SphericalCamera::dim
size_t dim() const
Definition: SphericalCamera.h:220
gtsam::Pose3::Identity
static Pose3 Identity()
identity for group operation
Definition: Pose3.h:106
gtsam::Pose3::matrix
Matrix4 matrix() const
Definition: Pose3.cpp:323
gtsam::SphericalCamera::Dim
static size_t Dim()
Definition: SphericalCamera.h:223
Unit3.h
gtsam::project2
Point2_ project2(const Expression< CAMERA > &camera_, const Expression< POINT > &p_)
Definition: slam/expressions.h:151
gtsam::SphericalCamera::localCoordinates
Vector6 localCoordinates(const SphericalCamera &p) const
return canonical coordinate
Definition: SphericalCamera.h:199
gtsam::SphericalCamera::SphericalCamera
SphericalCamera(const Pose3 &pose, const EmptyCal::shared_ptr &cal)
Constructor with empty intrinsics (needed for smart factors)
Definition: SphericalCamera.h:101
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
ThreadsafeException.h
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::Pose3
Definition: Pose3.h:37
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:49
gtsam::make_shared
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, std::shared_ptr< T > > make_shared(Args &&... args)
Definition: make_shared.h:56
gtsam::SphericalCamera::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition: SphericalCamera.h:215
gtsam::SphericalCamera::Identity
static SphericalCamera Identity()
for Canonical
Definition: SphericalCamera.h:204
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
Manifold.h
Base class and basic functions for Manifold types.
Vector2
Definition: test_operator_overloading.cpp:18
gtsam::equals
Definition: Testable.h:112
gtsam::SphericalCamera::MeasurementVector
std::vector< Unit3 > MeasurementVector
Definition: SphericalCamera.h:79
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
gtsam::SphericalCamera::SphericalCamera
SphericalCamera(const Vector &v)
Definition: SphericalCamera.h:108
gtsam
traits
Definition: SFMdata.h:40
gtsam::traits
Definition: Group.h:36
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::SphericalCamera::pose
const Pose3 & pose() const
return pose, constant version
Definition: SphericalCamera.h:136
gtsam::EmptyCal::print
void print(const std::string &s) const
Definition: SphericalCamera.h:52
std
Definition: BFloat16.h:88
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
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::SphericalCamera::emptyCal_
EmptyCal::shared_ptr emptyCal_
Definition: SphericalCamera.h:86
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
BearingRange.h
Bearing-Range product.
camera
static const CalibratedCamera camera(kDefaultPose)
gtsam::SphericalCamera::pose_
Pose3 pose_
3D pose of camera
Definition: SphericalCamera.h:83
depth
static double depth
Definition: testSphericalCamera.cpp:64
gtsam::SphericalCamera::SphericalCamera
SphericalCamera()
Default constructor.
Definition: SphericalCamera.h:93
gtsam::EmptyCal::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition: SphericalCamera.h:50
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition: slam/expressions.h:131
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:279
gtsam::EmptyCal::shared_ptr
std::shared_ptr< EmptyCal > shared_ptr
Definition: SphericalCamera.h:47
gtsam::SphericalCamera::rotation
const Rot3 & rotation() const
get rotation
Definition: SphericalCamera.h:139
Pose3.h
3D Pose
gtsam::SphericalCamera::SphericalCamera
SphericalCamera(const Pose3 &pose)
Constructor with pose.
Definition: SphericalCamera.h:97


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