Go to the documentation of this file.
17 using namespace std::placeholders;
18 using namespace gtsam;
26 Rot3 R1 = Rot3::Rodrigues(0.1, 0.2, 0.3);
27 Rot3 R2 = Rot3::Rodrigues(0.4, 0.5, 0.6);
38 Matrix numericalH1 = numericalDerivative21<Vector3, Rot3, Rot3>(
43 Matrix numericalH2 = numericalDerivative22<Vector3,Rot3,Rot3>(
83 EXPECT(assert_equal<Vector3>(Vector3::Zero(),
error, 1
e-9));
97 EXPECT(assert_equal<Vector3>(Vector3::Zero(),
error, 1
e-9));
112 EXPECT(assert_equal<Vector6>(Vector6::Zero(),
error, 1
e-9));
static int runAllTests(TestResult &result)
Class between(const Class &g) const
TEST(BetweenFactor, Rot3)
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance)
Check the Jacobians produced by a factor against finite differences.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
#define EXPECT(condition)
Provides additional testing facilities for common data structures.
3D rotation represented as a rotation matrix or quaternion
Some functions to compute numerical derivatives.
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Evaluate derivatives of a nonlinear factor numerically.
void insert(Key j, const Vector &value)
noiseModel::Base::shared_ptr SharedNoiseModel
noiseModel::Diagonal::shared_ptr model
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5))
Vector evaluateError(const T &p1, const T &p2, OptionalMatrixType H1, OptionalMatrixType H2) const override
evaluate error, returns vector of errors size of tangent space
All noise models live in the noiseModel namespace.
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Rot2 R(Rot2::fromAngle(0.1))
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:07:17