Start Madwick filters.
This commit is contained in:
parent
7a932f422a
commit
971f324d7f
|
|
@ -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})
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
#include "wrmath/filter/madgwick.h"
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
Loading…
Reference in New Issue