Loading...
Searching...
No Matches
PinholePose.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/CalibratedCamera.h>
#include <gtsam/geometry/Point2.h>
#include <boost/make_shared.hpp>
namespace gtsam {
template<typename CALIBRATION>
class PinholeBaseK: public PinholeBase {
private:
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
// Get dimensions of calibration type at compile time
static const int DimK = FixedDimension<CALIBRATION>::value;
public:
typedef CALIBRATION CalibrationType;
PinholeBaseK() {
}
explicit PinholeBaseK(const Pose3& pose) :
PinholeBase(pose) {
}
explicit PinholeBaseK(const Vector &v) :
PinholeBase(v) {
}
virtual ~PinholeBaseK() {
}
virtual const CALIBRATION& calibration() const = 0;
std::pair<Point2, bool> projectSafe(const Point3& pw) const {
std::pair<Point2, bool> pn = PinholeBase::projectSafe(pw);
pn.first = calibration().uncalibrate(pn.first);
return pn;
}
template <class POINT>
Point2 _project(const POINT& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint,
OptionalJacobian<2, DimK> Dcal) const {
// project to normalized coordinates
const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
// uncalibrate to pixel coordinates
Matrix2 Dpi_pn;
const Point2 pi = calibration().uncalibrate(pn, Dcal,
Dpose || Dpoint ? &Dpi_pn : 0);
// If needed, apply chain rule
if (Dpose)
*Dpose = Dpi_pn * *Dpose;
if (Dpoint)
*Dpoint = Dpi_pn * *Dpoint;
return pi;
}
Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
return _project(pw, Dpose, Dpoint, Dcal);
}
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 {
return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured);
}
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
return _project(pw, Dpose, Dpoint, Dcal);
}
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 {
typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
Matrix2K Dpn_dcal;
Matrix22 Dpn_dp;
const Point2 pn = calibration().calibrate(p, Dresult_dcal ? &Dpn_dcal : 0,
Dresult_dp ? &Dpn_dp : 0);
Matrix32 Dpoint_dpn;
Matrix31 Dpoint_ddepth;
const Point3 point = BackprojectFromCamera(pn, depth,
(Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0,
Dresult_ddepth ? &Dpoint_ddepth : 0);
Matrix33 Dresult_dpoint;
const Point3 result = pose().transformFrom(point, Dresult_dpose,
(Dresult_ddepth ||
Dresult_dp ||
Dresult_dcal) ? &Dresult_dpoint : 0);
if (Dresult_dcal)
*Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal; // (3x3)*(3x2)*(2xDimK)
if (Dresult_dp)
*Dresult_dp = Dresult_dpoint * Dpoint_dpn * Dpn_dp; // (3x3)*(3x2)*(2x2)
if (Dresult_ddepth)
*Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth; // (3x3)*(3x1)
return result;
}
Unit3 backprojectPointAtInfinity(const Point2& p) const {
const Point2 pn = calibration().calibrate(p);
const Unit3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
return pose().rotation().rotate(pc);
}
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> Dcamera =
boost::none, OptionalJacobian<1, 6> Dother = boost::none) const {
return pose().range(camera.pose(), Dcamera, Dother);
}
template<class CalibrationB>
double range(const PinholeBaseK<CalibrationB>& camera,
OptionalJacobian<1, 6> Dcamera = boost::none,
OptionalJacobian<1, 6> Dother = boost::none) const {
return pose().range(camera.pose(), Dcamera, Dother);
}
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));
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// end of class PinholeBaseK
template<typename CALIBRATION>
class PinholePose: public PinholeBaseK<CALIBRATION> {
private:
typedef PinholeBaseK<CALIBRATION> Base;
boost::shared_ptr<CALIBRATION> K_;
public:
enum {
dimension = 6
};
PinholePose() {
}
explicit PinholePose(const Pose3& pose) :
Base(pose), K_(new CALIBRATION()) {
}
PinholePose(const Pose3& pose, const boost::shared_ptr<CALIBRATION>& K) :
Base(pose), K_(K) {
}
static PinholePose Level(const boost::shared_ptr<CALIBRATION>& K,
const Pose2& pose2, double height) {
return PinholePose(Base::LevelPose(pose2, height), K);
}
static PinholePose Level(const Pose2& pose2, double height) {
return PinholePose::Level(boost::make_shared<CALIBRATION>(), pose2, height);
}
static PinholePose Lookat(const Point3& eye, const Point3& target,
const Point3& upVector, const boost::shared_ptr<CALIBRATION>& K =
boost::make_shared<CALIBRATION>()) {
return PinholePose(Base::LookatPose(eye, target, upVector), K);
}
explicit PinholePose(const Vector &v) :
Base(v), K_(new CALIBRATION()) {
}
PinholePose(const Vector &v, const Vector &K) :
Base(v), K_(new CALIBRATION(K)) {
}
// Init from Pose3 and calibration
PinholePose(const Pose3 &pose, const Vector &K) :
Base(pose), K_(new CALIBRATION(K)) {
}
bool equals(const Base &camera, double tol = 1e-9) const {
const PinholePose* e = dynamic_cast<const PinholePose*>(&camera);
return Base::equals(camera, tol) && K_->equals(e->calibration(), tol);
}
friend std::ostream& operator<<(std::ostream &os, const PinholePose& camera) {
os << "{R: " << camera.pose().rotation().rpy().transpose();
os << ", t: " << camera.pose().translation().transpose();
if (!camera.K_) os << ", K: none";
else os << ", K: " << *camera.K_;
os << "}";
return os;
}
void print(const std::string& s = "PinholePose") const override {
Base::print(s);
if (!K_)
std::cout << "s No calibration given" << std::endl;
else
K_->print(s + ".calibration");
}
~PinholePose() override {
}
const boost::shared_ptr<CALIBRATION>& sharedCalibration() const {
return K_;
}
const CALIBRATION& calibration() const override {
return *K_;
}
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const {
return Base::project(pw, Dpose, Dpoint);
}
Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const {
return Base::project(pw, Dpose, Dpoint);
}
size_t dim() const {
return 6;
}
static size_t Dim() {
return 6;
}
PinholePose retract(const Vector6& d) const {
return PinholePose(Base::pose().retract(d), K_);
}
Vector6 localCoordinates(const PinholePose& p) const {
return Base::pose().localCoordinates(p.Base::pose());
}
static PinholePose Identity() {
return PinholePose(); // assumes that the default constructor is valid
}
Matrix34 cameraProjectionMatrix() const {
Matrix34 P = Matrix34(PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4));
return K_->K() * P;
}
Vector defaultErrorWhenTriangulatingBehindCamera() const {
return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());;
}
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int /*version*/) {
ar
& boost::serialization::make_nvp("PinholeBaseK",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(K_);
}
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// end of class PinholePose
template<typename CALIBRATION>
struct traits<PinholePose<CALIBRATION> > : public internal::Manifold<
PinholePose<CALIBRATION> > {
};
template<typename CALIBRATION>
struct traits<const PinholePose<CALIBRATION> > : public internal::Manifold<
PinholePose<CALIBRATION> > {
};
} // \ gtsam
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition types.h:308
Point2.h
2D Point
CalibratedCamera.h
Calibrated camera for which only pose is unknown.
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::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::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition Lie.h:136
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::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::projectSafe
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
Definition CalibratedCamera.cpp:109
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::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::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::project2
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
gtsam::CalibratedCamera
A Calibrated camera class [R|-R't], calibration K=I.
Definition CalibratedCamera.h:247
gtsam::PinholeBaseK::range
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
gtsam::PinholeBaseK::project
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
gtsam::PinholeBaseK::project
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
gtsam::PinholeBaseK::projectSafe
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
Definition PinholePose.h:82
gtsam::PinholeBaseK::PinholeBaseK
PinholeBaseK()
default constructor
Definition PinholePose.h:51
gtsam::PinholeBaseK::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 PinholePose.h:181
gtsam::PinholeBaseK::calibration
virtual const CALIBRATION & calibration() const =0
return calibration
gtsam::PinholeBaseK::reprojectionError
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
gtsam::PinholeBaseK::PinholeBaseK
PinholeBaseK(const Pose3 &pose)
constructor with pose
Definition PinholePose.h:55
gtsam::PinholeBaseK::backproject
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
gtsam::PinholeBaseK::range
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
gtsam::PinholeBaseK::backprojectPointAtInfinity
Unit3 backprojectPointAtInfinity(const Point2 &p) const
backproject a 2-dimensional point to a 3-dimensional point at infinity
Definition PinholePose.h:170
gtsam::PinholeBaseK::_project
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
gtsam::PinholeBaseK::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 PinholePose.h:192
gtsam::PinholePose
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition PinholePose.h:243
gtsam::PinholePose::Lookat
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
gtsam::PinholePose::PinholePose
PinholePose()
default constructor
Definition PinholePose.h:260
gtsam::PinholePose::cameraProjectionMatrix
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition PinholePose.h:420
gtsam::PinholePose::project2
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
gtsam::PinholePose::calibration
const CALIBRATION & calibration() const override
return calibration
Definition PinholePose.h:370
gtsam::PinholePose::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition PinholePose.h:426
gtsam::PinholePose::operator<<
friend std::ostream & operator<<(std::ostream &os, const PinholePose &camera)
stream operator
Definition PinholePose.h:339
gtsam::PinholePose::Identity
static PinholePose Identity()
for Canonical
Definition PinholePose.h:415
gtsam::PinholePose::PinholePose
PinholePose(const Vector &v, const Vector &K)
Init from Vector and calibration.
Definition PinholePose.h:319
gtsam::PinholePose::sharedCalibration
const boost::shared_ptr< CALIBRATION > & sharedCalibration() const
return shared pointer to calibration
Definition PinholePose.h:365
gtsam::PinholePose::dim
size_t dim() const
Definition PinholePose.h:395
gtsam::PinholePose::project2
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
gtsam::PinholePose::retract
PinholePose retract(const Vector6 &d) const
move a cameras according to d
Definition PinholePose.h:405
gtsam::PinholePose::PinholePose
PinholePose(const Vector &v)
Init from 6D vector.
Definition PinholePose.h:314
gtsam::PinholePose::PinholePose
PinholePose(const Pose3 &pose, const boost::shared_ptr< CALIBRATION > &K)
constructor with pose and calibration
Definition PinholePose.h:269
gtsam::PinholePose::Dim
static size_t Dim()
Definition PinholePose.h:400
gtsam::PinholePose::access
friend class boost::serialization::access
Serialization function.
Definition PinholePose.h:434
gtsam::PinholePose::equals
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition PinholePose.h:333
gtsam::PinholePose::print
void print(const std::string &s="PinholePose") const override
print
Definition PinholePose.h:349
gtsam::PinholePose::Level
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
gtsam::PinholePose::PinholePose
PinholePose(const Pose3 &pose)
constructor with pose, uses default calibration
Definition PinholePose.h:264
gtsam::PinholePose::Level
static PinholePose Level(const Pose2 &pose2, double height)
PinholePose::level with default calibration.
Definition PinholePose.h:290
gtsam::PinholePose::localCoordinates
Vector6 localCoordinates(const PinholePose &p) const
return canonical coordinate
Definition PinholePose.h:410
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::transformFrom
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
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::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition Pose3.cpp:308
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get rotation
Definition Pose3.cpp:315
gtsam::Rot3::rotate
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
gtsam::Rot3::rpy
Vector3 rpy(OptionalJacobian< 3, 3 > H=boost::none) const
Use RQ to calculate roll-pitch-yaw angle representation.
Definition Rot3.cpp:192
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43