mag_utils.h

Go to the documentation of this file.

// Copyright (C) 2022 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 MAG_UTILS_H
#define MAG_UTILS_H
 
#include <Eigen/Dense>
#include <vector>
 
namespace mars
{
class MagnetometerInit
{
public:
  MagnetometerInit() = default;
 
  void AddElement(const Eigen::Vector3d& mag_vector, const Eigen::Vector3d& imu_linear_acc_vector);
 
  Eigen::Matrix3d get_rot() const;
 
  Eigen::Quaterniond get_quat() const;
 
  int get_size() const;
 
  void set_done();
 
  bool IsDone() const;
 
  void Reset();
 
  static Eigen::Vector3d mag_var_ang_to_vec(const double& dec, const double& inc, const double& r = 1);
 
  class MagImuData
  {
  public:
    MagImuData(const Eigen::Vector3d& mag_vec, const Eigen::Vector3d& imu_vec) : mag_vec_(mag_vec), imu_vec_(imu_vec)
    {
    }
    Eigen::Vector3d mag_vec_;
 
    Eigen::Vector3d imu_vec_;
  };
 
  MagImuData get_vec_mean() const;
 
private:
  std::vector<MagImuData> rot_init_vec_;
 
  bool once_{ false };
};
}  // namespace mars
 
#endif  // MAG_UTILS_H