From ca2ad23194985bb432a18a609a2cee9e7ca0f40f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Tue, 30 Jul 2019 17:15:24 +0200
Subject: [PATCH] gpst timestamp
---
include/gnss/capture/capture_gnss_fix.h | 9 +++++++++
.../gnss/capture/capture_gnss_single_diff.h | 9 +++++++++
include/gnss/sensor/sensor_gnss.h | 20 +++++++++----------
src/sensor/sensor_gnss.cpp | 7 -------
4 files changed, 28 insertions(+), 17 deletions(-)
diff --git a/include/gnss/capture/capture_gnss_fix.h b/include/gnss/capture/capture_gnss_fix.h
index 08af47ba2..4e0468f2e 100644
--- a/include/gnss/capture/capture_gnss_fix.h
+++ b/include/gnss/capture/capture_gnss_fix.h
@@ -18,6 +18,7 @@ class CaptureGnssFix : public CaptureBase
protected:
Eigen::VectorXs data_; ///< Raw data.
Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
+ TimeStamp ts_GPST_; ///< Time stamp in GPS time
public:
CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
@@ -25,6 +26,14 @@ class CaptureGnssFix : public CaptureBase
const Eigen::VectorXs& getData() const;
const Eigen::MatrixXs& getDataCovariance() const;
void getDataAndCovariance(Eigen::VectorXs& data, Eigen::MatrixXs& data_cov) const;
+ const TimeStamp& getGpsTimeStamp() const
+ {
+ return ts_GPST_;
+ }
+ void setGpsTimeStamp(const TimeStamp& _ts_GPST)
+ {
+ ts_GPST_ = _ts_GPST;
+ }
};
inline const Eigen::VectorXs& CaptureGnssFix::getData() const
diff --git a/include/gnss/capture/capture_gnss_single_diff.h b/include/gnss/capture/capture_gnss_single_diff.h
index 3423496bd..3da98be38 100644
--- a/include/gnss/capture/capture_gnss_single_diff.h
+++ b/include/gnss/capture/capture_gnss_single_diff.h
@@ -19,6 +19,7 @@ class CaptureGnssSingleDiff : public CaptureBase
Eigen::Vector3s data_; ///< Raw data.
Eigen::Matrix3s data_covariance_; ///< Noise of the capture.
FrameBasePtr origin_frame_ptr_;
+ TimeStamp ts_GPST_; ///< Time stamp in GPS time
public:
CaptureGnssSingleDiff(const TimeStamp& _ts,
@@ -36,6 +37,14 @@ class CaptureGnssSingleDiff : public CaptureBase
const Eigen::Matrix3s& getDataCovariance() const;
void getDataAndCovariance(Eigen::Vector3s& data, Eigen::Matrix3s& data_cov) const;
FrameBasePtr getOriginFrame() const;
+ const TimeStamp& getGpsTimeStamp() const
+ {
+ return ts_GPST_;
+ }
+ void setGpsTimeStamp(const TimeStamp& _ts_GPST)
+ {
+ ts_GPST_ = _ts_GPST;
+ }
};
inline const Eigen::Vector3s& CaptureGnssSingleDiff::getData() const
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index 13686e032..5efc012fd 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -144,22 +144,22 @@ inline const Eigen::Vector3s& SensorGnss::gettEnuEcef() const
inline Eigen::Vector3s SensorGnss::gettEnuMap() const
{
- return getStateBlockPtrStatic(3)->getState();
+ return getEnuMapTranslation()->getState();
}
template<typename T>
inline Eigen::Matrix<T,3,3> SensorGnss::computeREnuMap(const T& _r,const T& _p,const T& _y) const
{
- Eigen::Matrix<T,3,3> R_ENU_MAP = (Eigen::AngleAxis<T>(_r, Eigen::Matrix<T,3,1>::UnitX())
- * Eigen::AngleAxis<T>(_p, Eigen::Matrix<T,3,1>::UnitY())
- * Eigen::AngleAxis<T>(_y, Eigen::Matrix<T,3,1>::UnitZ())).toRotationMatrix();
-
- return R_ENU_MAP;
+ return Eigen::Matrix<T,3,3>(Eigen::AngleAxis<T>(_r, Eigen::Matrix<T,3,1>::UnitX()) *
+ Eigen::AngleAxis<T>(_p, Eigen::Matrix<T,3,1>::UnitY()) *
+ Eigen::AngleAxis<T>(_y, Eigen::Matrix<T,3,1>::UnitZ()));
+}
- // TODO: store R's when T=Scalar
- //Eigen::Matrix<T,3,3> R_roll_T, R_pitch_T, R_yaw_T;
- //if (!getRollState()->isFixed())
- // *R_roll_ptr_ = Eigen::AngleAxis<Scalar>(0.25*M_PI, Eigen::Vector3s::UnitX());
+inline Eigen::Matrix3s SensorGnss::getREnuMap() const
+{
+ return computeREnuMap(getEnuMapRoll() ->getState()(0),
+ getEnuMapPitch()->getState()(0),
+ getEnuMapYaw() ->getState()(0));
}
} // namespace wolf
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index 34f0e73e8..e38f10486 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -245,13 +245,6 @@ void SensorGnss::setEnuMapYawState(const Scalar& yaw_ENU_MAP)
R_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)
--
GitLab