24#include <boost/make_shared.hpp>
33template<
typename CALIBRATION>
38 GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
45 typedef CALIBRATION CalibrationType;
71 virtual ~PinholeBaseK() {
95 template <
class POINT>
144 typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
148 Dresult_dp ? &Dpn_dp : 0);
150 Matrix31 Dpoint_ddepth;
152 (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0,
153 Dresult_ddepth ? &Dpoint_ddepth : 0);
154 Matrix33 Dresult_dpoint;
158 Dresult_dcal) ? &Dresult_dpoint : 0);
160 *Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal;
162 *Dresult_dp = Dresult_dpoint * Dpoint_dpn * Dpn_dp;
164 *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;
172 const Unit3 pc(pn.x(), pn.y(), 1.0);
212 template<
class CalibrationB>
223 template<
class Archive>
224 void serialize(Archive & ar,
const unsigned int ) {
226 & boost::serialization::make_nvp(
"PinholeBase",
227 boost::serialization::base_object<PinholeBase>(*
this));
242template<
typename CALIBRATION>
248 boost::shared_ptr<CALIBRATION> K_;
285 const Pose2& pose2,
double height) {
304 const Point3& upVector,
const boost::shared_ptr<CALIBRATION>& K =
305 boost::make_shared<CALIBRATION>()) {
315 Base(v), K_(new CALIBRATION()) {
320 Base(v), K_(new CALIBRATION(K)) {
325 Base(
pose), K_(new CALIBRATION(K)) {
342 if (!camera.K_) os <<
", K: none";
343 else os <<
", K: " << *camera.K_;
349 void print(
const std::string& s =
"PinholePose")
const override {
352 std::cout <<
"s No calibration given" << std::endl;
354 K_->print(s +
".calibration");
427 return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());;
435 template<
class Archive>
436 void serialize(Archive & ar,
const unsigned int ) {
438 & boost::serialization::make_nvp(
"PinholeBaseK",
439 boost::serialization::base_object<Base>(*
this));
440 ar & BOOST_SERIALIZATION_NVP(K_);
448template<
typename CALIBRATION>
450 PinholePose<CALIBRATION> > {
453template<
typename CALIBRATION>
455 PinholePose<CALIBRATION> > {
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:308
Calibrated camera for which only pose is unknown.
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
A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras.
Definition: CalibratedCamera.h:52
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
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
Definition: CalibratedCamera.cpp:109
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 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
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
Point2 project2(const Point3 &point, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const
Project point into the image Throws a CheiralityException if point behind image plane iff GTSAM_THROW...
Definition: CalibratedCamera.cpp:116
A Calibrated camera class [R|-R't], calibration K=I.
Definition: CalibratedCamera.h:247
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition: PinholePose.h:34
double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const
Calculate range to a CalibratedCamera.
Definition: PinholePose.h:202
Point2 project(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none, OptionalJacobian< 2, DimK > Dcal=boost::none) const
project a point at infinity from world coordinates into the image
Definition: PinholePose.h:132
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
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
Definition: PinholePose.h:82
PinholeBaseK()
default constructor
Definition: PinholePose.h:51
double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 3 > Dpoint=boost::none) const
Calculate range to a landmark.
Definition: PinholePose.h:181
virtual const CALIBRATION & calibration() const =0
return calibration
Point2 reprojectionError(const Point3 &pw, const Point2 &measured, 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:125
PinholeBaseK(const Pose3 &pose)
constructor with pose
Definition: PinholePose.h:55
Point3 backproject(const Point2 &p, double depth, OptionalJacobian< 3, 6 > Dresult_dpose=boost::none, OptionalJacobian< 3, 2 > Dresult_dp=boost::none, OptionalJacobian< 3, 1 > Dresult_ddepth=boost::none, OptionalJacobian< 3, DimK > Dresult_dcal=boost::none) const
backproject a 2-dimensional point to a 3-dimensional point at given depth
Definition: PinholePose.h:139
double range(const PinholeBaseK< CalibrationB > &camera, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dother=boost::none) const
Calculate range to a PinholePoseK derived class.
Definition: PinholePose.h:213
Unit3 backprojectPointAtInfinity(const Point2 &p) const
backproject a 2-dimensional point to a 3-dimensional point at infinity
Definition: PinholePose.h:170
friend class boost::serialization::access
Serialization function.
Definition: PinholePose.h:222
Point2 _project(const POINT &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint, OptionalJacobian< 2, DimK > Dcal) const
Templated projection of a point (possibly at infinity) from world coordinate to the image.
Definition: PinholePose.h:96
double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera=boost::none, OptionalJacobian< 1, 6 > Dpose=boost::none) const
Calculate range to another pose.
Definition: PinholePose.h:192
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition: PinholePose.h:243
static PinholePose Lookat(const Point3 &eye, const Point3 &target, const Point3 &upVector, const boost::shared_ptr< CALIBRATION > &K=boost::make_shared< CALIBRATION >())
Create a camera at the given eye position looking at a target point in the scene with the specified u...
Definition: PinholePose.h:303
PinholePose()
default constructor
Definition: PinholePose.h:260
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition: PinholePose.h:420
Point2 project2(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
project2 version for point at infinity
Definition: PinholePose.h:385
const CALIBRATION & calibration() const override
return calibration
Definition: PinholePose.h:370
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition: PinholePose.h:426
friend std::ostream & operator<<(std::ostream &os, const PinholePose &camera)
stream operator
Definition: PinholePose.h:339
static PinholePose Identity()
for Canonical
Definition: PinholePose.h:415
PinholePose(const Vector &v, const Vector &K)
Init from Vector and calibration.
Definition: PinholePose.h:319
const boost::shared_ptr< CALIBRATION > & sharedCalibration() const
return shared pointer to calibration
Definition: PinholePose.h:365
size_t dim() const
Definition: PinholePose.h:395
Point2 project2(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose=boost::none, OptionalJacobian< 2, 3 > Dpoint=boost::none) const
project a point from world coordinate to the image, 2 derivatives only
Definition: PinholePose.h:379
PinholePose retract(const Vector6 &d) const
move a cameras according to d
Definition: PinholePose.h:405
PinholePose(const Vector &v)
Init from 6D vector.
Definition: PinholePose.h:314
PinholePose(const Pose3 &pose, const boost::shared_ptr< CALIBRATION > &K)
constructor with pose and calibration
Definition: PinholePose.h:269
static size_t Dim()
Definition: PinholePose.h:400
friend class boost::serialization::access
Serialization function.
Definition: PinholePose.h:434
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition: PinholePose.h:333
void print(const std::string &s="PinholePose") const override
print
Definition: PinholePose.h:349
static PinholePose Level(const boost::shared_ptr< CALIBRATION > &K, const Pose2 &pose2, double height)
Create a level camera at the given 2D pose and height.
Definition: PinholePose.h:284
PinholePose(const Pose3 &pose)
constructor with pose, uses default calibration
Definition: PinholePose.h:264
static PinholePose Level(const Pose2 &pose2, double height)
PinholePose::level with default calibration.
Definition: PinholePose.h:290
Vector6 localCoordinates(const PinholePose &p) const
return canonical coordinate
Definition: PinholePose.h:410
A 2D pose (Point2,Rot2)
Definition: Pose2.h:36
A 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in Pose coordinates and transforms it to world coordinates
Definition: Pose3.cpp:347
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
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition: Pose3.cpp:308
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition: Pose3.cpp:315
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from rotated coordinate frame to world
Definition: Rot3M.cpp:149
Vector3 rpy(OptionalJacobian< 3, 3 > H=boost::none) const
Use RQ to calculate roll-pitch-yaw angle representation.
Definition: Rot3.cpp:192
Represents a 3D point on a unit sphere.
Definition: Unit3.h:43