euler_angles_zxy.h

Go to the documentation of this file.

/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#pragma once

// TODO(all): should use Angle class internally.

#include <cmath>

#include "Eigen/Geometry"

#include "modules/common/math/math_utils.h"

namespace apollo {
namespace common {
namespace math {

template <typename T>
class EulerAnglesZXY {
 public:
  EulerAnglesZXY() : roll_(0), pitch_(0), yaw_(0) {}

  explicit EulerAnglesZXY(T yaw) : roll_(0), pitch_(0), yaw_(yaw) {}

  EulerAnglesZXY(T roll, T pitch, T yaw)
      : roll_(roll), pitch_(pitch), yaw_(yaw) {}

  EulerAnglesZXY(T qw, T qx, T qy, T qz)
      : roll_(std::atan2(static_cast<T>(2.0) * (qw * qy - qx * qz),
                         static_cast<T>(2.0) * (Square<T>(qw) + Square<T>(qz)) -
                             static_cast<T>(1.0))),
        pitch_(std::asin(static_cast<T>(2.0) * (qw * qx + qy * qz))),
        yaw_(std::atan2(static_cast<T>(2.0) * (qw * qz - qx * qy),
                        static_cast<T>(2.0) * (Square<T>(qw) + Square<T>(qy)) -
                            static_cast<T>(1.0))) {}

  explicit EulerAnglesZXY(const Eigen::Quaternion<T> &q)
      : EulerAnglesZXY(q.w(), q.x(), q.y(), q.z()) {}

  T roll() const { return roll_; }

  T pitch() const { return pitch_; }

  T yaw() const { return yaw_; }

  void Normalize() {
    roll_ = NormalizeAngle(roll_);
    pitch_ = NormalizeAngle(pitch_);
    yaw_ = NormalizeAngle(yaw_);
  }

  bool IsValid() {
    Normalize();
    return pitch_ < M_PI_2 && pitch_ > -M_PI_2;
  }

  Eigen::Quaternion<T> ToQuaternion() const {
    T coeff = static_cast<T>(0.5);
    T r = roll_ * coeff;
    T p = pitch_ * coeff;
    T y = yaw_ * coeff;

    T sr = std::sin(r);
    T sp = std::sin(p);
    T sy = std::sin(y);

    T cr = std::cos(r);
    T cp = std::cos(p);
    T cy = std::cos(y);

    T qw = cr * cp * cy - sr * sp * sy;
    T qx = cr * sp * cy - sr * cp * sy;
    T qy = cr * sp * sy + sr * cp * cy;
    T qz = cr * cp * sy + sr * sp * cy;
    if (qw < 0.0) {
      return {-qw, -qx, -qy, -qz};
    }
    return {qw, qx, qy, qz};
  }

 private:
  T roll_;
  T pitch_;
  T yaw_;
};

using EulerAnglesZXYf = EulerAnglesZXY<float>;
using EulerAnglesZXYd = EulerAnglesZXY<double>;

}  // namespace math
}  // namespace common
}  // namespace apollo