32template<
typename Calibration>
42 typedef Point2Vector MeasurementVector;
106 const Point3& upVector,
const Calibration& K = Calibration()) {
114 typedef Eigen::Matrix<double, DimK, 6> MatrixK6;
116 *H1 << I_6x6, MatrixK6::Zero();
117 typedef Eigen::Matrix<double, 6, DimK> Matrix6K;
118 typedef Eigen::Matrix<double, DimK, DimK> MatrixK;
120 *H2 << Matrix6K::Zero(), MatrixK::Identity();
132 K_ = Calibration(v.tail<DimK>());
151 void print(
const std::string& s =
"PinholeCamera")
const override {
153 K_.print(s +
".calibration");
172 H->template block<6, 6>(0, 0) = I_6x6;
196 typedef Eigen::Matrix<double, dimension, 1> VectorK6;
200 if ((
size_t) d.size() == 6)
224 typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
229 template<
class POINT>
235 Eigen::Matrix<double, 2, DimK> Dcal;
237 *Dcamera <<
Dpose, Dcal;
264 double result = this->
pose().
range(point, Dcamera ? &Dpose_ : 0,
Dpoint);
266 *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
278 double result = this->
pose().
range(pose, Dcamera ? &Dpose_ : 0,
Dpose);
280 *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
289 template<
class CalibrationB>
293 Matrix16 Dcamera_, Dother_;
294 double result = this->
pose().
range(camera.
pose(), Dcamera ? &Dcamera_ : 0,
295 Dother ? &Dother_ : 0);
297 *Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
301 Dother->template block<1, 6>(0, 0) = Dother_;
314 return range(camera.
pose(), Dcamera, Dother);
324 return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());;
331 template<
class Archive>
332 void serialize(Archive & ar,
const unsigned int ) {
334 & boost::serialization::make_nvp(
"PinholeBaseK",
335 boost::serialization::base_object<Base>(*
this));
336 ar & BOOST_SERIALIZATION_NVP(K_);
345template <
typename Calibration>
349template <
typename Calibration>
354template <
typename Calibration,
typename T>
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Pinhole camera with known calibration.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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
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
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition concepts.h:30
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition Lie.h:136
Both ManifoldTraits and Testable.
Definition Manifold.h:120
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition Manifold.h:164
OptionalJacobian is an Eigen::Ref like class that can take be constructed using either a fixed size o...
Definition OptionalJacobian.h:41
Definition BearingRange.h:40
Definition BearingRange.h:194
static Matrix26 Dpose(const Point2 &pn, double d)
Calculate Jacobian with respect to pose.
Definition CalibratedCamera.cpp:27
virtual void print(const std::string &s="PinholeBase") const
print
Definition CalibratedCamera.cpp:74
const Pose3 & pose() const
return pose, constant version
Definition CalibratedCamera.h:152
static Pose3 LevelPose(const Pose2 &pose2, double height)
Create a level pose at the given 2D pose and height.
Definition CalibratedCamera.cpp:49
bool equals(const PinholeBase &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition CalibratedCamera.cpp:69
static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt)
Calculate Jacobian with respect to point.
Definition CalibratedCamera.cpp:37
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
A Calibrated camera class [R|-R't], calibration K=I.
Definition CalibratedCamera.h:247
A pinhole camera class that has a Pose3 and a Calibration.
Definition PinholeCamera.h:33
void print(const std::string &s="PinholeCamera") const override
print
Definition PinholeCamera.h:151
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
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition PinholeCamera.h:323
const Calibration & calibration() const override
return calibration
Definition PinholeCamera.h:178
const Pose3 & getPose(OptionalJacobian< 6, dimension > H) const
return pose, with derivative
Definition PinholeCamera.h:169
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
PinholeCamera(const Pose3 &pose)
constructor with pose
Definition PinholeCamera.h:66
static PinholeCamera Level(const Pose2 &pose2, double height)
PinholeCamera::level with default calibration.
Definition PinholeCamera.h:92
PinholeCamera(const Pose3 &pose, const Calibration &K)
constructor with pose and calibration
Definition PinholeCamera.h:71
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
size_t dim() const
Definition PinholeCamera.h:187
static PinholeCamera Identity()
for Canonical
Definition PinholeCamera.h:216
static size_t Dim()
Definition PinholeCamera.h:192
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition PinholeCamera.h:145
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
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
PinholeCamera(const Vector &v, const Vector &K)
Init from Vector and calibration.
Definition PinholeCamera.h:136
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
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition PinholeCamera.h:318
VectorK6 localCoordinates(const PinholeCamera &T2) const
return canonical coordinate
Definition PinholeCamera.h:208
PinholeCamera retract(const Vector &d) const
move a cameras according to d
Definition PinholeCamera.h:199
friend class boost::serialization::access
Serialization function.
Definition PinholeCamera.h:330
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
const Pose3 & pose() const
return pose
Definition PinholeCamera.h:164
PinholeCamera(const Vector &v)
Init from vector, can be 6D (default calibration) or dim.
Definition PinholeCamera.h:129
PinholeCamera()
default constructor
Definition PinholeCamera.h:62
Point2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition PinholeCamera.h:41
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
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
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:34
Point2 project(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a 3D point from world coordinates into the image
Definition PinholePose.h:118
A 2D pose (Point2,Rot2)
Definition Pose2.h:36
A 3D pose (R,t) : (Rot3,Point3)
Definition Pose3.h:37
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
Matrix4 matrix() const
convert to 4*4 matrix
Definition Pose3.cpp:323
Pose3 inverse() const
inverse transformation with derivatives
Definition Pose3.cpp:49
Represents a 3D point on a unit sphere.
Definition Unit3.h:43