forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
82 lines
3.0 KiB
82 lines
3.0 KiB
2 years ago
|
/// @file
|
||
|
|
/// Hopf coordinates parametrizes SO(3) locally as the Cartesian product of a
|
||
|
|
/// one-sphere and a two-sphere S¹ x S². Computationally, each rotation in the
|
||
|
|
/// Hopf coordinates can be written as (θ, φ, ψ), in which ψ parametrizes the
|
||
|
|
/// circle S¹ and has a range of 2π, and θ, φ represent the spherical
|
||
|
|
/// coordinates for S², with the range of π and 2π respectively.
|
||
|
|
|
||
|
|
#pragma once
|
||
|
|
|
||
|
|
#include <cmath>
|
||
|
|
|
||
|
|
#include <Eigen/Dense>
|
||
|
|
#include <Eigen/Geometry>
|
||
|
|
|
||
|
|
#include "drake/common/drake_assert.h"
|
||
|
|
#include "drake/common/eigen_types.h"
|
||
|
|
|
||
|
|
namespace drake {
|
||
|
|
namespace math {
|
||
|
|
/**
|
||
|
|
* Transforms Hopf coordinates to a quaternion w, x, y, z as
|
||
|
|
* w = cos(θ/2)cos(ψ/2)
|
||
|
|
* x = cos(θ/2)sin(ψ/2)
|
||
|
|
* y = sin(θ/2)cos(φ+ψ/2)
|
||
|
|
* z = sin(θ/2)sin(φ+ψ/2)
|
||
|
|
* The user can refer to equation 5 of
|
||
|
|
* Generating Uniform Incremental Grids on SO(3) Using the Hopf Fibration
|
||
|
|
* by Anna Yershova, Steven LaValle and Julie Mitchell, 2008
|
||
|
|
* @param theta The θ angle.
|
||
|
|
* @param phi The φ angle.
|
||
|
|
* @param psi The ψ angle.
|
||
|
|
*/
|
||
|
|
template <typename T>
|
||
|
|
const Eigen::Quaternion<T> HopfCoordinateToQuaternion(const T& theta,
|
||
|
|
const T& phi,
|
||
|
|
const T& psi) {
|
||
|
|
using std::cos;
|
||
|
|
using std::sin;
|
||
|
|
const T cos_half_theta = cos(theta / 2);
|
||
|
|
const T sin_half_theta = sin(theta / 2);
|
||
|
|
const T phi_plus_half_psi = phi + psi / 2;
|
||
|
|
return Eigen::Quaternion(cos_half_theta * cos(psi / 2),
|
||
|
|
cos_half_theta * sin(psi / 2),
|
||
|
|
sin_half_theta * cos(phi_plus_half_psi),
|
||
|
|
sin_half_theta * sin(phi_plus_half_psi));
|
||
|
|
}
|
||
|
|
|
||
|
|
/**
|
||
|
|
* Convert a unit-length quaternion (w, x, y, z) (with the requirement w >= 0)
|
||
|
|
* to Hopf coordinate as
|
||
|
|
* ψ = 2*atan2(x, w)
|
||
|
|
* φ = mod(atan2(z, y) - ψ/2, 2pi)
|
||
|
|
* θ = 2*atan2(√(y²+z²), √(w²+x²))
|
||
|
|
* ψ is in the range of [-pi, pi].
|
||
|
|
* φ is in the range of [0, 2pi].
|
||
|
|
* θ is in the range of [0, pi].
|
||
|
|
* If the given quaternion has w < 0, then we reverse the signs of (w, x, y, z),
|
||
|
|
* and compute the Hopf coordinate of (-w, -x, -y, -z).
|
||
|
|
* @param quaternion A unit length quaternion.
|
||
|
|
* @return hopf_coordinate (θ, φ, ψ) as an Eigen vector.
|
||
|
|
*/
|
||
|
|
template <typename T>
|
||
|
|
Vector3<T> QuaternionToHopfCoordinate(const Eigen::Quaternion<T>& quaternion) {
|
||
|
|
if (quaternion.w() < 0) {
|
||
|
|
return QuaternionToHopfCoordinate(Eigen::Quaternion<T>(
|
||
|
|
-quaternion.w(), -quaternion.x(), -quaternion.y(), -quaternion.z()));
|
||
|
|
}
|
||
|
|
using std::atan2;
|
||
|
|
const T psi = 2 * atan2(quaternion.x(), quaternion.w());
|
||
|
|
const T phi_unwrapped = atan2(quaternion.z(), quaternion.y()) - psi / 2;
|
||
|
|
// The range of phi_unwrapped is [-1.5pi, 1.5pi]
|
||
|
|
const T phi = phi_unwrapped >= 0 ? phi_unwrapped : phi_unwrapped + 2 * M_PI;
|
||
|
|
using std::pow;
|
||
|
|
using std::sqrt;
|
||
|
|
const T theta =
|
||
|
|
2 * atan2(sqrt(pow(quaternion.y(), 2) + pow(quaternion.z(), 2)),
|
||
|
|
sqrt(pow(quaternion.w(), 2) + pow(quaternion.x(), 2)));
|
||
|
|
return Vector3<T>(theta, phi, psi);
|
||
|
|
}
|
||
|
|
} // namespace math.
|
||
|
|
} // namespace drake
|