32 template<
typename CALIBRATION>
37 GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
44 typedef CALIBRATION CalibrationType;
89 template <
class POINT>
100 Dpose ||
Dpoint ? &Dpi_pn : 0);
104 *Dpose = Dpi_pn * *
Dpose;
138 typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
142 Dresult_dp ? &Dpn_dp : 0);
144 Matrix31 Dpoint_ddepth;
146 (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0,
147 Dresult_ddepth ? &Dpoint_ddepth : 0);
148 Matrix33 Dresult_dpoint;
152 Dresult_dcal) ? &Dresult_dpoint : 0);
154 *Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal;
156 *Dresult_dp = Dresult_dpoint * Dpoint_dpn * Dpn_dp;
158 *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;
166 const Unit3 pc(pn.x(), pn.y(), 1.0);
178 return pose().
range(point, Dcamera, Dpoint);
188 return this->
pose().
range(pose, Dcamera, Dpose);
206 template<
class CalibrationB>
215 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 217 friend class boost::serialization::access;
218 template<
class Archive>
219 void serialize(Archive & ar,
const unsigned int ) {
221 & boost::serialization::make_nvp(
"PinholeBase",
222 boost::serialization::base_object<PinholeBase>(*
this));
238 template<
typename CALIBRATION>
244 std::shared_ptr<CALIBRATION> K_;
261 Base(pose), K_(new CALIBRATION()) {
281 const Pose2& pose2,
double height) {
282 return PinholePose(Base::LevelPose(pose2, height), K);
300 const Point3& upVector,
const std::shared_ptr<CALIBRATION>& K =
301 std::make_shared<CALIBRATION>()) {
302 return PinholePose(Base::LookatPose(eye, target, upVector), K);
311 Base(v), K_(new CALIBRATION()) {
316 Base(v), K_(new CALIBRATION(K)) {
321 Base(pose), K_(
new CALIBRATION(K)) {
329 bool equals(
const Base &camera,
double tol = 1e-9)
const {
338 if (!camera.K_) os <<
", K: none";
339 else os <<
", K: " << *camera.K_;
345 void print(
const std::string& s =
"PinholePose")
const override {
348 std::cout <<
"s No calibration given" << std::endl;
350 K_->print(s +
".calibration");
377 return Base::project(pw, Dpose, Dpoint);
383 return Base::project(pw, Dpose, Dpoint);
407 return Base::pose().localCoordinates(p.Base::pose());
423 return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());
429 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 431 friend class boost::serialization::access;
432 template<
class Archive>
433 void serialize(Archive & ar,
const unsigned int ) {
435 & boost::serialization::make_nvp(
"PinholeBaseK",
436 boost::serialization::base_object<Base>(*
this));
437 ar & BOOST_SERIALIZATION_NVP(K_);
446 template<
typename CALIBRATION>
448 PinholePose<CALIBRATION> > {
451 template<
typename CALIBRATION>
453 PinholePose<CALIBRATION> > {
const CALIBRATION & calibration() const override
return calibration
Definition: PinholePose.h:366
static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt)
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself={}, OptionalJacobian< 1, 3 > Hpoint={}) const
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
Definition: CalibratedCamera.h:251
Point2 _project(const POINT &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint, OptionalJacobian< 2, DimK > Dcal) const
Definition: PinholePose.h:90
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
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
Definition: PinholePose.h:126
Definition: PinholePose.h:33
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
Definition: PinholePose.h:416
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
Definition: PinholePose.h:133
double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 3 > Dpoint={}) const
Definition: PinholePose.h:175
PinholePose(const Vector &v, const Vector &K)
Init from Vector and calibration.
Definition: PinholePose.h:315
PinholePose(const Pose3 &pose, const std::shared_ptr< CALIBRATION > &K)
Definition: PinholePose.h:265
Vector2 Point2
Definition: Point2.h:32
static PinholePose Level(const Pose2 &pose2, double height)
PinholePose::level with default calibration.
Definition: PinholePose.h:286
Give fixed size dimension of a type, fails at compile time if dynamic.
Definition: Manifold.h:161
static PinholePose Identity()
for Canonical
Definition: PinholePose.h:411
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
PinholePose(const Pose3 &pose)
Definition: PinholePose.h:260
virtual const CALIBRATION & calibration() const =0
return calibration
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
Definition: PinholePose.h:329
PinholePose()
Definition: PinholePose.h:256
PinholeBase()
Default constructor.
Definition: CalibratedCamera.h:125
Unit3 backprojectPointAtInfinity(const Point2 &p) const
backproject a 2-dimensional point to a 3-dimensional point at infinity
Definition: PinholePose.h:164
Definition: CalibratedCamera.h:54
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in Pose coordinates and transforms it to world coordinates
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
Definition: PinholePose.h:76
void print(const std::string &s="PinholePose") const override
print
Definition: PinholePose.h:345
PinholePose retract(const Vector6 &d) const
move a cameras according to d
Definition: PinholePose.h:401
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
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
Definition: PinholePose.h:112
static size_t Dim()
Definition: PinholePose.h:396
Point2 project2(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
project2 version for point at infinity
Definition: PinholePose.h:381
const Pose3 & pose() const
return pose, constant version
Definition: CalibratedCamera.h:154
GTSAM_EXPORT void print(const Matrix &A, const std::string &s, std::ostream &stream)
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition: PinholePose.h:422
Vector3 rpy(OptionalJacobian< 3, 3 > H={}) const
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
double range(const PinholeBaseK< CalibrationB > &camera, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 6 > Dother={}) const
Definition: PinholePose.h:207
PinholeBaseK(const Pose3 &pose)
Definition: PinholePose.h:54
static PinholePose Lookat(const Point3 &eye, const Point3 &target, const Point3 &upVector, const std::shared_ptr< CALIBRATION > &K=std::make_shared< CALIBRATION >())
Definition: PinholePose.h:299
PinholeBaseK()
Definition: PinholePose.h:50
PinholePose(const Vector &v)
Init from 6D vector.
Definition: PinholePose.h:310
Definition: PinholePose.h:239
Calibrated camera for which only pose is unknown.
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
Definition: PinholePose.h:119
Definition: chartTesting.h:28
static PinholePose Level(const std::shared_ptr< CALIBRATION > &K, const Pose2 &pose2, double height)
Definition: PinholePose.h:280
size_t dim() const
Definition: PinholePose.h:391
Definition: OptionalJacobian.h:38
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
friend std::ostream & operator<<(std::ostream &os, const PinholePose &camera)
stream operator
Definition: PinholePose.h:335
Point2 project2(const Point3 &point, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Point2 project2(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Definition: PinholePose.h:375
static Matrix26 Dpose(const Point2 &pn, double d)
double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 6 > Dother={}) const
Definition: PinholePose.h:196
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:284
Vector3 Point3
Definition: Point3.h:38
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Vector6 localCoordinates(const PinholePose &p) const
return canonical coordinate
Definition: PinholePose.h:406
const std::shared_ptr< CALIBRATION > & sharedCalibration() const
return shared pointer to calibration
Definition: PinholePose.h:361
double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 6 > Dpose={}) const
Definition: PinholePose.h:186