32 using namespace gtsam;
37 static std::shared_ptr<Cal3_S2Stereo>
K(
new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
53 TestStereoFactor factor(measurement,
model,
X(1),
L(1),
K);
61 TestStereoFactor factor(measurement,
model,
X(1),
L(1),
K, body_P_sensor);
81 TestStereoFactor
factor1(measurement,
model,
X(1),
L(1),
K, body_P_sensor);
82 TestStereoFactor
factor2(measurement,
model,
X(1),
L(1),
K, body_P_sensor);
91 TestStereoFactor factor(measurement,
model,
X(1),
L(1),
K);
112 TestStereoFactor factor(measurement,
model,
X(1),
L(1),
K, body_P_sensor);
132 TestStereoFactor factor(measurement,
model,
X(1),
L(1),
K);
139 Matrix H1Actual, H2Actual;
143 Matrix H1Expected = (
Matrix(3, 6) << 0.0, -625.0, 0.0, -100.0, 0.0, 0.0,
144 0.0, -625.0, 0.0, -100.0, 0.0, -8.0,
145 625.0, 0.0, 0.0, 0.0, -100.0, 0.0).finished();
148 0.0, 100.0, 0.0).finished();
160 TestStereoFactor factor(measurement,
model,
X(1),
L(1),
K, body_P_sensor);
167 Matrix H1Actual, H2Actual;
171 Matrix H1Expected = (
Matrix(3, 6) << -100.0, 0.0, 650.0, 0.0, 100.0, 0.0,
172 -100.0, -8.0, 649.2, -8.0, 100.0, 0.0,
173 -10.0, -650.0, 0.0, 0.0, 0.0, 100.0).finished();
176 0.0, 0.0, -100.0).finished();
199 values.insert(
L(1), l1);
static Pose3 camera1(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 6.25))
virtual const Values & optimize()
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
static int runAllTests(TestResult &result)
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
#define DOUBLES_EQUAL(expected, actual, threshold)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0))
NonlinearFactorGraph graph
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
GenericStereoFactor< Pose3, Point3 > TestStereoFactor
static Point2 measurement(323.0, 240.0)
static SharedNoiseModel model(noiseModel::Unit::Create(3))
The most common 5DOF 3D->2D calibration, stereo version.
static std::shared_ptr< Cal3_S2Stereo > K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5))
Array< double, 1, 3 > e(1./3., 0.5, 2.)
A non-linear factor for stereo measurements.
The most common 5DOF 3D->2D calibration + Stereo baseline.
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
GaussianFactorGraph::shared_ptr iterate() override
A Stereo Camera based on two Simple Cameras.
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > Matrix
TEST(StereoFactor, Constructor)
double error() const
return error in current optimizer state
Vector evaluateError(const Pose3 &pose, const Point3 &point, OptionalMatrixType H1, OptionalMatrixType H2) const override
noiseModel::Base::shared_ptr SharedNoiseModel