31 using namespace gtsam;
53 auto Q = SO4::Random(rng);
60 Vector6
v1 = (
Vector(6) << 0, 0, 0, 0.1, 0, 0).finished();
62 Vector6
v2 = (
Vector(6) << 0.00, 0.00, 0.00, 0.01, 0.02, 0.03).finished();
64 Vector6
v3 = (
Vector(6) << 1, 2, 3, 4, 5, 6).finished();
79 for (
size_t i = 0;
i < 6;
i++) {
80 Vector6
xi = Vector6::Zero();
100 auto X1 = SOn::Hat(
v1), X2 = SOn::Hat(
v2), X3 = SOn::Hat(
v3);
116 auto v = Vector6::Zero();
129 auto v0 = Vector6::Zero();
136 EXPECT(check_group_invariants(
id,
id));
137 EXPECT(check_group_invariants(
id,
Q1));
138 EXPECT(check_group_invariants(
Q2,
id));
142 EXPECT(check_manifold_invariants(
id,
id));
143 EXPECT(check_manifold_invariants(
id,
Q1));
144 EXPECT(check_manifold_invariants(
Q2,
id));
152 Matrix actualH1, actualH2;
170 const Vector16 actual =
Q2.
vec(actualH);
172 std::function<Vector16(const SO4&)>
f = [](
const SO4&
Q) {
return Q.
vec(); };
179 const Matrix3
expected = Q3.matrix().topLeftCorner<3, 3>();
181 const Matrix3 actual =
topLeft(Q3, actualH);
183 std::function<Matrix3(const SO4&)>
f = [](
const SO4& Q3) {
192 const Matrix43
expected = Q3.matrix().leftCols<3>();
194 const Matrix43 actual =
stiefel(Q3, actualH);
196 std::function<Matrix43(const SO4&)>
f = [](
const SO4& Q3) {
#define GTSAM_CONCEPT_ASSERT(concept)
Concept check for values that can be used in unit tests.
Provides convenient mappings of common member functions for testing.
static int runAllTests(TestResult &result)
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
EIGEN_DEVICE_FUNC const VectorBlock< const Coefficients, 3 > vec() const
A matrix or vector expression mapping an existing array of data.
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Rot2 R(Rot2::fromAngle(0.1))
4*4 matrix representation of SO(4)
Pose2_ Expmap(const Vector3_ &xi)
Some functions to compute numerical derivatives.
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
3*3 matrix representation of SO(3)
static const Similarity3 id
internal::FixedSizeMatrix< Y, X >::type numericalDerivative11(std::function< Y(const X &)> h, const X &x, double delta=1e-5)
New-style numerical derivatives using manifold_traits.
T compose(const T &t1, const T &t2)
internal::FixedSizeMatrix< Y, X1 >::type numericalDerivative21(const std::function< Y(const X1 &, const X2 &)> &h, const X1 &x1, const X2 &x2, double delta=1e-5)
Base class and basic functions for Manifold types.
GTSAM_EXPORT Matrix43 stiefel(const SO4 &Q, OptionalJacobian< 12, 6 > H)
const MatrixNN & matrix() const
Return matrix.
#define EXPECT(condition)
Array< int, Dynamic, 1 > v
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Class compose(const Class &g) const
GTSAM_EXPORT Matrix3 topLeft(const SO4 &Q, OptionalJacobian< 9, 6 > H)
#define EXPECT_LONGS_EQUAL(expected, actual)
The quaternion class used to represent 3D orientations and rotations.
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
The matrix class, also used for vectors and row-vectors.
EIGEN_DEVICE_FUNC bool isApprox(const Scalar &x, const Scalar &y, const typename NumTraits< Scalar >::Real &precision=NumTraits< Scalar >::dummy_precision())
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
EIGEN_DEVICE_FUNC RotationMatrixType matrix() const
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H={}) const