33 typedef QUATERNION_TYPE ManifoldType;
34 typedef QUATERNION_TYPE Q;
52 typedef Eigen::Matrix<_Scalar, 3, 1, _Options, 3, 1> TangentVector;
57 static Q Compose(
const Q &g,
const Q & h,
58 ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
59 if (Hg) *Hg = h.toRotationMatrix().transpose();
64 static Q Between(
const Q &g,
const Q & h,
65 ChartJacobian Hg = boost::none, ChartJacobian Hh = boost::none) {
66 Q d = g.inverse() * h;
67 if (Hg) *Hg = -d.toRotationMatrix().transpose();
72 static Q Inverse(
const Q &g,
73 ChartJacobian H = boost::none) {
74 if (H) *H = -g.toRotationMatrix();
79 static Q
Expmap(
const Eigen::Ref<const TangentVector>& omega,
80 ChartJacobian H = boost::none) {
84 _Scalar theta2 = omega.dot(omega);
85 if (theta2 > std::numeric_limits<_Scalar>::epsilon()) {
86 _Scalar theta = std::sqrt(theta2);
87 _Scalar ha = _Scalar(0.5) * theta;
88 Vector3 vec = (sin(ha) / theta) * omega;
89 return Q(cos(ha), vec.x(), vec.y(), vec.z());
92 Vector3 vec = _Scalar(0.5) * omega;
93 return Q(1.0, vec.x(), vec.y(), vec.z());
98 static TangentVector
Logmap(
const Q& q, ChartJacobian H = boost::none) {
103 static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
104 NearlyNegativeOne = -1.0 + 1e-10;
108 const _Scalar qw = q.w();
110 if (qw > NearlyOne) {
113 omega = ( 8. / 3. - 2. / 3. * qw) * q.vec();
114 }
else if (qw < NearlyNegativeOne) {
117 omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
121 _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
125 else if (angle < -M_PI)
127 omega = (angle / s) * q.vec();
130 _Scalar angle = 2 * acos(-qw), s = sqrt(1 - qw * qw);
133 else if (angle < -M_PI)
135 omega = (angle / s) * -q.vec();
147 static TangentVector Local(
const Q& g,
const Q& h,
148 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
149 Q b = Between(g, h, H1, H2);
151 TangentVector v = Logmap(b, (H1 || H2) ? &D_v_b : 0);
152 if (H1) *H1 = D_v_b * (*H1);
153 if (H2) *H2 = D_v_b * (*H2);
157 static Q Retract(
const Q& g,
const TangentVector& v,
158 ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
160 Q b = Expmap(v,H2 ? &D_h_v : 0);
161 Q h = Compose(g, b, H1, H2);
162 if (H2) *H2 = (*H2) * D_h_v;
169 static void Print(
const Q& q,
const std::string& str =
"") {
171 std::cout <<
"Eigen::Quaternion: ";
173 std::cout << str <<
" ";
174 std::cout << q.vec().transpose() << std::endl;
176 static bool Equals(
const Q& q1,
const Q& q2,
double tol = 1e-8) {
177 return Between(q1, q2).vec().array().abs().maxCoeff() < tol;
static Q Expmap(const Eigen::Ref< const TangentVector > &omega, ChartJacobian H=boost::none)
Exponential map, using the inlined code from Eigen's conversion from axis/angle.
Definition Quaternion.h:79
static TangentVector Logmap(const Q &q, ChartJacobian H=boost::none)
We use our own Logmap, as there is a slight bug in Eigen.
Definition Quaternion.h:98