TYPED_TEST(EulerAnglesXyzDiffTest, testFiniteDifference)
{
typedef typename TestFixture::Scalar Scalar;
typedef typename TestFixture::Rotation Rotation;
typedef typename TestFixture::RotationDiff RotationDiff;
typedef typename TestFixture::Vector3 Vector3;
const double dt = 1.0e-3;
for (auto rotation : this->rotations) {
for (auto angularVelocity2 : this->angularVelocities) {
RotationDiff rotationDiff(angularVelocity2.toImplementation());
rot::LocalAngularVelocity<Scalar> angularVelocity(rotation, rotationDiff);
// Finite difference method for checking derivatives
Rotation rotationNext = rotation.boxPlus(dt*rotation.rotate(angularVelocity.vector()));
rotationNext.setUnique();
rotation.setUnique();
Vector3 dn = (rotationNext.toImplementation()-rotation.toImplementation())/dt;
ASSERT_NEAR(rotationDiff.x(),dn(0),1e-3) << " angular velocity: " << angularVelocity << " rotation: " << rotation << " rotationNext: " << rotationNext << " diff: " << rotationDiff << " approxdiff: " << dn.transpose();
ASSERT_NEAR(rotationDiff.y(),dn(1),1e-3) << " angular velocity: " << angularVelocity << "rotation: " << rotation << " rotationNext: " << rotationNext << " diff: " << rotationDiff << " approxdiff: " << dn.transpose();
ASSERT_NEAR(rotationDiff.z(),dn(2),1e-3) << " angular velocity: " << angularVelocity << " rotation: " << rotation << " rotationNext: " << rotationNext << " diff: " << rotationDiff << " approxdiff: " << dn.transpose();
}
}
}
