25 using namespace gtsam;
47 const double angle = 2.5;
49 SO3 R = SO3::AxisAngle(axis, angle);
56 M << 0.79067393, 0.6051136, -0.0930814,
57 0.4155925, -0.64214347, -0.64324489,
58 -0.44948549, 0.47046326, -0.75917576;
61 expected << 0.790687, 0.605096, -0.0931312,
62 0.415746, -0.642355, -0.643844,
63 -0.449411, 0.47036, -0.759468;
65 auto actual = SO3::ClosestTo(3 * M);
94 auto X1 = SOn::Hat(z_axis), X2 = SOn::Hat(
v2), X3 = SOn::Hat(
v3);
130 EXPECT(check_group_invariants(
id,
id));
131 EXPECT(check_group_invariants(
id,
R1));
132 EXPECT(check_group_invariants(
R2,
id));
135 EXPECT(check_manifold_invariants(
id,
id));
136 EXPECT(check_manifold_invariants(
id,
R1));
137 EXPECT(check_manifold_invariants(
R2,
id));
160 double angle = 3.14 / 4.0;
162 expected << 0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388;
165 so3::ExpmapFunctor
f1(axis, angle);
170 so3::ExpmapFunctor
f2(axis, angle - 2*
M_PI);
175 so3::ExpmapFunctor
f3(axis * angle);
180 so3::ExpmapFunctor
f4(axis * (angle - 2*
M_PI));
187 static const Vector3 w(0.1, 0.27, -0.2);
200 const Matrix actualDexpL = SO3::ExpmapDerivative(
w);
201 const Matrix expectedDexpL =
202 numericalDerivative11<Vector3, Vector3>(
testDexpL, Vector3::Zero(), 1
e-2);
205 const Matrix actualDexpInvL = SO3::LogmapDerivative(
w);
211 const Vector3 theta(0.1, 0, 0.1);
212 const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
213 std::bind(&
SO3::Expmap, std::placeholders::_1,
nullptr), theta);
217 SO3::ExpmapDerivative(-theta)));
222 const Vector3 theta(10, 20, 30);
223 const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
224 std::bind(&
SO3::Expmap, std::placeholders::_1,
nullptr), theta);
228 SO3::ExpmapDerivative(-theta)));
247 auto w_dot = [](
double t) {
return Vector3(2,
cos(
t), 8 *
t); };
252 for (
double t = -2.0;
t < 2.0;
t += 0.3) {
254 const Matrix actual = SO3::ExpmapDerivative(
w(
t)) * w_dot(
t);
262 auto w_dot = [](
double t) {
return Vector3(2,
cos(
t), 8 *
t); };
268 for (
double t = -2.0;
t < 2.0;
t += 0.3) {
270 const Matrix actual = SO3::ExpmapDerivative(
w(
t)) * w_dot(
t);
277 const Vector3 thetahat(0.1, 0, 0.1);
278 const Matrix Jexpected = numericalDerivative11<SO3, Vector3>(
279 std::bind(&
SO3::Expmap, std::placeholders::_1,
nullptr), thetahat);
287 const Vector3 thetahat(0.1, 0, 0.1);
289 const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
290 std::bind(&SO3::Logmap, std::placeholders::_1,
nullptr),
R);
291 const Matrix3 Jactual = SO3::LogmapDerivative(thetahat);
297 const Vector3 thetahat(0.1, 0, 0.1);
299 const Matrix Jexpected = numericalDerivative11<Vector, SO3>(
300 std::bind(&SO3::Logmap, std::placeholders::_1,
nullptr),
R);
302 SO3::Logmap(R, Jactual);
309 for (
bool nearZeroApprox : {
true,
false}) {
310 std::function<Vector3(const Vector3&, const Vector3&)>
f =
312 return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(
v);
316 so3::DexpFunctor local(omega, nearZeroApprox);
320 local.applyDexp(
v, aH1, aH2)));
332 for (
bool nearZeroApprox : {
true,
false}) {
333 std::function<Vector3(const Vector3&, const Vector3&)>
f =
335 return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(
v);
339 so3::DexpFunctor local(omega, nearZeroApprox);
340 Matrix invDexp = local.dexp().inverse();
344 local.applyInvDexp(v, aH1, aH2)));
357 const Vector9 actual =
R2.
vec(actualH);
359 std::function<Vector9(const SO3&)>
f = [](
const SO3&
Q) {
return Q.
vec(); };
367 M << 1, 2, 3, 4, 5, 6, 7, 8, 9;
373 std::function<Matrix3(const Matrix3&)>
f = [
R](
const Matrix3&
M) {
Matrix< RealScalar, Dynamic, Dynamic > M
Jet< T, N > cos(const Jet< T, N > &f)
#define GTSAM_CONCEPT_ASSERT(concept)
Concept check for values that can be used in unit tests.
static int runAllTests(TestResult &result)
SO inverse() const
inverse of a rotation = transpose
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.
Vector3 testDexpL(const Vector3 &dw)
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Rot2 R(Rot2::fromAngle(0.1))
Jet< T, N > sin(const Jet< T, N > &f)
Pose2_ Expmap(const Vector3_ &xi)
double f2(const Vector2 &x)
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)
#define CHECK_LIE_GROUP_DERIVATIVES(t1, t2)
static const Vector3 w(0.1, 0.27, -0.2)
const MatrixNN & matrix() const
Return matrix.
#define EXPECT(condition)
Array< int, Dynamic, 1 > v
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
Vector::Scalar omega(const Vector &t, const Vector &s, RealScalar angle)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
#define EXPECT_LONGS_EQUAL(expected, actual)
The quaternion class used to represent 3D orientations and rotations.
Point2 f1(const Point3 &p, OptionalJacobian< 2, 3 > H)
double f4(double x, double y, double z)
Class between(const Class &g) const
#define CHECK_CHART_DERIVATIVES(t1, t2)
double f3(double x1, double x2)
internal::FixedSizeMatrix< Y, X2 >::type numericalDerivative22(std::function< Y(const X1 &, const X2 &)> h, const X1 &x1, const X2 &x2, double delta=1e-5)
Class expmap(const TangentVector &v) const
Represents a 3D rotation as a rotation angle around an arbitrary 3D axis.
VectorN2 vec(OptionalJacobian< internal::NSquaredSO(N), dimension > H={}) const