Gnss.h

Go to the documentation of this file.

/*
Copyright 2017 by Dominic Jud, Robotic Systems Lab, ETH Zurich.
All rights reserved.
This file is released under the "BSD-3-Clause License".
Please see the LICENSE file that has been included as part of this package.
 */
 
#ifndef GNSS_H
#define GNSS_H
 
#pragma once
 
#include <Eigen/Dense>
 
namespace holistic_fusion {
 
class Gnss {
 public:
  Gnss();
  virtual ~Gnss() = default;
 
  // Setters
  void setReference(const double& referenceLatitude, const double& referenceLongitude, const double& referenceAltitude,
                    const double& referenceHeading);
  void setMercatorReferenceFrame(const Eigen::Vector3d newReferencePoint);
 
  // Conversion methods
  // Default
  Eigen::Vector3d gnssToCartesian(const double& latitudeInDegrees, const double& longitudeInDegrees, const double& altitude);
  Eigen::Vector3d cartesianToGps(const Eigen::Matrix<double, 3, 1> position) const;
  Eigen::Vector3d besselEllipsoidToMercator(const double& latitudeInRad, const double& longitudeInRad, const double& altitude);
  // LV03
  Eigen::Vector3d gnssToLv03Raw(const double latitude_in_degrees, const double longitude_in_degrees, const double altitude);
  Eigen::Vector3d gnssToLv03(const double latitude_in_degrees, const double longitude_in_degrees, const double altitude);
  double wGStoCHx(double lat, double lng);
  double wGStoCHy(double lat, double lng);
  double decToSexAngle(double dec);
  double sexAngleToSeconds(double dms);
 
  // Getters
  double getReferenceLongitude() const { return referenceLongitude_; }
  double getReferenceLatitude() const { return referenceLatitude_; }
  double getReferenceAltitude() const { return referenceAltitude_; }
  double getReferenceHeading() const { return referenceHeading_; }
 
 protected:
  void calculateConversionParameters();

  double referenceLongitude_ = 7.43958;

  double referenceLatitude_ = 46.95241;

  double referenceAltitude_ = 0.0;

  double referenceHeading_ = 0.0;
 
  // LV03 Reference
  Eigen::Vector3d referencePointLv03_ = Eigen::Vector3d::Zero();
 
  // Conversion parameters
  double earthRadiusN_ = 0.0;
  double earthRadiusE_ = 0.0;
 
  // WGS84 constants for conversion
  const double equatorialRadius_ = 6378137.0;
  const double flattening_ = 1.0 / 298.257223563;
  // calculate WGS84 constants
  const double excentricity2_ = 2 * flattening_ - flattening_ * flattening_;
 
  // As described in :
  // https://www.swisstopo.admin.ch/content/swisstopo-internet/de/topics/survey/reference-systems/switzerland/_jcr_content/contentPar/tabs/items/dokumente_publikatio/tabPar/downloadlist/downloadItems/517_1459343190376.download/refsys_d.pdf
  // Major Axis Bessel Ellipsoid
  const double a_ = 6377397.155;
  // 1. numerical Excentricity Bessel Ellipsoid
  const double e2_ = 0.006674372230614;
  // Latitude Bern
  const double phi0_ = 46.95241 * M_PI / 180.0;
  // Longitude Bern
  const double lambda0_ = 7.43958 * M_PI / 180.0;
  // Projection Sphere Radius
  const double r_ = a_ * sqrt(1 - e2_) / (1 - e2_ * sin(phi0_) * sin(phi0_));
  // Ratio Sphere Length to Ellipsoid Length;
  const double alpha_ = sqrt(1 + e2_ / (1 - e2_) * pow(cos(phi0_), 4));
  // Latitude of Zero Point on Sphere
  const double b0_ = asin(sin(phi0_) / alpha_);
  // Konstante der Breitenformel
  const double k_ = log(tan(M_PI_4 + b0_ / 2.0)) - alpha_ * log(tan(M_PI_4 + phi0_ / 2.0)) +
                    alpha_ * sqrt(e2_) / 2.0 * log((1 + sqrt(e2_) * sin(phi0_)) / (1 - sqrt(e2_) * sin(phi0_)));
  // xyz-offset
  Eigen::Vector3d xyzOffset_ = Eigen::Vector3d::Zero();
};
 
}  // namespace holistic_fusion
 
#endif  // GNSS_H