#include <CalibratedCamera.h>
Public Types | |
typedef Point2 | Measurement |
typedef Point2Vector | MeasurementVector |
typedef Rot3 | Rotation |
typedef Point3 | Translation |
Private Attributes | |
Pose3 | pose_ |
3D pose of camera More... | |
Derivatives | |
static Matrix26 | Dpose (const Point2 &pn, double d) |
static Matrix23 | Dpoint (const Point2 &pn, double d, const Matrix3 &Rt) |
Static functions | |
static Pose3 | LevelPose (const Pose2 &pose2, double height) |
static Pose3 | LookatPose (const Point3 &eye, const Point3 &target, const Point3 &upVector) |
Standard Constructors | |
PinholeBase () | |
Default constructor. More... | |
PinholeBase (const Pose3 &pose) | |
Constructor with pose. More... | |
Advanced Constructors | |
PinholeBase (const Vector &v) | |
virtual | ~PinholeBase ()=default |
Default destructor. More... | |
Testable | |
bool | equals (const PinholeBase &camera, double tol=1e-9) const |
assert equality up to a tolerance More... | |
virtual void | print (const std::string &s="PinholeBase") const |
print More... | |
Standard Interface | |
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... | |
Transformations and measurement functions | |
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... | |
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 |
Advanced interface | |
static std::pair< size_t, size_t > | translationInterval () |
A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras
Definition at line 54 of file CalibratedCamera.h.
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes what "project" returns.
Definition at line 66 of file CalibratedCamera.h.
Definition at line 67 of file CalibratedCamera.h.
typedef Rot3 gtsam::PinholeBase::Rotation |
Pose Concept requirements
Definition at line 59 of file CalibratedCamera.h.
Definition at line 60 of file CalibratedCamera.h.
|
inline |
Default constructor.
Definition at line 125 of file CalibratedCamera.h.
|
inlineexplicit |
Constructor with pose.
Definition at line 128 of file CalibratedCamera.h.
|
inlineexplicit |
Definition at line 134 of file CalibratedCamera.h.
|
virtualdefault |
Default destructor.
|
static |
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition at line 167 of file CalibratedCamera.cpp.
|
staticprotected |
Calculate Jacobian with respect to point
pn | projection in normalized coordinates |
d | disparity (inverse depth) |
Rt | transposed rotation matrix |
Definition at line 37 of file CalibratedCamera.cpp.
|
staticprotected |
Calculate Jacobian with respect to pose
pn | projection in normalized coordinates |
d | disparity (inverse depth) |
Definition at line 27 of file CalibratedCamera.cpp.
bool gtsam::PinholeBase::equals | ( | const PinholeBase & | camera, |
double | tol = 1e-9 |
||
) | const |
assert equality up to a tolerance
Definition at line 69 of file CalibratedCamera.cpp.
const Pose3 & gtsam::PinholeBase::getPose | ( | OptionalJacobian< 6, 6 > | H | ) | const |
return pose, with derivative
Definition at line 79 of file CalibratedCamera.cpp.
Create a level pose 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 49 of file CalibratedCamera.cpp.
|
static |
Create a camera pose 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 |
Definition at line 58 of file CalibratedCamera.cpp.
|
inline |
return pose, constant version
Definition at line 154 of file CalibratedCamera.h.
|
virtual |
Reimplemented in gtsam::PinholePose< CALIBRATION >, gtsam::PinholeCamera< Calibration >, gtsam::PinholeCamera< Cal3Bundler >, and gtsam::CalibratedCamera.
Definition at line 74 of file CalibratedCamera.cpp.
|
static |
Project from 3D point in camera coordinates into image Does not throw a CheiralityException, even if pc behind image plane
pc | point in camera coordinates |
Definition at line 88 of file CalibratedCamera.cpp.
|
static |
Project from 3D point at infinity in camera coordinates into image Does not throw a CheiralityException, even if pc behind image plane
pc | point in camera coordinates |
Definition at line 97 of file CalibratedCamera.cpp.
Point2 gtsam::PinholeBase::project2 | ( | const Point3 & | point, |
OptionalJacobian< 2, 6 > | Dpose = {} , |
||
OptionalJacobian< 2, 3 > | Dpoint = {} |
||
) | const |
Project point into the image Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
point | 3D point in world coordinates |
Definition at line 116 of file CalibratedCamera.cpp.
Point2 gtsam::PinholeBase::project2 | ( | const Unit3 & | point, |
OptionalJacobian< 2, 6 > | Dpose = {} , |
||
OptionalJacobian< 2, 2 > | Dpoint = {} |
||
) | const |
Project point at infinity into the image Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
point | 3D point in world coordinates |
Definition at line 138 of file CalibratedCamera.cpp.
Project a point into the image and check depth.
Definition at line 109 of file CalibratedCamera.cpp.
|
inline |
get rotation
Definition at line 159 of file CalibratedCamera.h.
|
inline |
get translation
Definition at line 164 of file CalibratedCamera.h.
Return the start and end indices (inclusive) of the translation component of the exponential map parameterization
Definition at line 225 of file CalibratedCamera.h.
|
private |
3D pose of camera
Definition at line 71 of file CalibratedCamera.h.