13 using namespace gtsam;
45 Vector expectedError = -noise;
73 Matrix numerical_H1 = numericalDerivative21<Vector, Pose2, Point2>(
f,
p,
l);
74 Matrix numerical_H2 = numericalDerivative22<Vector, Pose2, Point2>(
f,
p,
l);
91 Point3 noise(0.0, 0.0, 0.0);
108 Point3 noise(-1.0, 0.5, 0.3);
115 Vector expectedError = -noise;
144 Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(
f,
p,
l);
145 Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(
f,
p,
l);
static Rot3 RzRyRx(double x, double y, double z, OptionalJacobian< 3, 1 > Hx={}, OptionalJacobian< 3, 1 > Hy={}, OptionalJacobian< 3, 1 > Hz={})
Rotations around Z, Y, then X axes as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
static int runAllTests(TestResult &result)
Vector evaluateError(const POSE &w_T_b, const POINT &w_P, OptionalMatrixType H1, OptionalMatrixType H2) const override
Error = w_T_b.inverse()*w_P - measured_.
static Pose2 Identity()
identity for group operation
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
static Pose3 Identity()
identity for group operation
Some functions to compute numerical derivatives.
static const Point3 pt(1.0, 2.0, 3.0)
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
static const Line3 l(Rot3(), 1, 1)
#define EXPECT(condition)
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
TEST(SmartFactorBase, Pinhole)
This factor can be used to model relative position measurements from a (2D or 3D) pose to a landmark...
std::uint64_t Key
Integer nonlinear key type.
noiseModel::Gaussian::shared_ptr SharedGaussian
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
All noise models live in the noiseModel namespace.