32 : std::runtime_error(
"Stereo Cheirality Exception"),
35 Key nearbyVariable()
const {
56 typedef StereoPoint2Vector MeasurementVector;
60 Cal3_S2Stereo::shared_ptr K_;
89 void print(
const std::string& s =
"")
const {
90 leftCamPose_.
print(s +
".camera.");
91 K_->print(s +
".calibration.");
96 return leftCamPose_.
equals(camera.leftCamPose_, tol)
97 && K_->equals(*camera.K_, tol);
105 inline size_t dim()
const {
110 static inline size_t Dim() {
135 return K_->baseline();
175 return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());
182 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION 183 friend class boost::serialization::access;
184 template<
class Archive>
185 void serialize(Archive & ar,
const unsigned int ) {
186 ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
187 ar & BOOST_SERIALIZATION_NVP(K_);
Definition: StereoCamera.h:26
bool equals(const Pose3 &pose, double tol=1e-9) const
assert equality up to a tolerance
Both ManifoldTraits and Testable.
Definition: Manifold.h:117
Point2_ project(const Point3_ &p_cam)
Expression version of PinholeBase::Project.
Definition: expressions.h:131
bool equals(const StereoCamera &camera, double tol=1e-9) const
equals
Definition: StereoCamera.h:95
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
StereoPoint2 Measurement
Definition: StereoCamera.h:55
StereoCamera()
Default constructor allocates a calibration!
Definition: StereoCamera.h:72
Vector defaultErrorWhenTriangulatingBehindCamera() const
for Nonlinear Triangulation
Definition: StereoCamera.h:174
size_t dim() const
Dimensionality of the tangent space.
Definition: StereoCamera.h:105
const Pose3 & pose() const
pose
Definition: StereoCamera.h:129
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g ...
Definition: Lie.h:136
static size_t Dim()
Dimensionality of the tangent space.
Definition: StereoCamera.h:110
Vector6 localCoordinates(const StereoCamera &t2) const
Local coordinates of manifold neighborhood around current value.
Definition: StereoCamera.h:120
The most common 5DOF 3D->2D calibration, stereo version.
Definition: Cal3_S2Stereo.h:30
StereoCamera retract(const Vector &v) const
Updates a with tangent space delta.
Definition: StereoCamera.h:115
void print(const std::string &s="") const
print
Definition: StereoCamera.h:89
Definition: chartTesting.h:28
The most common 5DOF 3D->2D calibration + Stereo baseline.
Definition: StereoPoint2.h:34
Definition: OptionalJacobian.h:38
void print(const std::string &s="") const
print with optional string
const Cal3_S2Stereo & calibration() const
Return shared pointer to calibration.
Definition: StereoCamera.h:80
A 2D stereo point (uL,uR,v)
double baseline() const
baseline
Definition: StereoCamera.h:134
Vector3 Point3
Definition: Point3.h:38
Definition: StereoCamera.h:47
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102