Start Madwick filters.

This commit is contained in:
Kyle Isom 2019-08-06 22:49:20 -07:00
parent 7a932f422a
commit 971f324d7f
9 changed files with 286 additions and 69 deletions

View File

@ -40,7 +40,12 @@ message("${${PROJECT_NAME}_SOURCES} -> libwrmath")
add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES})
add_executable(euler2quat tools/euler2quat.cc)
target_link_libraries(euler2quat ${PROJECT_NAME})
set_target_properties(${TESTNAME} PROPERTIES
set_target_properties(euler2quat PROPERTIES
FOLDER bin
RUNTIME_OUTPUT_DIRECTORY bin)
add_executable(quaternion tools/quaternion.cc)
target_link_libraries(quaternion ${PROJECT_NAME})
set_target_properties(quaternion PROPERTIES
FOLDER bin
RUNTIME_OUTPUT_DIRECTORY bin)
@ -84,6 +89,7 @@ endmacro()
package_add_gtest(vector_test test/vector_test.cc)
package_add_gtest(orientation_test test/orientation_test.cc)
package_add_gtest(quaternion_test test/quaternion_test.cc)
package_add_gtest(madgwick_test test/madgwick_test.cc)
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} --verbose DEPENDS ${TEST_EXECS})

View File

@ -0,0 +1,100 @@
#ifndef __WRMATH_FILTER_MADGWICK_H
#define __WRMATH_FILTER_MADGWICK_H
#include <wrmath/geom/vector.h>
#include <wrmath/geom/quaternion.h>
namespace wr {
namespace filter {
/// Madgwick implements an efficient orientation filter for IMUs.
///
/// \tparam T A floating point type.
template <typename T>
class Madgwick {
public:
/// The Madgwick filter is initialised with an identity quaternion.
Madgwick() : deltaT(0.0), previousSensorFrame(), sensorFrame() {};
/// The Madgwick filter is initialised with a sensor frame.
///
/// \param sf A sensor frame; if zero, the sensor frame will be
/// initialised as an identity quaternion.
Madgwick(geom::Vector<T, 3> sf) : deltaT(0.0), previousSensorFrame()
{
if (!sf.isZero()) {
sensorFrame = geom::quaternion(sf, 0.0);
}
}
/// Initialise the filter with a sensor frame quaternion.
///
/// \param sf A quaternion representing the current orientation.
Madgwick(geom::Quaternion<T> sf) :
deltaT(0.0), previousSensorFrame(), sensorFrame(sf) {};
/// Return the current orientation as measured by the filter.
///
/// \return The current sensor frame.
geom::Quaternion<T>
orientation() const
{
return this->sensorFrame;
}
/// Return the rate of change of the orientation of the earth frame
/// with respect to the sensor frame.
///
/// \param gyro A three-dimensional vector containing gyro readings
/// as \f$<\omega_x, \omega_y, \omega_z\>f$.
/// \return A quaternion representing the rate of angular change.
geom::Quaternion<T>
angularRate(const geom::Vector<T, 3> &gyro) const
{
return (this->sensorFrame * 0.5) * geom::Quaternion<T>(gyro, 0.0);
}
/// Update the sensor frame to a new frame.
///
void
updateFrame(const geom::Quaternion<T> &sf, T delta)
{
this->previousSensorFrame = this->sensorFrame;
this->sensorFrame = sf;
this->deltaT = delta;
}
/// Update the sensor frame with a gyroscope reading.
///
/// \param gyro A three-dimensional vector containing gyro readings
/// as \f$<\omega_x, \omega_y, \omega_z\>f$.
/// \param delta The time step between readings. It must not be zero.
void
updateAngularOrientation(const geom::Vector<T, 3> &gyro, T delta)
{
assert(!math::WithinTolerance(delta, 0.0, 0.001));
geom::Quaternion<T> q = this->angularRate(gyro) * delta;
this->updateFrame(this->sensorFrame + q, delta);
}
private:
T deltaT;
geom::Quaternion<T> previousSensorFrame;
geom::Quaternion<T> sensorFrame;
};
} // namespace filter
} // namespace wr
#endif // __WRMATH_FILTER_MADGWICK_H

View File

@ -24,7 +24,7 @@ namespace geom {
/// Quaternions encode rotations in three-dimensional space. While technically
/// a quaternion is comprised of a real element and a complex vector<3>, for
/// the purposes of this library, it is modeled as a floating point 4D vector
/// of the form <x, y, z, w>, where x, y, and z represent an axis of rotation in
/// of the form <w, x, y, z>, where x, y, and z represent an axis of rotation in
/// R3 and w the angle, in radians, of the rotation about that axis. Where Euler
/// angles are concerned, the ZYX (or yaw, pitch, roll) sequence is used.
///
@ -43,9 +43,7 @@ namespace geom {
template<typename T>
class Quaternion {
public:
///
/// The default Quaternion constructor returns an identity quaternion.
///
Quaternion() : v(Vector<T, 3>{0.0, 0.0, 0.0}), w(1.0)
{
wr::math::DefaultEpsilon(this->eps);
@ -56,6 +54,7 @@ public:
/// A Quaternion may be initialised with a Vector<T, 3> axis of rotation
/// and an angle of rotation. This doesn't do the angle transforms to simplify
/// internal operations.
///
/// @param _axis A three-dimensional vector of the same type as the Quaternion.
/// @param _angle The angle of rotation about the axis of rotation.
Quaternion(Vector<T, 3> _axis, T _angle) : v(_axis), w(_angle)
@ -65,14 +64,14 @@ public:
v.setEpsilon(this->eps);
};
///
/// A Quaternion may be initialised with a Vector<T, 4> comprised of
/// the axis of rotation followed by the angle of rotation.
/// @param vector A vector in the form <x, y, z, w>.
///
/// @param vector A vector in the form <w, x, y, z>.
Quaternion(Vector<T, 4> vector) :
v(Vector<T, 3>{vector[0], vector[1], vector[2]}),
w(vector[3])
v(Vector<T, 3>{vector[1], vector[2], vector[3]}),
w(vector[0])
{
this->constrainAngle();
wr::math::DefaultEpsilon(this->eps);
@ -80,15 +79,16 @@ public:
}
/// A Quaternion may be constructed with an initializer list of type T, which must have
/// exactly N element.
/// @param ilst An initial set of values in the form <x, y, z, w>.
/// A Quaternion may be constructed with an initializer list of
/// type T, which must have exactly N elements.
///
/// @param ilst An initial set of values in the form <w, x, y, z>.
Quaternion(std::initializer_list<T> ilst)
{
auto it = ilst.begin();
this->v = Vector<T, 3>{it[0], it[1], it[2]};
this->w = it[3];
this->v = Vector<T, 3>{it[1], it[2], it[3]};
this->w = it[0];
this->constrainAngle();
wr::math::DefaultEpsilon(this->eps);
@ -97,6 +97,7 @@ public:
/// Set the comparison tolerance for this quaternion.
///
/// @param epsilon A tolerance value.
void
setEpsilon(T epsilon)
@ -107,6 +108,7 @@ public:
/// Return the axis of rotation of this quaternion.
///
/// @return The axis of rotation of this quaternion.
Vector<T, 3>
axis() const
@ -116,6 +118,7 @@ public:
/// Return the angle of rotation of this quaternion.
///
/// @return the angle of rotation of this quaternion.
T
angle() const
@ -168,15 +171,17 @@ public:
}
/// Compute the conjugate of a quaternion.
///
/// @return The conjugate of this quaternion.
Quaternion
conjugate() const
{
return Quaternion(Vector<T, 4>{-this->v[0], -this->v[1], -this->v[2], this->w});
return Quaternion(Vector<T, 4>{this->w, -this->v[0], -this->v[1], -this->v[2]});
}
/// Compute the inverse of a quaternion.
///
/// @return The inverse of this quaternion.
Quaternion
inverse() const
@ -187,7 +192,18 @@ public:
}
/// Determine whether this is an identity quaternion.
///
/// \return true if this is an identity quaternion.
bool
isIdentity() const {
return this->v.isZero() &&
math::WithinTolerance(this->w, (T)1.0, this->eps);
}
/// Determine whether this is a unit quaternion.
///
/// @return true if this is a unit quaternion.
bool
isUnitQuaternion() const
@ -198,15 +214,17 @@ public:
/// Return the quaternion as a Vector<T, 4>, with the axis of rotation
/// followed by the angle of rotation.
///
/// @return A vector representation of the quaternion.
Vector<T, 4>
asVector() const
{
return Vector<T, 4>{this->v[0], this->v[1], this->v[2], this->w};
return Vector<T, 4>{this->w, this->v[0], this->v[1], this->v[2]};
}
/// Rotate vector v about this quaternion.
///
/// @param v The vector to be rotated.
/// @return The rotated vector.
Vector<T, 3>
@ -219,6 +237,7 @@ public:
/// Return the Euler angles for this quaternion as a vector of
/// <yaw, pitch, roll>. Users of this function should watch out
/// for gimbal lock.
///
/// @return A vector<T, 3> containing <yaw, pitch, roll>
Vector<T, 3>
euler() const
@ -238,6 +257,7 @@ public:
/// Perform quaternion addition with another quaternion.
///
/// @param other The quaternion to be added with this one.
/// @return The result of adding the two quaternions together.
Quaternion
@ -248,6 +268,7 @@ public:
/// Perform quaternion subtraction with another quaternion.
///
/// @param other The quaternion to be subtracted from this one.
/// @return The result of subtracting the other quaternion from this one.
Quaternion
@ -258,6 +279,7 @@ public:
/// Perform scalar multiplication.
///
/// @param k The scaling value.
/// @return A scaled quaternion.
Quaternion
@ -268,6 +290,7 @@ public:
/// Perform scalar division.
///
/// @param k The scalar divisor.
/// @return A scaled quaternion.
Quaternion
@ -280,6 +303,7 @@ public:
/// Perform quaternion Hamilton multiplication with a three-
/// dimensional vector; this is done by treating the vector
/// as a pure quaternion (e.g. with an angle of rotation of 0).
///
/// @param vector The vector to multiply with this quaternion.
/// @return The Hamilton product of the quaternion and vector.
Quaternion
@ -291,6 +315,7 @@ public:
/// Perform quaternion Hamilton multiplication.
///
/// @param other The other quaternion to multiply with this one.
/// @result The Hamilton product of the two quaternions.
Quaternion
@ -317,6 +342,7 @@ public:
/// Perform quaternion inequality checking.
///
/// @param other The quaternion to check inequality against.
/// @return True if the two quaternions are unequal within their tolerance.
bool
@ -328,6 +354,7 @@ public:
/// Support stream output of a quaternion in the form `a + <i, j, k>`.
/// \todo improve the formatting.
///
/// @param outs An output stream
/// @param q A quaternion
/// @return The output stream
@ -375,6 +402,7 @@ typedef Quaternion<double> Quaterniond;
/// Return a float quaternion scaled appropriately from a vector and angle,
/// e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2).
///
/// @param axis The axis of rotation.
/// @param angle The angle of rotation.
/// @return A quaternion.
@ -384,6 +412,7 @@ Quaternionf quaternionf(Vector3f axis, float angle);
/// Return a double quaternion scaled appropriately from a vector and angle,
/// e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2).
///
/// @param axis The axis of rotation.
/// @param angle The angle of rotation.
/// @return A quaternion.
@ -391,8 +420,25 @@ Quaternionf quaternionf(Vector3f axis, float angle);
Quaterniond quaterniond(Vector3d axis, double angle);
/// Return a double quaternion scaled appropriately from a vector and angle,
/// e.g. angle = cos(angle / 2), axis.unitVector() * sin(angle / 2).
///
/// @param axis The axis of rotation.
/// @param angle The angle of rotation.
/// @return A quaternion.
/// @relatesalso Quaternion
template <typename T>
Quaternion<T>
quaternion(Vector<T, 3> axis, T angle)
{
return Quaternion<T>(axis.unitVector() * std::sin(angle / (T)2.0),
std::cos(angle / (T)2.0));
}
/// Given a vector of Euler angles in ZYX sequence (e.g. yaw, pitch, roll),
/// return a quaternion.
///
/// @param euler A vector Euler angle in ZYX sequence.
/// @return A Quaternion representation of the orientation represented
/// by the Euler angles.
@ -402,6 +448,7 @@ Quaternionf quaternionf_from_euler(Vector3f euler);
/// Given a vector of Euler angles in ZYX sequence (e.g. yaw, pitch, roll),
/// return a quaternion.
///
/// @param euler A vector Euler angle in ZYX sequence.
/// @return A Quaternion representation of the orientation represented
/// by the Euler angles.

1
src/madgwick.cc Normal file
View File

@ -0,0 +1 @@
#include "wrmath/filter/madgwick.h"

View File

@ -1,3 +1,4 @@
#include <iostream>
#include <wrmath/geom/quaternion.h>
@ -39,7 +40,7 @@ quaternionf_from_euler(Vector3f euler)
z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll);
w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll);
return Quaternionf(Vector4f{x, y, z, w});
return Quaternionf(Vector4f{w, x, y, z});
}
@ -61,7 +62,7 @@ quaterniond_from_euler(Vector3d euler)
z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll);
w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll);
return Quaterniond(Vector4d{x, y, z, w});
return Quaterniond(Vector4d{w, x, y, z});
}
@ -78,6 +79,7 @@ Quaternion_SelfTest()
Vector3f vr {0.0, 0.0, 1.0};
assert(p.isUnitQuaternion());
std::cerr << p.rotate(v) << std::endl;
assert(p.rotate(v) == vr);
assert(p * q == p);
#endif

34
test/madgwick_test.cc Normal file
View File

@ -0,0 +1,34 @@
#include <cmath>
#include <sstream>
#include <gtest/gtest.h>
#include <wrmath/geom/vector.h>
#include <wrmath/geom/quaternion.h>
#include <wrmath/filter/madgwick.h>
using namespace std;
using namespace wr;
TEST(MadgwickFilter, SimpleAngularOrientation)
{
filter::Madgwick<double> mf;
geom::Vector3d gyro {0.17453292519943295, 0.0, 0.0}; // 10° X rotation.
geom::Quaterniond frame20Deg {0.984808, 0.173648, 0, 0}; // 20° final orientation.
double delta = 0.00917; // assume 109 updates per second, as per the paper.
// The paper specifies a minimum of 109 IMU readings to stabilize; for
// two seconds, that means 218 updates.
for (int i = 0; i < 218; i++) {
mf.updateAngularOrientation(gyro, delta);
}
EXPECT_EQ(mf.orientation(), frame20Deg);
}
int
main(int argc, char **argv)
{
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

View File

@ -15,9 +15,9 @@ TEST(Quaternion, SelfTest)
TEST(Quaterniond, Addition)
{
geom::Quaterniond p(geom::Vector4d {1.0, -2.0, 1.0, 3.0});
geom::Quaterniond q(geom::Vector4d {-1.0, 2.0, 3.0, 2.0});
geom::Quaterniond expected(geom::Vector4d{0.0, 0.0, 4.0, 5.0});
geom::Quaterniond p(geom::Vector4d {3.0, 1.0, -2.0, 1.0});
geom::Quaterniond q(geom::Vector4d {2.0, -1.0, 2.0, 3.0});
geom::Quaterniond expected(geom::Vector4d{5.0, 0.0, 0.0, 4.0});
EXPECT_EQ(p + q, expected);
EXPECT_EQ(expected - q, p);
@ -27,8 +27,8 @@ TEST(Quaterniond, Addition)
TEST(Quaterniond, Conjugate)
{
geom::Quaterniond p(geom::Vector4d {3.0, 4.0, 5.0, 2.0});
geom::Quaterniond q(geom::Vector4d {-3.0, -4.0, -5.0, 2.0});
geom::Quaterniond p {2.0, 3.0, 4.0, 5.0};
geom::Quaterniond q {2.0, -3.0, -4.0, -5.0};
EXPECT_EQ(p.conjugate(), q);
}
@ -45,17 +45,18 @@ TEST(Quaterniond, Euler)
TEST(Quaterniond, Identity)
{
geom::Quaterniond p(geom::Vector4d {1.0, -2.0, 1.0, 3.0});
geom::Quaterniond p {3.0, 1.0, -2.0, 1.0};
geom::Quaterniond q;
EXPECT_TRUE(q.isIdentity());
EXPECT_EQ(p * q, p);
}
TEST(Quaterniond, Inverse)
{
geom::Quaterniond p(geom::Vector4d {3.0, 4.0, 5.0, 2.0});
geom::Quaterniond q(geom::Vector4d {-0.05556, -0.07407, -0.09259, 0.03704 });
geom::Quaterniond p {2.0, 3.0, 4.0, 5.0};
geom::Quaterniond q {0.03704, -0.05556, -0.07407, -0.09259};
EXPECT_EQ(p.inverse(), q);
}
@ -63,8 +64,7 @@ TEST(Quaterniond, Inverse)
TEST(Quaterniond, Norm)
{
geom::Quaterniond p(geom::Vector4d {0.9899139811480784, 9.387110042325054, 6.161341707794767,
5.563199889674063});
geom::Quaterniond p {5.563199889674063, 0.9899139811480784, 9.387110042325054, 6.161341707794767};
double norm = 12.57016663729933;
EXPECT_DOUBLE_EQ(p.norm(), norm);
@ -73,9 +73,9 @@ TEST(Quaterniond, Norm)
TEST(Quaterniond, Product)
{
geom::Quaterniond p(geom::Vector4d {1.0, -2.0, 1.0, 3.0});
geom::Quaterniond q(geom::Vector4d {-1.0, 2.0, 3.0, 2.0});
geom::Quaterniond expected(geom::Vector4d{-9.0, -2.0, 11.0, 8.0});
geom::Quaterniond p {3.0, 1.0, -2.0, 1.0};
geom::Quaterniond q {2.0, -1.0, 2.0, 3.0};
geom::Quaterniond expected {8.0, -9.0, -2.0, 11.0};
EXPECT_EQ(p * q, expected);
}
@ -111,10 +111,10 @@ TEST(Quaterniond, ShortestSLERP)
{
// Our starting point is an orientation that is yawed 45° - our
// orientation is pointed π/4 radians in the X axis.
geom::Quaterniond p {0.382683, 0, 0, 0.92388};
geom::Quaterniond p {0.92388, 0.382683, 0, 0};
// Our ending point is an orientation that is yawed -45° - or
// pointed -π/4 radians in the X axis.
geom::Quaterniond q {-0.382683, 0, 0, 0.92388};
geom::Quaterniond q {0.92388, -0.382683, 0, 0};
// The halfway point should be oriented midway about the X axis. It turns
// out this is an identity quaternion.
geom::Quaterniond r;
@ -129,13 +129,13 @@ TEST(Quaterniond, ShortestSLERP2)
{
// Start with an orientation pointing forward, all Euler angles
// set to 0.
geom::Quaterniond start {0.0, 0.0, 0.0, 1.0};
geom::Quaterniond start {1.0, 0.0, 0.0, 0.0};
// The goal is to end up face up, or 90º pitch (still facing forward).
geom::Quaterniond end {0, -0.707107, 0, 0.707107};
geom::Quaterniond end {0.707107, 0, -0.707107, 0};
// Halfway to the endpoint should be a 45º pitch.
geom::Quaterniond halfway {0, -0.382683, 0, 0.92388 };
geom::Quaterniond halfway {0.92388, 0, -0.382683, 0};
// 2/3 of the way should be 60º pitch.
geom::Quaterniond twoThirds {0, -0.5, 0, 0.866025};
geom::Quaterniond twoThirds {0.866025, 0, -0.5, 0};
EXPECT_EQ(ShortestSLERP(start, end, 0.0), start);
EXPECT_EQ(ShortestSLERP(start, end, 1.0), end);
@ -146,7 +146,7 @@ TEST(Quaterniond, ShortestSLERP2)
TEST(Quaterniond, Unit)
{
geom::Quaterniond q(geom::Vector4d{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0});
geom::Quaterniond q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258};
EXPECT_TRUE(q.isUnitQuaternion());
}
@ -157,7 +157,7 @@ TEST(Quaterniond, UtilityCreator)
geom::Vector3d v {1.0, 1.0, 1.0};
double w = M_PI;
geom::Quaterniond p = geom::quaterniond(v, w);
geom::Quaterniond q(geom::Vector4d{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0});
geom::Quaterniond q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258};
EXPECT_EQ(p, q);
}
@ -165,9 +165,9 @@ TEST(Quaterniond, UtilityCreator)
TEST(Quaternionf, Addition)
{
geom::Quaternionf p(geom::Vector4f {1.0, -2.0, 1.0, 3.0});
geom::Quaternionf q(geom::Vector4f {-1.0, 2.0, 3.0, 2.0});
geom::Quaternionf expected(geom::Vector4f{0.0, 0.0, 4.0, 5.0});
geom::Quaternionf p {3.0, 1.0, -2.0, 1.0};
geom::Quaternionf q {2.0, -1.0, 2.0, 3.0};
geom::Quaternionf expected {5.0, 0.0, 0.0, 4.0};
EXPECT_EQ(p + q, expected);
EXPECT_EQ(expected - q, p);
@ -177,8 +177,8 @@ TEST(Quaternionf, Addition)
TEST(Quaternionf, Conjugate)
{
geom::Quaternionf p(geom::Vector4f {3.0, 4.0, 5.0, 2.0});
geom::Quaternionf q(geom::Vector4f {-3.0, -4.0, -5.0, 2.0});
geom::Quaternionf p {2.0, 3.0, 4.0, 5.0};
geom::Quaternionf q {2.0, -3.0, -4.0, -5.0};
EXPECT_EQ(p.conjugate(), q);
}
@ -195,7 +195,7 @@ TEST(Quaternionf, Euler)
TEST(Quaternionf, Identity)
{
geom::Quaternionf p(geom::Vector4f {1.0, -2.0, 1.0, 3.0});
geom::Quaternionf p {1.0, 3.0, 1.0, -2.0};
geom::Quaternionf q;
EXPECT_EQ(p * q, p);
@ -204,8 +204,8 @@ TEST(Quaternionf, Identity)
TEST(Quaternionf, Inverse)
{
geom::Quaternionf p(geom::Vector4f {3.0, 4.0, 5.0, 2.0});
geom::Quaternionf q(geom::Vector4f {-0.05556, -0.07407, -0.09259, 0.03704 });
geom::Quaternionf p {2.0, 3.0, 4.0, 5.0};
geom::Quaternionf q {0.03704, -0.05556, -0.07407, -0.09259};
EXPECT_EQ(p.inverse(), q);
}
@ -213,10 +213,7 @@ TEST(Quaternionf, Inverse)
TEST(Quaternionf, Norm)
{
geom::Quaternionf p(geom::Vector4f {0.9899139811480784,
9.387110042325054,
6.161341707794767,
5.563199889674063});
geom::Quaternionf p {0.9899139811480784, 9.387110042325054, 6.161341707794767, 5.563199889674063};
float norm = 12.57016663729933;
EXPECT_FLOAT_EQ(p.norm(), norm);
@ -225,9 +222,9 @@ TEST(Quaternionf, Norm)
TEST(Quaternionf, Product)
{
geom::Quaternionf p(geom::Vector4f {1.0, -2.0, 1.0, 3.0});
geom::Quaternionf q(geom::Vector4f {-1.0, 2.0, 3.0, 2.0});
geom::Quaternionf expected(geom::Vector4f{-9.0, -2.0, 11.0, 8.0});
geom::Quaternionf p {3.0, 1.0, -2.0, 1.0};
geom::Quaternionf q {2.0, -1.0, 2.0, 3.0};
geom::Quaternionf expected {8.0, -9.0, -2.0, 11.0};
EXPECT_EQ(p * q, expected);
}
@ -251,10 +248,10 @@ TEST(Quaternionf, ShortestSLERP)
{
// Our starting point is an orientation that is yawed 45° - our
// orientation is pointed π/4 radians in the X axis.
geom::Quaternionf p {0.382683, 0, 0, 0.92388};
geom::Quaternionf p {0.92388, 0.382683, 0, 0};
// Our ending point is an orientation that is yawed -45° - or
// pointed -π/4 radians in the X axis.
geom::Quaternionf q {-0.382683, 0, 0, 0.92388};
geom::Quaternionf q {0.92388, -0.382683, 0, 0};
// The halfway point should be oriented midway about the X axis. It turns
// out this is an identity quaternion.
geom::Quaternionf r;
@ -269,13 +266,13 @@ TEST(Quaternionf, ShortestSLERP2)
{
// Start with an orientation pointing forward, all Euler angles
// set to 0.
geom::Quaternionf start {0.0, 0.0, 0.0, 1.0};
geom::Quaternionf start {1.0, 0.0, 0.0, 0.0};
// The goal is to end up face up, or 90º pitch (still facing forward).
geom::Quaternionf end {0, -0.707107, 0, 0.707107};
geom::Quaternionf end {0.707107, 0, -0.707107, 0};
// Halfway to the endpoint should be a 45º pitch.
geom::Quaternionf halfway {0, -0.382683, 0, 0.92388 };
geom::Quaternionf halfway {0.92388, 0, -0.382683, 0};
// 2/3 of the way should be 60º pitch.
geom::Quaternionf twoThirds {0, -0.5, 0, 0.866025};
geom::Quaternionf twoThirds {0.866025, 0, -0.5, 0};
EXPECT_EQ(ShortestSLERP(start, end, (float)0.0), start);
EXPECT_EQ(ShortestSLERP(start, end, (float)1.0), end);
@ -286,7 +283,7 @@ TEST(Quaternionf, ShortestSLERP2)
TEST(Quaternionf, Unit)
{
geom::Quaternionf q(geom::Vector4f{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0});
geom::Quaternionf q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258};
EXPECT_TRUE(q.isUnitQuaternion());
}
@ -297,7 +294,7 @@ TEST(Quaternionf, UtilityCreator)
geom::Vector3f v {1.0, 1.0, 1.0};
float w = M_PI;
geom::Quaternionf p = geom::quaternionf(v, w);
geom::Quaternionf q(geom::Vector4f{0.5773502691896258, 0.5773502691896258, 0.5773502691896258, 0.0});
geom::Quaternionf q {0.0, 0.5773502691896258, 0.5773502691896258, 0.5773502691896258};
EXPECT_EQ(p, q);
}
@ -305,20 +302,22 @@ TEST(Quaternionf, UtilityCreator)
TEST(QuaternionMiscellaneous, SanityChecks)
{
geom::Vector4d q {1.0, 2.0, 3.0, 4.0};
geom::Vector4d q {4.0, 1.0, 2.0, 3.0};
geom::Vector3d v {1.0, 2.0, 3.0};
double w = 4.0;
geom::Quaterniond p(q);
geom::Quaterniond u = p.unitQuaternion();
EXPECT_EQ(p.axis(), v);
EXPECT_DOUBLE_EQ(p.angle(), w);
EXPECT_TRUE(u.isUnitQuaternion());
}
TEST(QuaternionMiscellaneous, OutputStream)
{
geom::Quaternionf p(geom::Vector4f {1.0, 2.0, 3.0, 4.0});
geom::Quaterniond q(geom::Vector4d {1.0, 2.0, 3.0, 4.0});
geom::Quaternionf p {4.0, 1.0, 2.0, 3.0};
geom::Quaterniond q {4.0, 1.0, 2.0, 3.0};
stringstream ss;
ss << p;

View File

@ -34,12 +34,12 @@ convertEulerToQuat(char **argv)
static void
convertQuatToEuler(char **argv)
{
double x = stod(string(argv[0]));
double y = stod(string(argv[1]));
double z = stod(string(argv[1]));
double w = stod(string(argv[1]));
double x = stod(string(argv[1]));
double y = stod(string(argv[2]));
double z = stod(string(argv[3]));
double w = stod(string(argv[0]));
geom::Quaterniond quaternion {x, y, z, w};
geom::Quaterniond quaternion {w, x, y, z};
auto euler = quaternion.euler() * (180.0 / M_PI);
cout << "Euler ZYX: " << euler << endl;

28
tools/quaternion.cc Normal file
View File

@ -0,0 +1,28 @@
#include <cstdlib>
#include <iostream>
#include <string>
#include <wrmath/geom/vector.h>
#include <wrmath/geom/quaternion.h>
using namespace std;
using namespace wr::geom;
int
main(int argc, char **argv)
{
if (argc != 5) {
cerr << "Usage: quaternion w x y z" << endl;
return EXIT_FAILURE;
}
double w = stod(string(argv[1]));
double x = stod(string(argv[2]));
double y = stod(string(argv[3]));
double z = stod(string(argv[4]));
Vector3d frame {x, y, z};
Quaterniond quat = quaternion(frame, w);
cout << quat << endl;
}