Loading...
Searching...
No Matches
Cal3_S2Stereo.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_S2.h>
#include <iosfwd>
namespace gtsam {
class GTSAM_EXPORT Cal3_S2Stereo : public Cal3_S2 {
private:
double b_ = 1.0f;
public:
enum { dimension = 6 };
using shared_ptr = boost::shared_ptr<Cal3_S2Stereo>;
Cal3_S2Stereo() = default;
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
: Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
Cal3_S2Stereo(const Vector6& d)
: Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
Cal3_S2Stereo(double fov, int w, int h, double b)
: Cal3_S2(fov, w, h), b_(b) {}
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Point2 calibrate(const Point2& p, OptionalJacobian<2, 6> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
Vector3 calibrate(const Vector3& p) const { return Cal3_S2::calibrate(p); }
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os,
const Cal3_S2Stereo& cal);
void print(const std::string& s = "") const override;
bool equals(const Cal3_S2Stereo& other, double tol = 10e-9) const;
const Cal3_S2& calibration() const { return *this; }
Matrix3 K() const override { return Cal3_S2::K(); }
inline double baseline() const { return b_; }
Vector6 vector() const {
Vector6 v;
v << Cal3_S2::vector(), b_;
return v;
}
inline size_t dim() const override { return Dim(); }
inline static size_t Dim() { return dimension; }
inline Cal3_S2Stereo retract(const Vector& d) const {
return Cal3_S2Stereo(fx() + d(0), fy() + d(1), skew() + d(2), px() + d(3),
py() + d(4), b_ + d(5));
}
Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
return T2.vector() - vector();
}
private:
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& boost::serialization::make_nvp(
"Cal3_S2", boost::serialization::base_object<Cal3_S2>(*this));
ar& BOOST_SERIALIZATION_NVP(b_);
}
};
// Define GTSAM traits
template <>
struct traits<Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {};
template <>
struct traits<const Cal3_S2Stereo> : public internal::Manifold<Cal3_S2Stereo> {
};
} // \ namespace gtsam
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
gtsam::print
void print(const Matrix &A, const string &s, ostream &stream)
print without optional string, must specify cout yourself
Definition Matrix.cpp:156
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::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::equals
Template to create a binary predicate.
Definition Testable.h:111
gtsam::Cal3::K
virtual Matrix3 K() const
return calibration matrix K
Definition Cal3.h:167
gtsam::Cal3::vector
Vector5 vector() const
vectorized form (column-wise)
Definition Cal3.h:160
gtsam::Cal3::px
double px() const
image center in x
Definition Cal3.h:151
gtsam::Cal3::fx
double fx() const
focal length x
Definition Cal3.h:139
gtsam::Cal3::py
double py() const
image center in y
Definition Cal3.h:154
gtsam::Cal3::skew
double skew() const
skew
Definition Cal3.h:148
gtsam::Cal3::fy
double fy() const
focal length y
Definition Cal3.h:142
gtsam::Cal3_S2::Cal3_S2
Cal3_S2()=default
Create a default calibration that leaves coordinates unchanged.
gtsam::Cal3_S2::calibrate
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none) const
Convert image coordinates uv to intrinsic coordinates xy.
Definition Cal3_S2.cpp:53
gtsam::Cal3_S2Stereo
The most common 5DOF 3D->2D calibration, stereo version.
Definition Cal3_S2Stereo.h:30
gtsam::Cal3_S2Stereo::vector
Vector6 vector() const
vectorized form (column-wise)
Definition Cal3_S2Stereo.h:114
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
constructor from doubles
Definition Cal3_S2Stereo.h:47
gtsam::Cal3_S2Stereo::localCoordinates
Vector6 localCoordinates(const Cal3_S2Stereo &T2) const
Unretraction for the calibration.
Definition Cal3_S2Stereo.h:137
gtsam::Cal3_S2Stereo::calibration
const Cal3_S2 & calibration() const
return calibration, same for left and right
Definition Cal3_S2Stereo.h:105
gtsam::Cal3_S2Stereo::dim
size_t dim() const override
return DOF, dimensionality of tangent space
Definition Cal3_S2Stereo.h:125
gtsam::Cal3_S2Stereo::calibrate
Vector3 calibrate(const Vector3 &p) const
Convert homogeneous image coordinates to intrinsic coordinates.
Definition Cal3_S2Stereo.h:85
gtsam::Cal3_S2Stereo::retract
Cal3_S2Stereo retract(const Vector &d) const
Given 6-dim tangent vector, create new calibration.
Definition Cal3_S2Stereo.h:131
gtsam::Cal3_S2Stereo::K
Matrix3 K() const override
return calibration matrix K, same for left and right
Definition Cal3_S2Stereo.h:108
gtsam::Cal3_S2Stereo::Dim
static size_t Dim()
return DOF, dimensionality of tangent space
Definition Cal3_S2Stereo.h:128
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo(double fov, int w, int h, double b)
easy constructor; field-of-view in degrees, assumes zero skew
Definition Cal3_S2Stereo.h:55
gtsam::Cal3_S2Stereo::baseline
double baseline() const
return baseline
Definition Cal3_S2Stereo.h:111
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo(const Vector6 &d)
constructor from vector
Definition Cal3_S2Stereo.h:51
gtsam::Cal3_S2Stereo::Cal3_S2Stereo
Cal3_S2Stereo()=default
default calibration leaves coordinates unchanged