23 using namespace gtsam;
35 Cal3_S2Stereo
expected(554.256, 554.256, 0, 640 / 2, 480 / 2, 3);
38 size_t w = 640,
h = 480;
39 Cal3_S2Stereo actual(fov, w,
h, 3);
47 Point2 expectedimage(1320.3, 1740);
108 d << 1, 2, 3, 4, 5, 6;
117 std::stringstream
os;
118 os <<
"fx: " << cal.
fx() <<
", fy: " << cal.
fy() <<
", s: " << cal.
skew()
119 <<
", px: " << cal.
px() <<
", py: " << cal.
py()
bool assert_stdout_equal(const std::string &expected, const V &actual)
double fy() const
focal length y
Provides additional testing facilities for common data structures.
size_t dim() const override
return DOF, dimensionality of tangent space
static size_t Dim()
return DOF, dimensionality of tangent space
Concept check for values that can be used in unit tests.
static int runAllTests(TestResult &result)
Point2 uncalibrate_(const Cal3_S2Stereo &k, const Point2 &pt)
double baseline() const
return baseline
Some functions to compute numerical derivatives.
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const boost::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
static const Point3 pt(1.0, 2.0, 3.0)
static Point2 p_uv(1320.3, 1740)
Point2 calibrate_(const Cal3_S2Stereo &K, const Point2 &pt)
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(boost::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
double py() const
image center in y
#define EXPECT(condition)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static Cal3_S2::shared_ptr K1(new Cal3_S2(fov, w, h))
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 6 > Dcal=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none) const
The most common 5DOF 3D->2D calibration + Stereo baseline.
#define GTSAM_CONCEPT_MANIFOLD_INST(T)
static Cal3_S2Stereo K(500, 500, 0.1, 640/2, 480/2, 1)
#define EXPECT_LONGS_EQUAL(expected, actual)
ofstream os("timeSchurFactors.csv")
Cal3_S2Stereo retract(const Vector &d) const
Given 6-dim tangent vector, create new calibration.
TEST(LPInitSolver, InfiniteLoopSingleVar)
double px() const
image center in x
double fx() const
focal length x
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 6 > Dcal=boost::none, OptionalJacobian< 2, 2 > Dp=boost::none) const
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Vector6 localCoordinates(const Cal3_S2Stereo &T2) const
Unretraction for the calibration.