Loading...
Searching...
No Matches
PinholeCamera.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/geometry/PinholePose.h>
#include <gtsam/geometry/BearingRange.h>
namespace gtsam {
template<typename Calibration>
class PinholeCamera: public PinholeBaseK<Calibration> {
public:
typedef Point2 Measurement;
typedef Point2Vector MeasurementVector;
private:
typedef PinholeBaseK<Calibration> Base;
Calibration K_;
// Get dimensions of calibration type at compile time
static const int DimK = FixedDimension<Calibration>::value;
public:
enum {
dimension = 6 + DimK
};
PinholeCamera() {
}
explicit PinholeCamera(const Pose3& pose) :
Base(pose) {
}
PinholeCamera(const Pose3& pose, const Calibration& K) :
Base(pose), K_(K) {
}
static PinholeCamera Level(const Calibration &K, const Pose2& pose2,
double height) {
return PinholeCamera(Base::LevelPose(pose2, height), K);
}
static PinholeCamera Level(const Pose2& pose2, double height) {
return PinholeCamera::Level(Calibration(), pose2, height);
}
static PinholeCamera Lookat(const Point3& eye, const Point3& target,
const Point3& upVector, const Calibration& K = Calibration()) {
return PinholeCamera(Base::LookatPose(eye, target, upVector), K);
}
// Create PinholeCamera, with derivatives
static PinholeCamera Create(const Pose3& pose, const Calibration &K,
OptionalJacobian<dimension, 6> H1 = boost::none, //
OptionalJacobian<dimension, DimK> H2 = boost::none) {
typedef Eigen::Matrix<double, DimK, 6> MatrixK6;
if (H1)
*H1 << I_6x6, MatrixK6::Zero();
typedef Eigen::Matrix<double, 6, DimK> Matrix6K;
typedef Eigen::Matrix<double, DimK, DimK> MatrixK;
if (H2)
*H2 << Matrix6K::Zero(), MatrixK::Identity();
return PinholeCamera(pose,K);
}
explicit PinholeCamera(const Vector &v) :
Base(v.head<6>()) {
if (v.size() > 6)
K_ = Calibration(v.tail<DimK>());
}
PinholeCamera(const Vector &v, const Vector &K) :
Base(v), K_(K) {
}
bool equals(const Base &camera, double tol = 1e-9) const {
const PinholeCamera* e = dynamic_cast<const PinholeCamera*>(&camera);
return Base::equals(camera, tol) && K_.equals(e->calibration(), tol);
}
void print(const std::string& s = "PinholeCamera") const override {
Base::print(s);
K_.print(s + ".calibration");
}
~PinholeCamera() override {
}
const Pose3& pose() const {
return Base::pose();
}
const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
if (H) {
H->setZero();
H->template block<6, 6>(0, 0) = I_6x6;
}
return Base::pose();
}
const Calibration& calibration() const override {
return K_;
}
size_t dim() const {
return dimension;
}
static size_t Dim() {
return dimension;
}
typedef Eigen::Matrix<double, dimension, 1> VectorK6;
PinholeCamera retract(const Vector& d) const {
if ((size_t) d.size() == 6)
return PinholeCamera(this->pose().retract(d), calibration());
else
return PinholeCamera(this->pose().retract(d.head<6>()),
calibration().retract(d.tail(calibration().dim())));
}
VectorK6 localCoordinates(const PinholeCamera& T2) const {
VectorK6 d;
d.template head<6>() = this->pose().localCoordinates(T2.pose());
d.template tail<DimK>() = calibration().localCoordinates(T2.calibration());
return d;
}
static PinholeCamera Identity() {
return PinholeCamera(); // assumes that the default constructor is valid
}
typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
template<class POINT>
Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) const {
// We just call 3-derivative version in Base
if (Dcamera){
Matrix26 Dpose;
Eigen::Matrix<double, 2, DimK> Dcal;
const Point2 pi = Base::project(pw, Dpose, Dpoint, Dcal);
*Dcamera << Dpose, Dcal;
return pi;
} else {
return Base::project(pw, boost::none, Dpoint, boost::none);
}
}
Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const {
return _project2(pw, Dcamera, Dpoint);
}
Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera =
boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const {
return _project2(pw, Dcamera, Dpoint);
}
double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera =
boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const {
Matrix16 Dpose_;
double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint);
if (Dcamera)
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
return result;
}
double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera =
boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const {
Matrix16 Dpose_;
double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose);
if (Dcamera)
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
return result;
}
template<class CalibrationB>
double range(const PinholeCamera<CalibrationB>& camera,
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = boost::none) const {
Matrix16 Dcamera_, Dother_;
double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
Dother ? &Dother_ : 0);
if (Dcamera) {
*Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
}
if (Dother) {
Dother->setZero();
Dother->template block<1, 6>(0, 0) = Dother_;
}
return result;
}
double range(const CalibratedCamera& camera,
OptionalJacobian<1, dimension> Dcamera = boost::none,
OptionalJacobian<1, 6> Dother = boost::none) const {
return range(camera.pose(), Dcamera, Dother);
}
Matrix34 cameraProjectionMatrix() const {
return K_.K() * PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4);
}
Vector defaultErrorWhenTriangulatingBehindCamera() const {
return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());;
}
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar
& boost::serialization::make_nvp("PinholeBaseK",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(K_);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// manifold traits
template <typename Calibration>
struct traits<PinholeCamera<Calibration> >
: public internal::Manifold<PinholeCamera<Calibration> > {};
template <typename Calibration>
struct traits<const PinholeCamera<Calibration> >
: public internal::Manifold<PinholeCamera<Calibration> > {};
// range traits, used in RangeFactor
template <typename Calibration, typename T>
struct Range<PinholeCamera<Calibration>, T> : HasRange<PinholeCamera<Calibration>, T, double> {};
} // \ gtsam
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
PinholePose.h
Pinhole camera with known calibration.
BearingRange.h
Bearing-Range product.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition Point2.h:27
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::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::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::internal::Manifold
Both ManifoldTraits and Testable.
Definition Manifold.h:120
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::Range
Definition BearingRange.h:40
gtsam::HasRange
Definition BearingRange.h:194
gtsam::PinholeBase::Dpose
static Matrix26 Dpose(const Point2 &pn, double d)
Calculate Jacobian with respect to pose.
Definition CalibratedCamera.cpp:27
gtsam::PinholeBase::print
virtual void print(const std::string &s="PinholeBase") const
print
Definition CalibratedCamera.cpp:74
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition CalibratedCamera.h:152
gtsam::PinholeBase::LevelPose
static Pose3 LevelPose(const Pose2 &pose2, double height)
Create a level pose at the given 2D pose and height.
Definition CalibratedCamera.cpp:49
gtsam::PinholeBase::equals
bool equals(const PinholeBase &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition CalibratedCamera.cpp:69
gtsam::PinholeBase::Dpoint
static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt)
Calculate Jacobian with respect to point.
Definition CalibratedCamera.cpp:37
gtsam::PinholeBase::LookatPose
static Pose3 LookatPose(const Point3 &eye, const Point3 &target, const Point3 &upVector)
Create a camera pose at the given eye position looking at a target point in the scene with the specif...
Definition CalibratedCamera.cpp:58
gtsam::CalibratedCamera
A Calibrated camera class [R|-R't], calibration K=I.
Definition CalibratedCamera.h:247
gtsam::PinholeCamera
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
gtsam::PinholeCamera::print
void print(const std::string &s="PinholeCamera") const override
print
Definition PinholeCamera.h:151
gtsam::PinholeCamera::_project2
Point2 _project2(const POINT &pw, OptionalJacobian< 2, dimension > Dcamera, OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint) const
Templated projection of a 3D point or a point at infinity into the image.
Definition PinholeCamera.h:230
gtsam::PinholeCamera::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition PinholeCamera.h:323
gtsam::PinholeCamera::calibration
const Calibration & calibration() const override
return calibration
Definition PinholeCamera.h:178
gtsam::PinholeCamera::getPose
const Pose3 & getPose(OptionalJacobian< 6, dimension > H) const
return pose, with derivative
Definition PinholeCamera.h:169
gtsam::PinholeCamera::project2
Point2 project2(const Point3 &pw, OptionalJacobian< 2, dimension > Dcamera=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholeCamera.h:245
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Pose3 &pose)
constructor with pose
Definition PinholeCamera.h:66
gtsam::PinholeCamera::Level
static PinholeCamera Level(const Pose2 &pose2, double height)
PinholeCamera::level with default calibration.
Definition PinholeCamera.h:92
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Pose3 &pose, const Calibration &K)
constructor with pose and calibration
Definition PinholeCamera.h:71
gtsam::PinholeCamera::Level
static PinholeCamera Level(const Calibration &K, const Pose2 &pose2, double height)
Create a level camera at the given 2D pose and height.
Definition PinholeCamera.h:86
gtsam::PinholeCamera::dim
size_t dim() const
Definition PinholeCamera.h:187
gtsam::PinholeCamera::Identity
static PinholeCamera Identity()
for Canonical
Definition PinholeCamera.h:216
gtsam::PinholeCamera::Dim
static size_t Dim()
Definition PinholeCamera.h:192
gtsam::PinholeCamera::equals
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition PinholeCamera.h:145
gtsam::PinholeCamera::range
double range(const Pose3 &pose, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dpose=boost::none) const
Calculate range to another pose.
Definition PinholeCamera.h:275
gtsam::PinholeCamera::range
double range(const PinholeCamera< CalibrationB > &camera, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 6+CalibrationB::dimension > Dother=boost::none) const
Calculate range to another camera.
Definition PinholeCamera.h:290
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Vector &v, const Vector &K)
Init from Vector and calibration.
Definition PinholeCamera.h:136
gtsam::PinholeCamera::Lookat
static PinholeCamera Lookat(const Point3 &eye, const Point3 &target, const Point3 &upVector, const Calibration &K=Calibration())
Create a camera at the given eye position looking at a target point in the scene with the specified u...
Definition PinholeCamera.h:105
gtsam::PinholeCamera::cameraProjectionMatrix
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition PinholeCamera.h:318
gtsam::PinholeCamera::localCoordinates
VectorK6 localCoordinates(const PinholeCamera &T2) const
return canonical coordinate
Definition PinholeCamera.h:208
gtsam::PinholeCamera::retract
PinholeCamera retract(const Vector &d) const
move a cameras according to d
Definition PinholeCamera.h:199
gtsam::PinholeCamera::project2
Point2 project2(const Unit3 &pw, OptionalJacobian< 2, dimension > Dcamera=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
project a point at infinity from world coordinates into the image
Definition PinholeCamera.h:251
gtsam::PinholeCamera< gtsam::Cal3_S2 >::pose
const Pose3 & pose() const
Definition PinholeCamera.h:164
gtsam::PinholeCamera::PinholeCamera
PinholeCamera(const Vector &v)
Init from vector, can be 6D (default calibration) or dim.
Definition PinholeCamera.h:129
gtsam::PinholeCamera::PinholeCamera
PinholeCamera()
default constructor
Definition PinholeCamera.h:62
gtsam::PinholeCamera::Measurement
Point2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition PinholeCamera.h:41
gtsam::PinholeCamera::range
double range(const CalibratedCamera &camera, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const
Calculate range to a calibrated camera.
Definition PinholeCamera.h:311
gtsam::PinholeCamera::range
double range(const Point3 &point, OptionalJacobian< 1, dimension > Dcamera=boost::none, OptionalJacobian< 1, 3 > Dpoint=boost::none) const
Calculate range to a landmark.
Definition PinholeCamera.h:261
gtsam::PinholeBaseK< Calibration >::project
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
Definition PinholePose.h:118
gtsam::PinholeBaseK< Calibration >::PinholeBaseK
PinholeBaseK()
Definition PinholePose.h:51
gtsam::Pose2
A 2D pose (Point2,Rot2).
Definition Pose2.h:36
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::Pose3::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself=boost::none, OptionalJacobian< 1, 3 > Hpoint=boost::none) const
Calculate range to a landmark.
Definition Pose3.cpp:399
gtsam::Pose3::matrix
Matrix4 matrix() const
convert to 4*4 matrix
Definition Pose3.cpp:323
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition Pose3.cpp:49
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43