Loading...
Searching...
No Matches
CalibratedCamera.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/BearingRange.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/concepts.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/dllexport.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
class GTSAM_EXPORT CheiralityException: public ThreadsafeException<CheiralityException> {
public:
CheiralityException()
: CheiralityException(std::numeric_limits<Key>::max()) {}
CheiralityException(Key j)
: ThreadsafeException<CheiralityException>("CheiralityException"),
j_(j) {}
Key nearbyVariable() const {return j_;}
private:
Key j_;
};
class GTSAM_EXPORT PinholeBase {
public:
typedef Rot3 Rotation;
typedef Point3 Translation;
typedef Point2 Measurement;
typedef Point2Vector MeasurementVector;
private:
Pose3 pose_;
protected:
static Matrix26 Dpose(const Point2& pn, double d);
static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
public:
static Pose3 LevelPose(const Pose2& pose2, double height);
static Pose3 LookatPose(const Point3& eye, const Point3& target,
const Point3& upVector);
PinholeBase() {}
explicit PinholeBase(const Pose3& pose) : pose_(pose) {}
explicit PinholeBase(const Vector& v) : pose_(Pose3::Expmap(v)) {}
virtual ~PinholeBase() = default;
bool equals(const PinholeBase &camera, double tol = 1e-9) const;
virtual void print(const std::string& s = "PinholeBase") const;
const Pose3& pose() const {
return pose_;
}
const Rot3& rotation() const {
return pose_.rotation();
}
const Point3& translation() const {
return pose_.translation();
}
const Pose3& getPose(OptionalJacobian<6, 6> H) const;
static Point2 Project(const Point3& pc, //
OptionalJacobian<2, 3> Dpoint = boost::none);
static Point2 Project(const Unit3& pc, //
OptionalJacobian<2, 2> Dpoint = boost::none);
std::pair<Point2, bool> projectSafe(const Point3& pw) const;
Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
Point2 project2(const Unit3& point,
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const;
static Point3 BackprojectFromCamera(const Point2& p, const double depth,
OptionalJacobian<3, 2> Dpoint = boost::none,
OptionalJacobian<3, 1> Ddepth = boost::none);
inline static std::pair<size_t, size_t> translationInterval() {
return std::make_pair(3, 5);
}
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(pose_);
}
};
// end of class PinholeBase
class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
public:
enum {
dimension = 6
};
CalibratedCamera() {
}
explicit CalibratedCamera(const Pose3& pose) :
PinholeBase(pose) {
}
// Create CalibratedCamera, with derivatives
static CalibratedCamera Create(const Pose3& pose,
OptionalJacobian<dimension, 6> H1 = boost::none) {
if (H1)
*H1 << I_6x6;
return CalibratedCamera(pose);
}
static CalibratedCamera Level(const Pose2& pose2, double height);
static CalibratedCamera Lookat(const Point3& eye, const Point3& target,
const Point3& upVector);
explicit CalibratedCamera(const Vector &v) :
PinholeBase(v) {
}
virtual ~CalibratedCamera() {
}
CalibratedCamera retract(const Vector& d) const;
Vector localCoordinates(const CalibratedCamera& T2) const;
void print(const std::string& s = "CalibratedCamera") const override {
PinholeBase::print(s);
}
inline size_t dim() const {
return dimension;
}
inline static size_t Dim() {
return dimension;
}
Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
Point3 backproject(const Point2& pn, double depth,
OptionalJacobian<3, 6> Dresult_dpose = boost::none,
OptionalJacobian<3, 2> Dresult_dp = boost::none,
OptionalJacobian<3, 1> Dresult_ddepth = boost::none) const {
Matrix32 Dpoint_dpn;
Matrix31 Dpoint_ddepth;
const Point3 point = BackprojectFromCamera(pn, depth,
Dresult_dp ? &Dpoint_dpn : 0,
Dresult_ddepth ? &Dpoint_ddepth : 0);
Matrix33 Dresult_dpoint;
const Point3 result = pose().transformFrom(point, Dresult_dpose,
(Dresult_ddepth ||
Dresult_dp) ? &Dresult_dpoint : 0);
if (Dresult_dp)
*Dresult_dp = Dresult_dpoint * Dpoint_dpn;
if (Dresult_ddepth)
*Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;
return result;
}
double range(const Point3& point,
OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 3> Dpoint = boost::none) const {
return pose().range(point, Dcamera, Dpoint);
}
double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 6> Dpose = boost::none) const {
return this->pose().range(pose, Dcamera, Dpose);
}
double range(const CalibratedCamera& camera, //
OptionalJacobian<1, 6> H1 = boost::none, //
OptionalJacobian<1, 6> H2 = boost::none) const {
return pose().range(camera.pose(), H1, H2);
}
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar
& boost::serialization::make_nvp("PinholeBase",
boost::serialization::base_object<PinholeBase>(*this));
}
};
// manifold traits
template <>
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
template <>
struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
// range traits, used in RangeFactor
template <typename T>
struct Range<CalibratedCamera, T> : HasRange<CalibratedCamera, T, double> {};
} // 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.
Point2.h
2D Point
Pose3.h
3D Pose
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::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition expressions.h:131
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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::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::ThreadsafeException< CheiralityException >::ThreadsafeException
ThreadsafeException()
Definition ThreadsafeException.h:59
gtsam::Range
Definition BearingRange.h:40
gtsam::HasRange
Definition BearingRange.h:194
gtsam::PinholeBase
A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras.
Definition CalibratedCamera.h:52
gtsam::PinholeBase::PinholeBase
PinholeBase()
Default constructor.
Definition CalibratedCamera.h:123
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::PinholeBase
PinholeBase(const Pose3 &pose)
Constructor with pose.
Definition CalibratedCamera.h:126
gtsam::PinholeBase::translation
const Point3 & translation() const
get translation
Definition CalibratedCamera.h:162
gtsam::PinholeBase::rotation
const Rot3 & rotation() const
get rotation
Definition CalibratedCamera.h:157
gtsam::PinholeBase::Measurement
Point2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition CalibratedCamera.h:64
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::translationInterval
static std::pair< size_t, size_t > translationInterval()
Return the start and end indices (inclusive) of the translation component of the exponential map para...
Definition CalibratedCamera.h:223
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::BackprojectFromCamera
static Point3 BackprojectFromCamera(const Point2 &p, const double depth, OptionalJacobian< 3, 2 > Dpoint=boost::none, OptionalJacobian< 3, 1 > Ddepth=boost::none)
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition CalibratedCamera.cpp:167
gtsam::PinholeBase::Rotation
Rot3 Rotation
Pose Concept requirements.
Definition CalibratedCamera.h:57
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::PinholeBase::~PinholeBase
virtual ~PinholeBase()=default
Default destructor.
gtsam::CalibratedCamera
A Calibrated camera class [R|-R't], calibration K=I.
Definition CalibratedCamera.h:247
gtsam::CalibratedCamera::backproject
Point3 backproject(const Point2 &pn, double depth, OptionalJacobian< 3, 6 > Dresult_dpose=boost::none, OptionalJacobian< 3, 2 > Dresult_dp=boost::none, OptionalJacobian< 3, 1 > Dresult_ddepth=boost::none) const
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition CalibratedCamera.h:352
gtsam::CalibratedCamera::CalibratedCamera
CalibratedCamera()
default constructor
Definition CalibratedCamera.h:259
gtsam::CalibratedCamera::range
double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dpose=boost::none) const
Calculate range to another pose.
Definition CalibratedCamera.h:392
gtsam::CalibratedCamera::dim
size_t dim() const
Definition CalibratedCamera.h:331
gtsam::CalibratedCamera::CalibratedCamera
CalibratedCamera(const Vector &v)
construct from vector
Definition CalibratedCamera.h:303
gtsam::CalibratedCamera::~CalibratedCamera
virtual ~CalibratedCamera()
destructor
Definition CalibratedCamera.h:312
gtsam::CalibratedCamera::Dim
static size_t Dim()
Definition CalibratedCamera.h:336
gtsam::CalibratedCamera::range
double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > H1=boost::none, OptionalJacobian< 1, 6 > H2=boost::none) const
Calculate range to another camera.
Definition CalibratedCamera.h:402
gtsam::CalibratedCamera::print
void print(const std::string &s="CalibratedCamera") const override
print
Definition CalibratedCamera.h:326
gtsam::CalibratedCamera::range
double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 3 > Dpoint=boost::none) const
Calculate range to a landmark.
Definition CalibratedCamera.h:381
gtsam::CalibratedCamera::CalibratedCamera
CalibratedCamera(const Pose3 &pose)
construct with pose
Definition CalibratedCamera.h:263
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::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43