23 using namespace gtsam;
38 size_t w = 640,
h = 480;
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)
Cal3_S2Stereo retract(const Vector &d) const
Given 6-dim tangent vector, create new calibration.
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)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
double py() const
image center in y
Some functions to compute numerical derivatives.
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, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
The most common 5DOF 3D->2D calibration, stereo version.
#define EXPECT(condition)
double fx() const
focal length x
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static Cal3_S2::shared_ptr K1(new Cal3_S2(fov, w, h))
Point2 uncalibrate(const Point2 &p, OptionalJacobian< 2, 6 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
Vector6 localCoordinates(const Cal3_S2Stereo &T2) const
Unretraction for the calibration.
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")
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
TEST(SmartFactorBase, Pinhole)
double baseline() const
return baseline
void Print(const CONTAINER &keys, const string &s, const KeyFormatter &keyFormatter)
double fy() const
focal length y
#define GTSAM_CONCEPT_TESTABLE_INST(T)
Point2 calibrate(const Point2 &p, OptionalJacobian< 2, 6 > Dcal={}, OptionalJacobian< 2, 2 > Dp={}) const
double px() const
image center in x