added function getREnuMap() and reorganized .h

include/gnss/sensor/sensor_gnss.h

@@ -55,32 +55,34 @@ class SensorGnss : public SensorBase
 
         virtual ~SensorGnss();
 
-        // ENU
-        void computeEnuEcef(const Eigen::Vector3s& _t_ECEF_ENU, Eigen::Matrix3s& R_ENU_ECEF, Eigen::Vector3s& t_ENU_ECEF) const;
-        void setEcefEnu(const Eigen::Vector3s& _t_ECEF_ENU);
-        void setEnuEcef(const Eigen::Matrix3s& _R_ENU_ECEF, const Eigen::Vector3s& _t_ENU_ECEF);
-        bool isEnuDefined() const;
-        bool isEnuMapInitialized() const;
-
-        // ENU_MAP
+        // Gets
         StateBlockPtr getEnuMapTranslation() const;
         StateBlockPtr getEnuMapRoll() const;
         StateBlockPtr getEnuMapPitch() const;
         StateBlockPtr getEnuMapYaw() const;
-        void initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, const Eigen::Vector3s& _t_ECEF_antenaENU,
-                              const Eigen::VectorXs& _pose_MAP_frame2, const Eigen::Vector3s& _t_ECEF_antena2);
-        void initializeEnuMapYaw(const Eigen::VectorXs& _pose_MAP_frame1, const Eigen::VectorXs& _pose_MAP_frame2,
-                                 const Eigen::Vector3s& _v_ECEF_antena1_antena2);
+        const Eigen::Matrix3s& getREnuEcef() const;
+        const Eigen::Vector3s& gettEnuEcef() const;
+        Eigen::Matrix3s getREnuMap() const;
+        Eigen::Vector3s gettEnuMap() const;
+        bool isEnuDefined() const;
+        bool isEnuMapInitialized() const;
+
+        // Sets
         void setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP);
         void setEnuMapRollState(const Scalar& roll_ENU_MAP);
         void setEnuMapPitchState(const Scalar& pitch_ENU_MAP);
         void setEnuMapYawState(const Scalar& yaw_ENU_MAP);
+        void setEcefEnu(const Eigen::Vector3s& _t_ECEF_ENU);
+        void setEnuEcef(const Eigen::Matrix3s& _R_ENU_ECEF, const Eigen::Vector3s& _t_ENU_ECEF);
 
-        // Gets
-        const Eigen::Matrix3s& getREnuEcef() const;
-        const Eigen::Vector3s& gettEnuEcef() const;
+        // compute
         template<typename T>
         Eigen::Matrix<T,3,3> computeREnuMap(const T& _r,const T& _p,const T& _y) const;
+        void computeEnuEcef(const Eigen::Vector3s& _t_ECEF_ENU, Eigen::Matrix3s& R_ENU_ECEF, Eigen::Vector3s& t_ENU_ECEF) const;
+        void initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, const Eigen::Vector3s& _t_ECEF_antenaENU,
+                              const Eigen::VectorXs& _pose_MAP_frame2, const Eigen::Vector3s& _t_ECEF_antena2);
+        void initializeEnuMapYaw(const Eigen::VectorXs& _pose_MAP_frame1, const Eigen::VectorXs& _pose_MAP_frame2,
+                                 const Eigen::Vector3s& _v_ECEF_antena1_antena2);
 
     public:
         static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p, const IntrinsicsBasePtr _intrinsics);

src/sensor/sensor_gnss.cpp

@@ -233,6 +233,13 @@ void SensorGnss::setEnuMapYawState(const Scalar& yaw_ENU_MAP)
     ENU_MAP_initialized_ = true;
 }
 
+Eigen::Matrix3s SensorGnss::getREnuMap() const
+{
+    return Eigen::Matrix3s(Eigen::AngleAxis<Scalar>(getEnuMapRoll() ->getState()(0), Eigen::Vector3s::UnitX()) *
+                           Eigen::AngleAxis<Scalar>(getEnuMapPitch()->getState()(0), Eigen::Vector3s::UnitY()) *
+                           Eigen::AngleAxis<Scalar>(getEnuMapYaw()  ->getState()(0), Eigen::Vector3s::UnitZ()));
+}
+
 // Define the factory method
 SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics,
                               const IntrinsicsBasePtr _intrinsics)