Go to the documentation of this file.
34 using namespace gtsam;
47 TEST(FrobeniusPriorSO3, evaluateError) {
48 using namespace ::
so3;
50 Vector actual = factor.evaluateError(
R1);
60 TEST(FrobeniusPriorSO3, ClosestTo) {
63 M << 0.79067393, 0.6051136, -0.0930814,
64 0.4155925, -0.64214347, -0.64324489,
65 -0.44948549, 0.47046326, -0.75917576;
81 TEST(FrobeniusPriorSO3, ChordalL2mean) {
88 using namespace ::
so3;
107 TEST(FrobeniusFactorSO3, evaluateError) {
108 using namespace ::
so3;
138 TEST(FrobeniusBetweenFactorSO3, evaluateError) {
139 using namespace ::
so3;
154 Vector6
v1 = (
Vector(6) << 0.1, 0, 0, 0, 0, 0).finished();
156 Vector6
v2 = (
Vector(6) << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06).finished();
161 TEST(FrobeniusFactorSO4, evaluateError) {
162 using namespace ::
so4;
175 TEST(FrobeniusBetweenFactorSO4, evaluateError) {
176 using namespace ::
so4;
181 Vector actual = factor.evaluateError(
Q1,
Q2, H1, H2);
static int runAllTests(TestResult &result)
Class between(const Class &g) const
SO inverse() const
inverse of a rotation = transpose
virtual const Values & optimize()
#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)
4*4 matrix representation of SO(4)
3D rotation represented as a rotation matrix or quaternion
3*3 matrix representation of SO(3)
double error(const Values &values) const
Pose2_ Expmap(const Vector3_ &xi)
const ValueType at(Key j) const
static const Similarity3 id
const MatrixNN & matrix() const
Return matrix.
TEST(FrobeniusPriorSO3, evaluateError)
Evaluate derivatives of a nonlinear factor numerically.
void insert(Key j, const Vector &value)
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
Factor Graph consisting of non-linear factors.
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
NonlinearFactorGraph graph
Provides convenient mappings of common member functions for testing.
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H={}) const
Various factors that minimize some Frobenius norm.
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Matrix< RealScalar, Dynamic, Dynamic > M
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:39:57