#include <PinholeCamera.h>
Public Types | |
enum | { dimension = 6 + DimK } |
typedef Point2 | Measurement |
typedef Point2Vector | MeasurementVector |
Public Types inherited from gtsam::PinholeBaseK< Calibration > | |
typedef Calibration | CalibrationType |
Public Types inherited from gtsam::PinholeBase | |
typedef Point2 | Measurement |
typedef Point2Vector | MeasurementVector |
typedef Rot3 | Rotation |
typedef Point3 | Translation |
Private Types | |
typedef PinholeBaseK< Calibration > | Base |
base class has 3D pose as private member More... | |
Private Attributes | |
Calibration | K_ |
Calibration, part of class now. More... | |
Static Private Attributes | |
static const int | DimK = FixedDimension<Calibration>::value |
Standard Constructors | |
PinholeCamera () | |
PinholeCamera (const Pose3 &pose) | |
PinholeCamera (const Pose3 &pose, const Calibration &K) | |
Named Constructors | |
static PinholeCamera | Level (const Calibration &K, const Pose2 &pose2, double height) |
static PinholeCamera | Level (const Pose2 &pose2, double height) |
PinholeCamera::level with default calibration. More... | |
static PinholeCamera | Lookat (const Point3 &eye, const Point3 &target, const Point3 &upVector, const Calibration &K=Calibration()) |
static PinholeCamera | Create (const Pose3 &pose, const Calibration &K, OptionalJacobian< dimension, 6 > H1={}, OptionalJacobian< dimension, DimK > H2={}) |
Advanced Constructors | |
PinholeCamera (const Vector &v) | |
Init from vector, can be 6D (default calibration) or dim. More... | |
PinholeCamera (const Vector &v, const Vector &K) | |
Init from Vector and calibration. More... | |
Testable | |
bool | equals (const Base &camera, double tol=1e-9) const |
assert equality up to a tolerance More... | |
void | print (const std::string &s="PinholeCamera") const override |
print More... | |
Standard Interface | |
~PinholeCamera () override | |
const Pose3 & | pose () const |
return pose More... | |
const Pose3 & | getPose (OptionalJacobian< 6, dimension > H) const |
return pose, with derivative More... | |
const Calibration & | calibration () const override |
return calibration More... | |
Manifold | |
typedef Eigen::Matrix< double, dimension, 1 > | VectorK6 |
size_t | dim () const |
PinholeCamera | retract (const Vector &d) const |
move a cameras according to d More... | |
VectorK6 | localCoordinates (const PinholeCamera &T2) const |
return canonical coordinate More... | |
static size_t | Dim () |
static PinholeCamera | Identity () |
for Canonical More... | |
Transformations and measurement functions | |
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 |
Point2 | project2 (const Point3 &pw, OptionalJacobian< 2, dimension > Dcamera={}, OptionalJacobian< 2, 3 > Dpoint={}) const |
project a 3D point from world coordinates into the image More... | |
Point2 | project2 (const Unit3 &pw, OptionalJacobian< 2, dimension > Dcamera={}, OptionalJacobian< 2, 2 > Dpoint={}) const |
project a point at infinity from world coordinates into the image More... | |
double | range (const Point3 &point, OptionalJacobian< 1, dimension > Dcamera={}, OptionalJacobian< 1, 3 > Dpoint={}) const |
double | range (const Pose3 &pose, OptionalJacobian< 1, dimension > Dcamera={}, OptionalJacobian< 1, 6 > Dpose={}) const |
template<class CalibrationB > | |
double | range (const PinholeCamera< CalibrationB > &camera, OptionalJacobian< 1, dimension > Dcamera={}, OptionalJacobian< 1, 6+CalibrationB::dimension > Dother={}) const |
double | range (const CalibratedCamera &camera, OptionalJacobian< 1, dimension > Dcamera={}, OptionalJacobian< 1, 6 > Dother={}) const |
Matrix34 | cameraProjectionMatrix () const |
for Linear Triangulation More... | |
Vector | defaultErrorWhenTriangulatingBehindCamera () const |
for Nonlinear Triangulation More... | |
Additional Inherited Members | |
Public Member Functions inherited from gtsam::PinholeBaseK< Calibration > | |
PinholeBaseK () | |
PinholeBaseK (const Pose3 &pose) | |
PinholeBaseK (const Vector &v) | |
virtual const Calibration & | calibration () const=0 |
return calibration More... | |
std::pair< Point2, bool > | projectSafe (const Point3 &pw) const |
Project a point into the image and check depth. More... | |
Point2 | _project (const POINT &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint, OptionalJacobian< 2, DimK > Dcal) const |
Point2 | project (const Point3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}, OptionalJacobian< 2, DimK > Dcal={}) const |
project a 3D point from world coordinates into the image More... | |
Point2 | project (const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}, OptionalJacobian< 2, DimK > Dcal={}) const |
project a point at infinity from world coordinates into the image More... | |
Point2 | reprojectionError (const Point3 &pw, const Point2 &measured, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}, OptionalJacobian< 2, DimK > Dcal={}) const |
project a 3D point from world coordinates into the image More... | |
Point3 | backproject (const Point2 &p, double depth, OptionalJacobian< 3, 6 > Dresult_dpose={}, OptionalJacobian< 3, 2 > Dresult_dp={}, OptionalJacobian< 3, 1 > Dresult_ddepth={}, OptionalJacobian< 3, DimK > Dresult_dcal={}) const |
backproject a 2-dimensional point to a 3-dimensional point at given depth More... | |
Unit3 | backprojectPointAtInfinity (const Point2 &p) const |
backproject a 2-dimensional point to a 3-dimensional point at infinity More... | |
double | range (const Point3 &point, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 3 > Dpoint={}) const |
double | range (const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 6 > Dpose={}) const |
double | range (const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 6 > Dother={}) const |
double | range (const PinholeBaseK< CalibrationB > &camera, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 6 > Dother={}) const |
Public Member Functions inherited from gtsam::PinholeBase | |
PinholeBase () | |
Default constructor. More... | |
PinholeBase (const Pose3 &pose) | |
Constructor with pose. More... | |
PinholeBase (const Vector &v) | |
virtual | ~PinholeBase ()=default |
Default destructor. More... | |
bool | equals (const PinholeBase &camera, double tol=1e-9) const |
assert equality up to a tolerance More... | |
const Pose3 & | pose () const |
return pose, constant version More... | |
const Rot3 & | rotation () const |
get rotation More... | |
const Point3 & | translation () const |
get translation More... | |
const Pose3 & | getPose (OptionalJacobian< 6, 6 > H) const |
return pose, with derivative More... | |
std::pair< Point2, bool > | projectSafe (const Point3 &pw) const |
Project a point into the image and check depth. More... | |
Point2 | project2 (const Point3 &point, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const |
Point2 | project2 (const Unit3 &point, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const |
Static Public Member Functions inherited from gtsam::PinholeBase | |
static Pose3 | LevelPose (const Pose2 &pose2, double height) |
static Pose3 | LookatPose (const Point3 &eye, const Point3 &target, const Point3 &upVector) |
static Point2 | Project (const Point3 &pc, OptionalJacobian< 2, 3 > Dpoint={}) |
static Point2 | Project (const Unit3 &pc, OptionalJacobian< 2, 2 > Dpoint={}) |
static Point3 | BackprojectFromCamera (const Point2 &p, const double depth, OptionalJacobian< 3, 2 > Dpoint={}, OptionalJacobian< 3, 1 > Ddepth={}) |
backproject a 2-dimensional point to a 3-dimensional point at given depth More... | |
static std::pair< size_t, size_t > | translationInterval () |
Static Protected Member Functions inherited from gtsam::PinholeBase | |
static Matrix26 | Dpose (const Point2 &pn, double d) |
static Matrix23 | Dpoint (const Point2 &pn, double d, const Matrix3 &Rt) |
A pinhole camera class that has a Pose3 and a Calibration. Use PinholePose if you will not be optimizing for Calibration
Definition at line 33 of file PinholeCamera.h.
|
private |
base class has 3D pose as private member
Definition at line 46 of file PinholeCamera.h.
typedef Eigen::Matrix<double, 2, DimK> gtsam::PinholeCamera< Calibration >::Matrix2K |
Definition at line 224 of file PinholeCamera.h.
typedef Point2 gtsam::PinholeCamera< Calibration >::Measurement |
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes what "project" returns.
Definition at line 41 of file PinholeCamera.h.
typedef Point2Vector gtsam::PinholeCamera< Calibration >::MeasurementVector |
Definition at line 42 of file PinholeCamera.h.
typedef Eigen::Matrix<double, dimension, 1> gtsam::PinholeCamera< Calibration >::VectorK6 |
Definition at line 196 of file PinholeCamera.h.
anonymous enum |
Enumerator | |
---|---|
dimension |
Definition at line 54 of file PinholeCamera.h.
|
inline |
default constructor
Definition at line 62 of file PinholeCamera.h.
|
inlineexplicit |
constructor with pose
Definition at line 66 of file PinholeCamera.h.
|
inline |
constructor with pose and calibration
Definition at line 71 of file PinholeCamera.h.
|
inlineexplicit |
Init from vector, can be 6D (default calibration) or dim.
Definition at line 129 of file PinholeCamera.h.
|
inline |
Init from Vector and calibration.
Definition at line 136 of file PinholeCamera.h.
|
inlineoverride |
Definition at line 160 of file PinholeCamera.h.
|
inline |
Templated projection of a 3D point or a point at infinity into the image
pw | either a Point3 or a Unit3, in world coordinates |
Definition at line 230 of file PinholeCamera.h.
|
inlineoverride |
return calibration
Definition at line 178 of file PinholeCamera.h.
|
inline |
for Linear Triangulation
Definition at line 318 of file PinholeCamera.h.
|
inlinestatic |
Definition at line 111 of file PinholeCamera.h.
|
inline |
for Nonlinear Triangulation
Definition at line 323 of file PinholeCamera.h.
|
inlinestatic |
Definition at line 192 of file PinholeCamera.h.
|
inline |
Definition at line 187 of file PinholeCamera.h.
|
inline |
assert equality up to a tolerance
Definition at line 145 of file PinholeCamera.h.
|
inline |
return pose, with derivative
Definition at line 169 of file PinholeCamera.h.
|
inlinestatic |
for Canonical
Definition at line 216 of file PinholeCamera.h.
|
inlinestatic |
Create a level camera at the given 2D pose and height
K | the calibration |
pose2 | specifies the location and viewing direction (theta 0 = looking in direction of positive X axis) |
height | camera height |
Definition at line 86 of file PinholeCamera.h.
|
inlinestatic |
PinholeCamera::level with default calibration.
Definition at line 92 of file PinholeCamera.h.
|
inline |
return canonical coordinate
Definition at line 208 of file PinholeCamera.h.
|
inlinestatic |
Create a camera at the given eye position looking at a target point in the scene with the specified up direction vector.
eye | specifies the camera position |
target | the point to look at |
upVector | specifies the camera up direction vector, doesn't need to be on the image plane nor orthogonal to the viewing axis |
K | optional calibration parameter |
Definition at line 105 of file PinholeCamera.h.
|
inline |
return pose
Definition at line 164 of file PinholeCamera.h.
|
inlineoverridevirtual |
|
inline |
project a 3D point from world coordinates into the image
Definition at line 245 of file PinholeCamera.h.
|
inline |
project a point at infinity from world coordinates into the image
Definition at line 251 of file PinholeCamera.h.
|
inline |
Calculate range to a calibrated camera
camera | Other camera |
Definition at line 311 of file PinholeCamera.h.
|
inline |
Calculate range to another camera
camera | Other camera |
Definition at line 290 of file PinholeCamera.h.
|
inline |
Calculate range to a landmark
point | 3D location of landmark |
Definition at line 261 of file PinholeCamera.h.
|
inline |
Calculate range to another pose
pose | Other SO(3) pose |
Definition at line 275 of file PinholeCamera.h.
|
inline |
move a cameras according to d
Definition at line 199 of file PinholeCamera.h.
|
staticprivate |
Definition at line 50 of file PinholeCamera.h.
|
private |
Calibration, part of class now.
Definition at line 47 of file PinholeCamera.h.