gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
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>
27#include <gtsam/geometry/Unit3.h>
28
29#include <boost/serialization/nvp.hpp>
30
31namespace gtsam {
32
40class GTSAM_EXPORT EmptyCal {
41 public:
42 enum { dimension = 0 };
43 EmptyCal() {}
44 virtual ~EmptyCal() = default;
45 using shared_ptr = boost::shared_ptr<EmptyCal>;
46
48 inline static size_t Dim() { return dimension; }
49
50 void print(const std::string& s) const {
51 std::cout << "empty calibration: " << s << std::endl;
52 }
53
54 private:
56 friend class boost::serialization::access;
57 template <class Archive>
58 void serialize(Archive& ar, const unsigned int /*version*/) {
59 ar& boost::serialization::make_nvp(
60 "EmptyCal", boost::serialization::base_object<EmptyCal>(*this));
61 }
62};
63
70class GTSAM_EXPORT SphericalCamera {
71 public:
72 enum { dimension = 6 };
73
74 using Measurement = Unit3;
75 using MeasurementVector = std::vector<Unit3>;
77
78 private:
79 Pose3 pose_;
80
81 protected:
82 EmptyCal::shared_ptr emptyCal_;
83
84 public:
87
90 : pose_(Pose3()), emptyCal_(boost::make_shared<EmptyCal>()) {}
91
93 explicit SphericalCamera(const Pose3& pose)
94 : pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
95
97 explicit SphericalCamera(const Pose3& pose,
98 const EmptyCal::shared_ptr& cal)
99 : pose_(pose), emptyCal_(cal) {}
100
104 explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {}
105
107 virtual ~SphericalCamera() = default;
108
110 const EmptyCal::shared_ptr& sharedCalibration() const {
111 return emptyCal_;
112 }
113
115 const EmptyCal& calibration() const { return *emptyCal_; }
116
120
122 bool equals(const SphericalCamera& camera, double tol = 1e-9) const;
123
125 virtual void print(const std::string& s = "SphericalCamera") const;
126
130
132 const Pose3& pose() const { return pose_; }
133
135 const Rot3& rotation() const { return pose_.rotation(); }
136
138 const Point3& translation() const { return pose_.translation(); }
139
140 // /// return pose, with derivative
141 // const Pose3& getPose(OptionalJacobian<6, 6> H) const;
142
146
148 std::pair<Unit3, bool> projectSafe(const Point3& pw) const;
149
155 Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
156 OptionalJacobian<2, 3> Dpoint = boost::none) const;
157
163 Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none,
164 OptionalJacobian<2, 2> Dpoint = boost::none) const;
165
167 Point3 backproject(const Unit3& p, const double depth) const;
168
170 Unit3 backprojectPointAtInfinity(const Unit3& p) const;
171
177 Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none,
178 OptionalJacobian<2, 3> Dpoint = boost::none) const;
179
184 Vector2 reprojectionError(const Point3& point, const Unit3& measured,
185 OptionalJacobian<2, 6> Dpose = boost::none,
186 OptionalJacobian<2, 3> Dpoint = boost::none) const;
188
190 SphericalCamera retract(const Vector6& d) const {
191 return SphericalCamera(pose().retract(d));
192 }
193
195 Vector6 localCoordinates(const SphericalCamera& p) const {
196 return pose().localCoordinates(p.pose());
197 }
198
201 return SphericalCamera(
202 Pose3::Identity()); // assumes that the default constructor is valid
203 }
204
206 Matrix34 cameraProjectionMatrix() const {
207 return Matrix34(pose_.inverse().matrix().block(0, 0, 3, 4));
208 }
209
212 return Eigen::Matrix<double, traits<Point2>::dimension, 1>::Constant(0.0);
213 }
214
216 size_t dim() const { return 6; }
217
219 static size_t Dim() { return 6; }
220
221 private:
223 friend class boost::serialization::access;
224 template <class Archive>
225 void serialize(Archive& ar, const unsigned int /*version*/) {
226 ar& BOOST_SERIALIZATION_NVP(pose_);
227 }
228
229 public:
231};
232// end of class SphericalCamera
233
234template <>
235struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {};
236
237template <>
238struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {};
239
240} // namespace gtsam
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Base class and basic functions for Manifold types.
Bearing-Range product.
3D Pose
Global functions in a separate testing namespace.
Definition chartTesting.h:28
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition Point3.h:36
gtsam::enable_if_t< needs_eigen_aligned_allocator< T >::value, boost::shared_ptr< T > > make_shared(Args &&... args)
Add our own make_shared as a layer of wrapping on boost::make_shared This solves the problem with the...
Definition make_shared.h:57
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition expressions.h:131
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
Both LieGroupTraits and Testable.
Definition Lie.h:229
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
Template to create a binary predicate.
Definition Testable.h:111
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition Pose3.cpp:308
Matrix4 matrix() const
convert to 4*4 matrix
Definition Pose3.cpp:323
Pose3 inverse() const
inverse transformation with derivatives
Definition Pose3.cpp:49
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition Pose3.cpp:315
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
Empty calibration.
Definition SphericalCamera.h:40
static size_t Dim()
return DOF, dimensionality of tangent space
Definition SphericalCamera.h:48
A spherical camera class that has a Pose3 and measures bearing vectors.
Definition SphericalCamera.h:70
const EmptyCal::shared_ptr & sharedCalibration() const
return shared pointer to calibration
Definition SphericalCamera.h:110
static size_t Dim()
Definition SphericalCamera.h:219
SphericalCamera(const Pose3 &pose)
Constructor with pose.
Definition SphericalCamera.h:93
virtual ~SphericalCamera()=default
Default destructor.
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition SphericalCamera.h:206
Vector6 localCoordinates(const SphericalCamera &p) const
return canonical coordinate
Definition SphericalCamera.h:195
const Rot3 & rotation() const
get rotation
Definition SphericalCamera.h:135
const Pose3 & pose() const
return pose, constant version
Definition SphericalCamera.h:132
const Point3 & translation() const
get translation
Definition SphericalCamera.h:138
SphericalCamera retract(const Vector6 &d) const
move a cameras according to d
Definition SphericalCamera.h:190
const EmptyCal & calibration() const
return calibration
Definition SphericalCamera.h:115
size_t dim() const
Definition SphericalCamera.h:216
static SphericalCamera Identity()
for Canonical
Definition SphericalCamera.h:200
SphericalCamera()
Default constructor.
Definition SphericalCamera.h:89
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition SphericalCamera.h:211
SphericalCamera(const Pose3 &pose, const EmptyCal::shared_ptr &cal)
Constructor with empty intrinsics (needed for smart factors)
Definition SphericalCamera.h:97
Represents a 3D point on a unit sphere.
Definition Unit3.h:43