Loading...
Searching...
No Matches
SphericalCamera.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/base/Manifold.h>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/base/concepts.h>
#include <gtsam/dllexport.h>
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
class GTSAM_EXPORT EmptyCal {
public:
enum { dimension = 0 };
EmptyCal() {}
virtual ~EmptyCal() = default;
using shared_ptr = boost::shared_ptr<EmptyCal>;
inline static size_t Dim() { return dimension; }
void print(const std::string& s) const {
std::cout << "empty calibration: " << s << std::endl;
}
private:
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"EmptyCal", boost::serialization::base_object<EmptyCal>(*this));
}
};
class GTSAM_EXPORT SphericalCamera {
public:
enum { dimension = 6 };
using Measurement = Unit3;
using MeasurementVector = std::vector<Unit3>;
using CalibrationType = EmptyCal;
private:
Pose3 pose_;
protected:
EmptyCal::shared_ptr emptyCal_;
public:
SphericalCamera()
: pose_(Pose3()), emptyCal_(boost::make_shared<EmptyCal>()) {}
explicit SphericalCamera(const Pose3& pose)
: pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
explicit SphericalCamera(const Pose3& pose,
const EmptyCal::shared_ptr& cal)
: pose_(pose), emptyCal_(cal) {}
explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {}
virtual ~SphericalCamera() = default;
const EmptyCal::shared_ptr& sharedCalibration() const {
return emptyCal_;
}
const EmptyCal& calibration() const { return *emptyCal_; }
bool equals(const SphericalCamera& camera, double tol = 1e-9) const;
virtual void print(const std::string& s = "SphericalCamera") const;
const Pose3& pose() const { return pose_; }
const Rot3& rotation() const { return pose_.rotation(); }
const Point3& translation() const { return pose_.translation(); }
// /// return pose, with derivative
// const Pose3& getPose(OptionalJacobian<6, 6> H) const;
std::pair<Unit3, bool> projectSafe(const Point3& pw) const;
Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const;
Point3 backproject(const Unit3& p, const double depth) const;
Unit3 backprojectPointAtInfinity(const Unit3& p) const;
Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
Vector2 reprojectionError(const Point3& point, const Unit3& measured,
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
SphericalCamera retract(const Vector6& d) const {
return SphericalCamera(pose().retract(d));
}
Vector6 localCoordinates(const SphericalCamera& p) const {
return pose().localCoordinates(p.pose());
}
static SphericalCamera Identity() {
return SphericalCamera(
Pose3::Identity()); // assumes that the default constructor is valid
}
Matrix34 cameraProjectionMatrix() const {
return Matrix34(pose_.inverse().matrix().block(0, 0, 3, 4));
}
Vector defaultErrorWhenTriangulatingBehindCamera() const {
return Eigen::Matrix<double, traits<Point2>::dimension, 1>::Constant(0.0);
}
size_t dim() const { return 6; }
static size_t Dim() { return 6; }
private:
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(pose_);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// end of class SphericalCamera
template <>
struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {};
template <>
struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {};
} // namespace gtsam
Manifold.h
Base class and basic functions for Manifold types.
ThreadsafeException.h
Base exception type that uses tbb_allocator if GTSAM is compiled with TBB.
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
BearingRange.h
Bearing-Range product.
Pose3.h
3D Pose
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
gtsam::Point3
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::make_shared
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
gtsam::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition expressions.h:131
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
gtsam::internal::LieGroup
Both LieGroupTraits and Testable.
Definition Lie.h:229
gtsam::OptionalJacobian
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
gtsam::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Pose3::Identity
static Pose3 Identity()
identity for group operation
Definition Pose3.h:106
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::EmptyCal
Empty calibration.
Definition SphericalCamera.h:40
gtsam::EmptyCal::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition SphericalCamera.h:48
gtsam::SphericalCamera
A spherical camera class that has a Pose3 and measures bearing vectors.
Definition SphericalCamera.h:70
gtsam::SphericalCamera::sharedCalibration
const EmptyCal::shared_ptr & sharedCalibration() const
return shared pointer to calibration
Definition SphericalCamera.h:110
gtsam::SphericalCamera::Dim
static size_t Dim()
Definition SphericalCamera.h:219
gtsam::SphericalCamera::SphericalCamera
SphericalCamera(const Pose3 &pose)
Constructor with pose.
Definition SphericalCamera.h:93
gtsam::SphericalCamera::~SphericalCamera
virtual ~SphericalCamera()=default
Default destructor.
gtsam::SphericalCamera::cameraProjectionMatrix
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition SphericalCamera.h:206
gtsam::SphericalCamera::localCoordinates
Vector6 localCoordinates(const SphericalCamera &p) const
return canonical coordinate
Definition SphericalCamera.h:195
gtsam::SphericalCamera::rotation
const Rot3 & rotation() const
get rotation
Definition SphericalCamera.h:135
gtsam::SphericalCamera::pose
const Pose3 & pose() const
return pose, constant version
Definition SphericalCamera.h:132
gtsam::SphericalCamera::translation
const Point3 & translation() const
get translation
Definition SphericalCamera.h:138
gtsam::SphericalCamera::retract
SphericalCamera retract(const Vector6 &d) const
move a cameras according to d
Definition SphericalCamera.h:190
gtsam::SphericalCamera::calibration
const EmptyCal & calibration() const
return calibration
Definition SphericalCamera.h:115
gtsam::SphericalCamera::dim
size_t dim() const
Definition SphericalCamera.h:216
gtsam::SphericalCamera::Identity
static SphericalCamera Identity()
for Canonical
Definition SphericalCamera.h:200
gtsam::SphericalCamera::SphericalCamera
SphericalCamera()
Default constructor.
Definition SphericalCamera.h:89
gtsam::SphericalCamera::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition SphericalCamera.h:211
gtsam::SphericalCamera::SphericalCamera
SphericalCamera(const Pose3 &pose, const EmptyCal::shared_ptr &cal)
Constructor with empty intrinsics (needed for smart factors).
Definition SphericalCamera.h:97
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43