utils.h

Go to the documentation of this file.

// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>
 
#ifndef UTILS_H
#define UTILS_H
 
#include <mars/sensors/imu/imu_measurement_type.h>
#include <Eigen/Dense>
#include <string>
#include <vector>
 
namespace mars
{
class Utils
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  Utils();
 
  static std::string get_mars_version_string();
 
  static void PrintMarsVersion();
 
  static void TransformImu(const mars::IMUMeasurementType& prev, const mars::IMUMeasurementType& now, const double& dt,
                           const Eigen::Vector3d& p_ab, const Eigen::Quaterniond& q_ab,
                           mars::IMUMeasurementType& result);
 
  static void TransformImu(const mars::IMUMeasurementType& now, const Eigen::Vector3d& p_ab,
                           const Eigen::Quaterniond& q_ab, mars::IMUMeasurementType& result);
 
  static Eigen::Matrix3d Skew(const Eigen::Vector3d& v);
 
  static Eigen::Matrix4d MatExp(const Eigen::Matrix4d& A, const int& order = 4);
 
  static Eigen::Matrix4d OmegaMat(const Eigen::Vector3d& v);
 
  static Eigen::Quaterniond QuatFromSmallAngle(const Eigen::Vector3d& d_theta_vec);
 
  static Eigen::Quaterniond ApplySmallAngleQuatCorr(const Eigen::Quaterniond& q_prior,
                                                    const Eigen::Vector3d& correction);
 
  static Eigen::Vector3d RPYFromRotMat(const Eigen::Matrix3d& rot_mat);
 
  static bool CheckCov(const Eigen::MatrixXd& cov_mat, const std::string& description, const bool& check_cond = false);
 
  static Eigen::MatrixXd EnforceMatrixSymmetry(const Eigen::Ref<const Eigen::MatrixXd>& mat_in);
 
  static Eigen::Quaterniond quaternionAverage(const std::vector<Eigen::Quaterniond>& quats);
 
  template <typename T, typename A>
  static std::vector<T> VecExtractEveryNthElm(std::vector<T, A> const& data, const int& nth)
  {
    std::vector<T> every_nth_data;
    const int len = data.size();
 
    for (int k = 0; k < len - nth; k += nth)
    {
      every_nth_data.push_back(data[k]);
    }
    return every_nth_data;
  }
 
  static Eigen::Quaterniond NormalizeQuaternion(const Eigen::Quaterniond& quat, std::string note = "");
  static Eigen::Quaterniond NormalizeQuaternion(const double& w, const double& x, const double& y, const double& z,
                                                std::string note = "");
};
}  // namespace mars
 
#endif  // UTILS_H