35 enum { dimension = 6 };
48 :
Cal3_S2(fx, fy, s, u0, v0), b_(b) {}
52 :
Cal3_S2(d(0), d(1), d(2), d(3), d(4)), b_(d(5)) {}
90 GTSAM_EXPORT
friend std::ostream&
operator<<(std::ostream&
os,
94 void print(
const std::string&
s =
"")
const override;
124 inline size_t dim()
const override {
return Dim(); }
127 inline static size_t Dim() {
return dimension; }
132 py() +
d(4), b_ +
d(5));
137 return T2.
vector() - vector();
146 friend class boost::serialization::access;
147 template <
class Archive>
149 ar& boost::serialization::make_nvp(
150 "Cal3_S2", boost::serialization::base_object<Cal3_S2>(*
this));
151 ar& BOOST_SERIALIZATION_NVP(b_);
void print(const Matrix &A, const string &s, ostream &stream)
Both ManifoldTraits and Testable.
size_t dim() const override
return DOF, dimensionality of tangent space
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
static size_t Dim()
return DOF, dimensionality of tangent space
Vector6 vector() const
vectorized form (column-wise)
double baseline() const
return baseline
Pose2 T2(M_PI/2.0, Point2(0.0, 2.0))
Matrix3 K() const override
return calibration matrix K, same for left and right
Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b)
constructor from doubles
virtual Matrix3 K() const
return calibration matrix K
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 5 > Dcal=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none) const
Cal3_S2Stereo(const Vector6 &d)
constructor from vector
Vector3 calibrate(const Vector3 &p) const
int RealScalar int RealScalar * py
boost::shared_ptr< Cal3 > shared_ptr
RealScalar RealScalar * px
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Vector5 vector() const
vectorized form (column-wise)
void serialize(Archive &ar, const unsigned int)
ofstream os("timeSchurFactors.csv")
Cal3_S2Stereo retract(const Vector &d) const
Given 6-dim tangent vector, create new calibration.
Point2_ uncalibrate(const Expression< CALIBRATION > &K, const Point2_ &xy_hat)
const Cal3_S2 & calibration() const
return calibration, same for left and right
Cal3_S2Stereo(double fov, int w, int h, double b)
easy constructor; field-of-view in degrees, assumes zero skew
The most common 5DOF 3D->2D calibration.
Vector6 localCoordinates(const Cal3_S2Stereo &T2) const
Unretraction for the calibration.