GPSTypes.h

#pragma once
#include <NDEVR/Vertex.h>
#if _WIN32
#pragma warning( push )
#pragma warning( disable : 4127)
#include <Eigen/Core>
#include <Eigen/Geometry>
#pragma warning( pop ) 
 
namespace NDEVR
{
    class GlobalPosition
    {
    protected:
        GlobalPosition()
        {}
        GlobalPosition(int id, double timestamp)
            : timestamp(timestamp)
            , id(id)
        {}
 
    public:
        virtual ~GlobalPosition() {}
        virtual Eigen::Vector3d getRelativeFromOrigin(const GlobalPosition& origin) const = 0;
        virtual Eigen::MatrixXd getCovariance() const = 0;
        double timestamp; 
        int id; 
    };
    class GpsMeasurement : public GlobalPosition
    {
    public:
        GpsMeasurement() {}
        GpsMeasurement(const int id, const double& latitude, const double& longitude, const double& altitude, const double& timestamp, const Eigen::Matrix3d& covariance, const Eigen::Vector3d noise = Eigen::Vector3d(0., 0., 0.)) :
            GlobalPosition(id, timestamp), lat_lon_location(latitude, longitude, altitude), noise(noise), covariance(covariance) {}
        Vertex<3, fltp08> lat_lon_location; 
        Eigen::Vector3d noise; 
        Eigen::Matrix3d covariance; 
 
        Eigen::Vector3d getRelativeFromOrigin(const GlobalPosition& origin) const override;
        Eigen::MatrixXd getCovariance() const override;
 
    };

    class MocapMeasurement : public GlobalPosition
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        MocapMeasurement() {}
        MocapMeasurement(const int id,
            const double& x, const double& y, const double& z,
            const double& qx, const double& qy, const double& qz,
            const double& qw, const double& timestamp,
            const Eigen::Matrix3d& covariance,
            const Eigen::Vector3d noise = Eigen::Vector3d(0., 0., 0.))
            : GlobalPosition(id, timestamp)
            , noise(noise)
            , covariance(covariance)
        {
            position = Eigen::Vector3d(x, y, z);
            rotation = Eigen::Quaterniond(qw, qx, qy, qz);
        }
        Eigen::Vector3d position; 
        Eigen::Quaterniond rotation; 
        Eigen::Vector3d noise; 
        Eigen::Matrix3d covariance; 
        Eigen::Vector3d getRelativeFromOrigin(const GlobalPosition& origin) const override;
        Eigen::MatrixXd getCovariance() const override;
    };
    class GPSCalib
    {
    public:
        GPSCalib(const Eigen::Vector3f& tbg)
            : tbg(tbg)
            , mbIsSet(true)
        {}
        GPSCalib()
            : mbIsSet(false)
        {}
    public:
        Eigen::Vector3f tbg; 
        bool mbIsSet; 
    };
}
#endif