Loading...
Searching...
No Matches
StereoCamera.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/Cal3_S2Stereo.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoPoint2.h>
namespace gtsam {
class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
public:
StereoCheiralityException()
: StereoCheiralityException(std::numeric_limits<Key>::max()) {}
StereoCheiralityException(Key j)
: std::runtime_error("Stereo Cheirality Exception"),
j_(j) {}
Key nearbyVariable() const {
return j_;
}
private:
Key j_;
};
class GTSAM_EXPORT StereoCamera {
public:
typedef StereoPoint2 Measurement;
typedef StereoPoint2Vector MeasurementVector;
private:
Pose3 leftCamPose_;
Cal3_S2Stereo::shared_ptr K_;
public:
enum {
dimension = 6
};
StereoCamera() :
K_(new Cal3_S2Stereo()) {
}
StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
const Cal3_S2Stereo& calibration() const {
return *K_;
}
void print(const std::string& s = "") const {
leftCamPose_.print(s + ".camera.");
K_->print(s + ".calibration.");
}
bool equals(const StereoCamera &camera, double tol = 1e-9) const {
return leftCamPose_.equals(camera.leftCamPose_, tol)
&& K_->equals(*camera.K_, tol);
}
inline size_t dim() const {
return 6;
}
static inline size_t Dim() {
return 6;
}
inline StereoCamera retract(const Vector& v) const {
return StereoCamera(pose().retract(v), K_);
}
inline Vector6 localCoordinates(const StereoCamera& t2) const {
return leftCamPose_.localCoordinates(t2.leftCamPose_);
}
const Pose3& pose() const {
return leftCamPose_;
}
double baseline() const {
return K_->baseline();
}
StereoPoint2 project(const Point3& point) const;
StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 =
boost::none, OptionalJacobian<3, 3> H2 = boost::none) const;
Point3 backproject(const StereoPoint2& z) const;
Point3 backproject2(const StereoPoint2& z,
OptionalJacobian<3, 6> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const;
StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1,
OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
boost::none) const;
Vector defaultErrorWhenTriangulatingBehindCamera() const {
return Eigen::Matrix<double,traits<Measurement>::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_NVP(leftCamPose_);
ar & BOOST_SERIALIZATION_NVP(K_);
}
};
template<>
struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {
};
template<>
struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {
};
}
StereoPoint2.h
A 2D stereo point (uL,uR,v).
Pose3.h
3D Pose
Cal3_S2Stereo.h
The most common 5DOF 3D->2D calibration + Stereo baseline.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition serialization.h:113
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::project
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition expressions.h:131
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
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::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::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition Cal3_S2Stereo.h:30
gtsam::Pose3
A 3D pose (R,t) : (Rot3,Point3).
Definition Pose3.h:37
gtsam::StereoCamera
A stereo camera class, parameterize by left camera pose and stereo calibration.
Definition StereoCamera.h:47
gtsam::StereoCamera::StereoCamera
StereoCamera()
Default constructor allocates a calibration!
Definition StereoCamera.h:72
gtsam::StereoCamera::Dim
static size_t Dim()
Dimensionality of the tangent space.
Definition StereoCamera.h:110
gtsam::StereoCamera::print
void print(const std::string &s="") const
print
Definition StereoCamera.h:89
gtsam::StereoCamera::localCoordinates
Vector6 localCoordinates(const StereoCamera &t2) const
Local coordinates of manifold neighborhood around current value.
Definition StereoCamera.h:120
gtsam::StereoCamera::equals
bool equals(const StereoCamera &camera, double tol=1e-9) const
equals
Definition StereoCamera.h:95
gtsam::StereoCamera::dim
size_t dim() const
Dimensionality of the tangent space.
Definition StereoCamera.h:105
gtsam::StereoCamera::retract
StereoCamera retract(const Vector &v) const
Updates a with tangent space delta.
Definition StereoCamera.h:115
gtsam::StereoCamera::Measurement
StereoPoint2 Measurement
Some classes template on either PinholeCamera or StereoCamera, and this typedef informs those classes...
Definition StereoCamera.h:55
gtsam::StereoCamera::calibration
const Cal3_S2Stereo & calibration() const
Return shared pointer to calibration.
Definition StereoCamera.h:80
gtsam::StereoCamera::baseline
double baseline() const
baseline
Definition StereoCamera.h:134
gtsam::StereoCamera::pose
const Pose3 & pose() const
pose
Definition StereoCamera.h:129
gtsam::StereoCamera::defaultErrorWhenTriangulatingBehindCamera
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition StereoCamera.h:174
gtsam::StereoPoint2
A 2D stereo point, v will be same for rectified images.
Definition StereoPoint2.h:32