GPSTypes.h

Go to the documentation of this file.

#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: http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html
        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:
        // Sophus/Eigen implementation
        Eigen::Vector3f tbg;
        bool mbIsSet;
    };
}
#endif