Go to the documentation of this file.
32 template<
typename CALIBRATION>
89 template <
class POINT>
114 OptionalJacobian<2, DimK> Dcal = {})
const {
121 OptionalJacobian<2, DimK> Dcal = {})
const {
128 OptionalJacobian<2, DimK> Dcal = {})
const {
136 OptionalJacobian<3, 1> Dresult_ddepth = {},
137 OptionalJacobian<3, DimK> Dresult_dcal = {})
const {
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);
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_;
300 const Point3& upVector,
const std::shared_ptr<CALIBRATION>&
K =
301 std::make_shared<CALIBRATION>()) {
311 Base(
v),
K_(new CALIBRATION()) {
337 os <<
"{R: " <<
camera.pose().rotation().rpy().transpose();
338 os <<
", t: " <<
camera.pose().translation().transpose();
346 void print(
const std::string&
s =
"PinholePose")
const override {
349 std::cout <<
"s No calibration given" << std::endl;
351 K_->print(
s +
".calibration");
430 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
432 friend class boost::serialization::access;
433 template<
class Archive>
434 void serialize(Archive & ar,
const unsigned int ) {
436 & boost::serialization::make_nvp(
"PinholeBaseK",
437 boost::serialization::base_object<Base>(*
this));
438 ar & BOOST_SERIALIZATION_NVP(
K_);
447 template<
typename CALIBRATION>
449 PinholePose<CALIBRATION> > {
452 template<
typename CALIBRATION>
454 PinholePose<CALIBRATION> > {
static PinholePose Level(const std::shared_ptr< CALIBRATION > &K, const Pose2 &pose2, double height)
Matrix34 cameraProjectionMatrix() const
for Linear Triangulation
static Matrix23 Dpoint(const Point2 &pn, double d, const Matrix3 &Rt)
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const PinholePose &camera)
stream operator
static Pose3 LookatPose(const Point3 &eye, const Point3 &target, const Point3 &upVector)
Unit3 backprojectPointAtInfinity(const Point2 &p) const
backproject a 2-dimensional point to a 3-dimensional point at infinity
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
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
static const double d[K][N]
virtual void print(const std::string &s="PinholeBase") const
print
static PinholePose Identity()
for Canonical
PinholeBaseK(const Vector &v)
Both ManifoldTraits and Testable.
PinholePose(const Pose3 &pose, const Vector &K)
ofstream os("timeSchurFactors.csv")
Calibrated camera for which only pose is unknown.
PinholePose(const Pose3 &pose, const std::shared_ptr< CALIBRATION > &K)
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
m m block(1, 0, 2, 2)<< 4
double range(const Point3 &point, OptionalJacobian< 1, 6 > Hself={}, OptionalJacobian< 1, 3 > Hpoint={}) const
PinholePose(const Vector &v, const Vector &K)
Init from Vector and calibration.
PinholePose retract(const Vector6 &d) const
move a cameras according to d
bool equals(const PinholeBase &camera, double tol=1e-9) const
assert equality up to a tolerance
const CALIBRATION & calibration() const override
return calibration
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
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
PinholePose(const Pose3 &pose)
double range(const Point3 &point, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 3 > Dpoint={}) const
Calibration used by Bundler.
std::shared_ptr< CALIBRATION > K_
shared pointer to fixed calibration
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
void print(const std::string &s="PinholePose") const override
print
int RealScalar int RealScalar int RealScalar * pc
virtual const CALIBRATION & calibration() const =0
return calibration
Point2 project2(const Point3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Point2 _project(const POINT &pw, OptionalJacobian< 2, 6 > Dpose, OptionalJacobian< 2, FixedDimension< POINT >::value > Dpoint, OptionalJacobian< 2, DimK > Dcal) const
std::pair< Point2, bool > projectSafe(const Point3 &pw) const
Project a point into the image and check depth.
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
PinholePose(const Vector &v)
Init from 6D vector.
double range(const CalibratedCamera &camera, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 6 > Dother={}) const
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
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
PinholeBaseK< CALIBRATION > Base
base class has 3D pose as private member
double range(const Pose3 &pose, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 6 > Dpose={}) const
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
Point2 project2(const Point3 &point, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 3 > Dpoint={}) const
Point2 project2(const Unit3 &pw, OptionalJacobian< 2, 6 > Dpose={}, OptionalJacobian< 2, 2 > Dpoint={}) const
project2 version for point at infinity
double range(const PinholeBaseK< CalibrationB > &camera, OptionalJacobian< 1, 6 > Dcamera={}, OptionalJacobian< 1, 6 > Dother={}) const
const std::shared_ptr< CALIBRATION > & sharedCalibration() const
return shared pointer to calibration
PinholeBaseK(const Pose3 &pose)
bool equals(const Base &camera, double tol=1e-9) const
assert equality up to a tolerance
const Pose3 & pose() const
return pose, constant version
Array< int, Dynamic, 1 > v
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
The matrix class, also used for vectors and row-vectors.
Give fixed size dimension of a type, fails at compile time if dynamic.
Represents a 3D point on a unit sphere.
static const CalibratedCamera camera(kDefaultPose)
static PinholePose Level(const Pose2 &pose2, double height)
PinholePose::level with default calibration.
static Matrix26 Dpose(const Point2 &pn, double d)
static PinholePose Lookat(const Point3 &eye, const Point3 &target, const Point3 &upVector, const std::shared_ptr< CALIBRATION > &K=std::make_shared< CALIBRATION >())
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T)
static Pose3 LevelPose(const Pose2 &pose2, double height)
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Vector6 localCoordinates(const PinholePose &p) const
return canonical coordinate
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:21