gps_conversion.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 GPS_CONVERSION_H
#define GPS_CONVERSION_H
 
#include <Eigen/Dense>
 
namespace mars
{
struct GpsCoordinates
{
  GpsCoordinates() = default;
  GpsCoordinates(double latitude, double longitude, double altitude)
    : latitude_(latitude), longitude_(longitude), altitude_(altitude)
  {
  }
  double latitude_{ 0 };
  double longitude_{ 0 };
  double altitude_{ 0 };
 
  friend std::ostream& operator<<(std::ostream& out, const GpsCoordinates& coordinates);
 
  inline GpsCoordinates operator+(const GpsCoordinates& coordinates)
  {
    return { latitude_ + coordinates.latitude_, longitude_ + coordinates.longitude_,
             altitude_ + coordinates.altitude_ };
  }
 
  inline void operator+=(const GpsCoordinates& coordinates)
  {
    latitude_ += coordinates.latitude_;
    longitude_ += coordinates.longitude_;
    altitude_ += coordinates.altitude_;
  }
 
  inline void operator/=(const double& n)
  {
    if (n == 0)
    {
      throw std::overflow_error("Divide by zero exception");
    }
 
    latitude_ /= n;
    longitude_ /= n;
    altitude_ /= n;
  }
};
 
class GpsConversion
{
public:
  GpsConversion(mars::GpsCoordinates coordinates);
  GpsConversion() = default;
 
  Eigen::Matrix<double, 3, 1> get_enu(mars::GpsCoordinates coordinates);
 
  mars::GpsCoordinates get_gps_reference();
 
  void set_gps_reference(mars::GpsCoordinates coordinates);
 
private:
  GpsCoordinates reference_;  
  Eigen::Matrix3d ecef_ref_orientation_;
  Eigen::Matrix<double, 3, 1> ecef_ref_point_;
  bool reference_is_set{ false };
  double deg2rad(const double& deg);
 
  Eigen::Matrix<double, 3, 1> WGS84ToECEF(const mars::GpsCoordinates& coordinates);
 
  Eigen::Matrix<double, 3, 1> ECEFToENU(const Eigen::Matrix<double, 3, 1>& ecef);
 
  Eigen::Matrix<double, 3, 1> WGS84ToENU(const mars::GpsCoordinates& coordinates);
};
}  // namespace mars
#endif  // GPS_CONVERSION_H