Go to the documentation of this file.
35 using namespace gtsam;
50 Vector6
v1 = (
Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
53 Vector6
v2 = (
Vector(6) << 0, 0, 0, 0.01, 0.02, 0.03).finished();
61 auto model = noiseModel::Isotropic::Sigma(3, 1.2);
62 for (
const size_t p : {5, 4, 3}) {
70 factor.evaluateError(
Q1,
Q2, H1, H2);
84 auto model = noiseModel::Isotropic::Sigma(6, 1.2);
87 const Matrix3 E3(
factor3.evaluateError(
R1,
R2).data());
89 factor4.evaluateError(
SOn(
Q1.matrix()),
SOn(
Q2.matrix())).data());
96 auto model = noiseModel::Isotropic::Sigma(1, 1.2);
98 for (
const size_t p : {5, 4, 3, 2}) {
106 factor.evaluateError(
Q1,
Q2, H1, H2);
static int runAllTests(TestResult &result)
Class between(const Class &g) const
#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)
Main factor type in Shonan averaging, on SO(n) pairs.
4*4 matrix representation of SO(4)
3D rotation represented as a rotation matrix or quaternion
3*3 matrix representation of SO(3)
ShonanFactor< 2 > ShonanFactor2
Pose2_ Expmap(const Vector3_ &xi)
static const Similarity3 id
TEST(ShonanFactor3, evaluateError)
const MatrixNN & matrix() const
Return matrix.
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::Diagonal::shared_ptr model
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Factor Graph consisting of non-linear factors.
ShonanFactor< 3 > ShonanFactor3
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
Provides convenient mappings of common member functions for testing.
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Various factors that minimize some Frobenius norm.
Matrix< RealScalar, Dynamic, Dynamic > M
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:08:36