diff --git a/include/gnss/capture/capture_gnss_fix.h b/include/gnss/capture/capture_gnss_fix.h
index 08af47ba2b24986727b7233e7e33ff7b2f90a68f..2676ba2143ed31c414702f05100829c632522d89 100644
--- a/include/gnss/capture/capture_gnss_fix.h
+++ b/include/gnss/capture/capture_gnss_fix.h
@@ -16,28 +16,28 @@ WOLF_PTR_TYPEDEFS(CaptureGnssFix);
class CaptureGnssFix : public CaptureBase
{
protected:
- Eigen::VectorXs data_; ///< Raw data.
- Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
+ Eigen::VectorXd data_; ///< Raw data.
+ Eigen::MatrixXd data_covariance_; ///< Noise of the capture.
public:
- CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
+ CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_covariance);
virtual ~CaptureGnssFix();
- const Eigen::VectorXs& getData() const;
- const Eigen::MatrixXs& getDataCovariance() const;
- void getDataAndCovariance(Eigen::VectorXs& data, Eigen::MatrixXs& data_cov) const;
+ const Eigen::VectorXd& getData() const;
+ const Eigen::MatrixXd& getDataCovariance() const;
+ void getDataAndCovariance(Eigen::VectorXd& data, Eigen::MatrixXd& data_cov) const;
};
-inline const Eigen::VectorXs& CaptureGnssFix::getData() const
+inline const Eigen::VectorXd& CaptureGnssFix::getData() const
{
return data_;
}
-inline const Eigen::MatrixXs& CaptureGnssFix::getDataCovariance() const
+inline const Eigen::MatrixXd& CaptureGnssFix::getDataCovariance() const
{
return data_covariance_;
}
-inline void CaptureGnssFix::getDataAndCovariance(Eigen::VectorXs& data, Eigen::MatrixXs& data_cov) const
+inline void CaptureGnssFix::getDataAndCovariance(Eigen::VectorXd& data, Eigen::MatrixXd& data_cov) const
{
data = data_;
data_cov = data_covariance_;
diff --git a/include/gnss/capture/capture_gnss_single_diff.h b/include/gnss/capture/capture_gnss_single_diff.h
index 3423496bdb5305db6fd4f4fca891546fe9c2c261..811caf993448570510762e970418bfd701b38691 100644
--- a/include/gnss/capture/capture_gnss_single_diff.h
+++ b/include/gnss/capture/capture_gnss_single_diff.h
@@ -16,15 +16,15 @@ WOLF_PTR_TYPEDEFS(CaptureGnssSingleDiff);
class CaptureGnssSingleDiff : public CaptureBase
{
protected:
- Eigen::Vector3s data_; ///< Raw data.
- Eigen::Matrix3s data_covariance_; ///< Noise of the capture.
+ Eigen::Vector3d data_; ///< Raw data.
+ Eigen::Matrix3d data_covariance_; ///< Noise of the capture.
FrameBasePtr origin_frame_ptr_;
public:
CaptureGnssSingleDiff(const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
- const Eigen::Vector3s& _data,
- const Eigen::Matrix3s& _data_covariance,
+ const Eigen::Vector3d& _data,
+ const Eigen::Matrix3d& _data_covariance,
const FrameBasePtr& _origin_frame_ptr) :
CaptureBase("GNSS SINGLE DIFFERENCE", _ts, _sensor_ptr),
data_(_data),
@@ -32,23 +32,23 @@ class CaptureGnssSingleDiff : public CaptureBase
origin_frame_ptr_(_origin_frame_ptr)
{};
virtual ~CaptureGnssSingleDiff(){};
- const Eigen::Vector3s& getData() const;
- const Eigen::Matrix3s& getDataCovariance() const;
- void getDataAndCovariance(Eigen::Vector3s& data, Eigen::Matrix3s& data_cov) const;
+ const Eigen::Vector3d& getData() const;
+ const Eigen::Matrix3d& getDataCovariance() const;
+ void getDataAndCovariance(Eigen::Vector3d& data, Eigen::Matrix3d& data_cov) const;
FrameBasePtr getOriginFrame() const;
};
-inline const Eigen::Vector3s& CaptureGnssSingleDiff::getData() const
+inline const Eigen::Vector3d& CaptureGnssSingleDiff::getData() const
{
return data_;
}
-inline const Eigen::Matrix3s& CaptureGnssSingleDiff::getDataCovariance() const
+inline const Eigen::Matrix3d& CaptureGnssSingleDiff::getDataCovariance() const
{
return data_covariance_;
}
-inline void CaptureGnssSingleDiff::getDataAndCovariance(Eigen::Vector3s& data, Eigen::Matrix3s& data_cov) const
+inline void CaptureGnssSingleDiff::getDataAndCovariance(Eigen::Vector3d& data, Eigen::Matrix3d& data_cov) const
{
data = data_;
data_cov = data_covariance_;
diff --git a/include/gnss/feature/feature_gnss_fix.h b/include/gnss/feature/feature_gnss_fix.h
index 0184490f96fcda1fcadbb97ffc12ef872a1a65ff..01180c100b55d999da260385669877e5cb49566f 100644
--- a/include/gnss/feature/feature_gnss_fix.h
+++ b/include/gnss/feature/feature_gnss_fix.h
@@ -21,7 +21,7 @@ class FeatureGnssFix : public FeatureBase
* \param _meas_covariance the noise of the measurement
*
*/
- FeatureGnssFix(const Eigen::Vector3s& _measurement, const Eigen::Matrix3s& _meas_covariance) :
+ FeatureGnssFix(const Eigen::Vector3d& _measurement, const Eigen::Matrix3d& _meas_covariance) :
FeatureBase("GNSS FIX", _measurement, _meas_covariance)
{};
diff --git a/include/gnss/feature/feature_gnss_single_diff.h b/include/gnss/feature/feature_gnss_single_diff.h
index 903a062ced9817a0de1f297b817ee7d69aadac08..938a084a653088f8cf2c7585f379d7304ec7625b 100644
--- a/include/gnss/feature/feature_gnss_single_diff.h
+++ b/include/gnss/feature/feature_gnss_single_diff.h
@@ -21,7 +21,7 @@ class FeatureGnssSingleDiff : public FeatureBase
* \param _meas_covariance the noise of the measurement
*
*/
- FeatureGnssSingleDiff(const Eigen::Vector3s& _measurement, const Eigen::Matrix3s& _meas_covariance) :
+ FeatureGnssSingleDiff(const Eigen::Vector3d& _measurement, const Eigen::Matrix3d& _meas_covariance) :
FeatureBase("GNSS SINGLE DIFFERENCE", _measurement, _meas_covariance)
{};
diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
index ebeeefab58531feacb82cd98513b9df495fb7f20..c1ec2de7728a7782ea01f4fcf6b23446c7b6aeea 100644
--- a/include/gnss/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -17,16 +17,16 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssFix);
struct ProcessorParamsGnssFix : public ProcessorParamsBase
{
- Scalar time_th;
- Scalar dist_traveled;
- Scalar enu_map_init_dist_min;
+ double time_th;
+ double dist_traveled;
+ double enu_map_init_dist_min;
ProcessorParamsGnssFix() = default;
ProcessorParamsGnssFix(std::string _unique_name, const ParamsServer& _server):
ProcessorParamsBase(_unique_name, _server)
{
- time_th = _server.getParam<Scalar>(_unique_name + "/time_th");
- dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled");
- enu_map_init_dist_min = _server.getParam<Scalar>(_unique_name + "/enu_map_init_dist_min");
+ time_th = _server.getParam<double>(_unique_name + "/time_th");
+ dist_traveled = _server.getParam<double>(_unique_name + "/dist_traveled");
+ enu_map_init_dist_min = _server.getParam<double>(_unique_name + "/enu_map_init_dist_min");
}
std::string print() const
{
@@ -63,7 +63,7 @@ class ProcessorGnssFix : public ProcessorBase
*
* The ProcessorTracker only processes incoming captures (it is not called).
*/
- virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) override {};
+ virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override {};
/** \brief trigger in capture
*
@@ -75,7 +75,7 @@ class ProcessorGnssFix : public ProcessorBase
*
* The ProcessorTracker only processes incoming captures, then it returns false.
*/
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const Scalar& _time_tolerance) const override {return false;}
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override {return false;}
/** \brief store key frame
*
diff --git a/include/gnss/processor/processor_gnss_single_diff.h b/include/gnss/processor/processor_gnss_single_diff.h
index 12f7943c67d51a93438108186a5e8bd60c782031..b5140ea00792a600f81a4cc06546fe6254cca1b2 100644
--- a/include/gnss/processor/processor_gnss_single_diff.h
+++ b/include/gnss/processor/processor_gnss_single_diff.h
@@ -17,16 +17,16 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssSingleDiff);
struct ProcessorParamsGnssSingleDiff : public ProcessorParamsBase
{
- Scalar time_th;
- Scalar dist_traveled;
- Scalar enu_map_init_dist_min;
+ double time_th;
+ double dist_traveled;
+ double enu_map_init_dist_min;
ProcessorParamsGnssSingleDiff() = default;
ProcessorParamsGnssSingleDiff(std::string _unique_name, const ParamsServer& _server):
ProcessorParamsBase(_unique_name, _server)
{
- time_th = _server.getParam<Scalar>(_unique_name + "/time_th");
- dist_traveled = _server.getParam<Scalar>(_unique_name + "/dist_traveled");
- enu_map_init_dist_min = _server.getParam<Scalar>(_unique_name + "/enu_map_init_dist_min");
+ time_th = _server.getParam<double>(_unique_name + "/time_th");
+ dist_traveled = _server.getParam<double>(_unique_name + "/dist_traveled");
+ enu_map_init_dist_min = _server.getParam<double>(_unique_name + "/enu_map_init_dist_min");
}
std::string print() const
{
@@ -64,7 +64,7 @@ class ProcessorGnssSingleDiff : public ProcessorBase
*
* The ProcessorTracker only processes incoming captures (it is not called).
*/
- virtual void processKeyFrame(FrameBasePtr _keyframe, const Scalar& _time_tolerance) override {};
+ virtual void processKeyFrame(FrameBasePtr _keyframe, const double& _time_tolerance) override {};
/** \brief store key frame
*
@@ -88,7 +88,7 @@ class ProcessorGnssSingleDiff : public ProcessorBase
*
* The ProcessorTracker only processes incoming captures, then it returns false.
*/
- virtual bool triggerInKeyFrame(FrameBasePtr _keyframe, const Scalar& _time_tolerance) const override {return false;}
+ virtual bool triggerInKeyFrame(FrameBasePtr _keyframe, const double& _time_tolerance) const override {return false;}
virtual bool voteForKeyFrame() const override;
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index 66c4109261c2399919aab90014abe877d844c0d3..c9d4c18de4342cd595244b3e592d6d1bb256b627 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -46,8 +46,8 @@ class SensorGnss : public SensorBase
protected:
IntrinsicsGnssPtr params_;
bool ENU_defined_, ENU_MAP_initialized_;
- Eigen::Matrix3s R_ENU_ECEF_;
- Eigen::Vector3s t_ENU_ECEF_;
+ Eigen::Matrix3d R_ENU_ECEF_;
+ Eigen::Vector3d t_ENU_ECEF_;
public:
SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, IntrinsicsGnssPtr params);
@@ -60,32 +60,32 @@ class SensorGnss : public SensorBase
StateBlockPtr getEnuMapRoll() const;
StateBlockPtr getEnuMapPitch() const;
StateBlockPtr getEnuMapYaw() const;
- const Eigen::Matrix3s& getREnuEcef() const;
- const Eigen::Vector3s& gettEnuEcef() const;
- Eigen::Matrix3s getREnuMap() const;
- Eigen::Vector3s gettEnuMap() const;
+ const Eigen::Matrix3d& getREnuEcef() const;
+ const Eigen::Vector3d& gettEnuEcef() const;
+ Eigen::Matrix3d getREnuMap() const;
+ Eigen::Vector3d 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);
+ void setEnuMapTranslationState(const Eigen::Vector3d& t_ENU_MAP);
+ void setEnuMapRollState(const double& roll_ENU_MAP);
+ void setEnuMapPitchState(const double& pitch_ENU_MAP);
+ void setEnuMapYawState(const double& yaw_ENU_MAP);
+ void setEcefEnu(const Eigen::Vector3d& _t_ECEF_ENU);
+ void setEnuEcef(const Eigen::Matrix3d& _R_ENU_ECEF, const Eigen::Vector3d& _t_ENU_ECEF);
// 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);
+ void computeEnuEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF) const;
+ void initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, const Eigen::Vector3d& _t_ECEF_antenaENU,
+ const Eigen::VectorXd& _pose_MAP_frame2, const Eigen::Vector3d& _t_ECEF_antena2);
+ void initializeEnuMapYaw(const Eigen::VectorXd& _pose_MAP_frame1, const Eigen::VectorXd& _pose_MAP_frame2,
+ const Eigen::Vector3d& _v_ECEF_antena1_antena2);
public:
- static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_p, const IntrinsicsBasePtr _intrinsics);
+ static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics_p, const IntrinsicsBasePtr _intrinsics);
};
@@ -120,12 +120,12 @@ inline StateBlockPtr SensorGnss::getEnuMapYaw() const
return getStateBlockPtrStatic(6);
}
-inline const Eigen::Matrix3s& SensorGnss::getREnuEcef() const
+inline const Eigen::Matrix3d& SensorGnss::getREnuEcef() const
{
return R_ENU_ECEF_;
}
-inline const Eigen::Vector3s& SensorGnss::gettEnuEcef() const
+inline const Eigen::Vector3d& SensorGnss::gettEnuEcef() const
{
return t_ENU_ECEF_;
}
@@ -139,10 +139,10 @@ inline Eigen::Matrix<T,3,3> SensorGnss::computeREnuMap(const T& _r,const T& _p,c
return R_ENU_MAP;
- // TODO: store R's when T=Scalar
+ // TODO: store R's when T=double
//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());
+ // *R_roll_ptr_ = Eigen::AngleAxis<double>(0.25*M_PI, Eigen::Vector3d::UnitX());
}
} // namespace wolf
diff --git a/src/capture/capture_gnss_fix.cpp b/src/capture/capture_gnss_fix.cpp
index 34f76594d5cff7256726e9eb663bf6ca0b1f0c1d..caa2858cae7bb0d74b3fce6458f73efa06a00e8f 100644
--- a/src/capture/capture_gnss_fix.cpp
+++ b/src/capture/capture_gnss_fix.cpp
@@ -3,7 +3,7 @@
namespace wolf {
-CaptureGnssFix::CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
+CaptureGnssFix::CaptureGnssFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_covariance) :
CaptureBase("GNSS FIX", _ts, _sensor_ptr),
data_(_data),
data_covariance_(_data_covariance)
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index a7ce606d8f6841a4e59cfe74d16137af91d1b01e..3c8662c00cb104181a99cf044496e9ca530a96fb 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -114,18 +114,18 @@ bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac)
// Cast feature
auto gnss_ftr = std::static_pointer_cast<FeatureGnssFix>(fac->getFeature());
// copy states
- Eigen::VectorXs x(gnss_ftr->getCapture()->getFrame()->getP()->getState());
- Eigen::VectorXs o(gnss_ftr->getCapture()->getFrame()->getP()->getState());
- Eigen::VectorXs x_antena(sensor_gnss_->getStateBlock(0)->getState());
- Eigen::VectorXs t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
- Eigen::VectorXs roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
- Eigen::VectorXs pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
- Eigen::VectorXs yaw_ENU_map(sensor_gnss_->getEnuMapYaw()->getState());
- // create Scalar* array of a copy of the state
- Scalar* parameters[7] = {x.data(), o.data(), x_antena.data(), t_ENU_map.data(), roll_ENU_map.data(),
+ Eigen::VectorXd x(gnss_ftr->getCapture()->getFrame()->getP()->getState());
+ Eigen::VectorXd o(gnss_ftr->getCapture()->getFrame()->getP()->getState());
+ Eigen::VectorXd x_antena(sensor_gnss_->getStateBlock(0)->getState());
+ Eigen::VectorXd t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
+ Eigen::VectorXd roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
+ Eigen::VectorXd pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
+ Eigen::VectorXd yaw_ENU_map(sensor_gnss_->getEnuMapYaw()->getState());
+ // create double* array of a copy of the state
+ double* parameters[7] = {x.data(), o.data(), x_antena.data(), t_ENU_map.data(), roll_ENU_map.data(),
pitch_ENU_map.data(), yaw_ENU_map.data()};
// create residuals pointer
- Eigen::VectorXs residuals(3);
+ Eigen::VectorXd residuals(3);
// evaluate the factor in this state
fac->evaluate(parameters, residuals.data(), nullptr);
// discard if residual too high evaluated at the current estimation
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index cb41fabb8f177605f32f8a9cdd53a2af32b8d2d3..0246eeda39e43170c5697c160da8bd7b24e811b8 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -114,9 +114,9 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame() const
// Distance criterion
std::cout << "params_gnss_->dist_traveled" << params_gnss_->dist_traveled << std::endl;
- Eigen::Vector2s v_current_origin = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
+ Eigen::Vector2d v_current_origin = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
std::cout << "v_current_origin: " << v_current_origin.transpose() << std::endl;
- Eigen::Vector2s v_origin_last_KF = last_KF_->getP()->getState() - incoming_capture_->getOriginFrame()->getP()->getState();
+ Eigen::Vector2d v_origin_last_KF = last_KF_->getP()->getState() - incoming_capture_->getOriginFrame()->getP()->getState();
std::cout << "v_origin_last_KF: " << v_origin_last_KF.transpose() << std::endl;
std::cout << "v_current_origin + v_origin_last_KF: " << (v_current_origin + v_origin_last_KF).transpose() << std::endl;
if ((v_current_origin + v_origin_last_KF).norm() > params_gnss_->dist_traveled)
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index d422362c3300eed41696dd290a164dc9a2b01505..51c4661760eebba624949a3ae5c6b63cfb42c829 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -5,11 +5,11 @@
namespace wolf {
// Geodetic system parameters
-static Scalar kSemimajorAxis = 6378137;
-static Scalar kSemiminorAxis = 6356752.3142;
-static Scalar kFirstEccentricitySquared = 6.69437999014 * 0.001;
-static Scalar kSecondEccentricitySquared = 6.73949674228 * 0.001;
-//static Scalar kFlattening = 1 / 298.257223563;
+static double kSemimajorAxis = 6378137;
+static double kSemiminorAxis = 6356752.3142;
+static double kFirstEccentricitySquared = 6.69437999014 * 0.001;
+static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
+//static double kFlattening = 1 / 298.257223563;
SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D position with respect to the car frame (base frame)
StateBlockPtr _bias_ptr, //GNSS sensor time bias
@@ -61,34 +61,34 @@ SensorGnss::~SensorGnss()
//
}
-void SensorGnss::computeEnuEcef(const Eigen::Vector3s& _t_ECEF_ENU, Eigen::Matrix3s& R_ENU_ECEF, Eigen::Vector3s& t_ENU_ECEF) const
+void SensorGnss::computeEnuEcef(const Eigen::Vector3d& _t_ECEF_ENU, Eigen::Matrix3d& R_ENU_ECEF, Eigen::Vector3d& t_ENU_ECEF) const
{
// Convert ECEF coordinates to geodetic coordinates.
// J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates
// to geodetic coordinates," IEEE Transactions on Aerospace and
// Electronic Systems, vol. 30, pp. 957-961, 1994.
- Scalar r = std::sqrt(_t_ECEF_ENU(0) * _t_ECEF_ENU(0) + _t_ECEF_ENU(1) * _t_ECEF_ENU(1));
- Scalar Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
- Scalar F = 54 * kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) * _t_ECEF_ENU(2);
- Scalar G = r * r + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) - kFirstEccentricitySquared * Esq;
- Scalar C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
- Scalar S = cbrt(1 + C + sqrt(C * C + 2 * C));
- Scalar P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
- Scalar Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
- Scalar r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
+ double r = std::sqrt(_t_ECEF_ENU(0) * _t_ECEF_ENU(0) + _t_ECEF_ENU(1) * _t_ECEF_ENU(1));
+ double Esq = kSemimajorAxis * kSemimajorAxis - kSemiminorAxis * kSemiminorAxis;
+ double F = 54 * kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) * _t_ECEF_ENU(2);
+ double G = r * r + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) - kFirstEccentricitySquared * Esq;
+ double C = (kFirstEccentricitySquared * kFirstEccentricitySquared * F * r * r) / pow(G, 3);
+ double S = cbrt(1 + C + sqrt(C * C + 2 * C));
+ double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
+ double Q = sqrt(1 + 2 * kFirstEccentricitySquared * kFirstEccentricitySquared * P);
+ double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
+ sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q)
- P * (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2) / (Q * (1 + Q)) - 0.5 * P * r * r);
- Scalar V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2));
- Scalar Z_0 = kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) / (kSemimajorAxis * V);
+ double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + (1 - kFirstEccentricitySquared) * _t_ECEF_ENU(2) * _t_ECEF_ENU(2));
+ double Z_0 = kSemiminorAxis * kSemiminorAxis * _t_ECEF_ENU(2) / (kSemimajorAxis * V);
- Scalar latitude = atan((_t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
- Scalar longitude = atan2(_t_ECEF_ENU(1), _t_ECEF_ENU(0));
+ double latitude = atan((_t_ECEF_ENU(2) + kSecondEccentricitySquared * Z_0) / r);
+ double longitude = atan2(_t_ECEF_ENU(1), _t_ECEF_ENU(0));
- Scalar sLat = sin(latitude);
- Scalar cLat = cos(latitude);
- Scalar sLon = sin(longitude);
- Scalar cLon = cos(longitude);
+ double sLat = sin(latitude);
+ double cLat = cos(latitude);
+ double sLon = sin(longitude);
+ double cLon = cos(longitude);
R_ENU_ECEF(0,0) = -sLon;
R_ENU_ECEF(0,1) = cLon;
@@ -105,7 +105,7 @@ void SensorGnss::computeEnuEcef(const Eigen::Vector3s& _t_ECEF_ENU, Eigen::Matri
t_ENU_ECEF = -R_ENU_ECEF*_t_ECEF_ENU;
}
-void SensorGnss::setEcefEnu(const Eigen::Vector3s& _t_ECEF_ENU)
+void SensorGnss::setEcefEnu(const Eigen::Vector3d& _t_ECEF_ENU)
{
computeEnuEcef(_t_ECEF_ENU, R_ENU_ECEF_, t_ENU_ECEF_);
ENU_defined_ = true;
@@ -113,41 +113,41 @@ void SensorGnss::setEcefEnu(const Eigen::Vector3s& _t_ECEF_ENU)
WOLF_INFO("SensorGnss: ECEF-ENU initialized.")
}
-void SensorGnss::setEnuEcef(const Eigen::Matrix3s& _R_ENU_ECEF, const Eigen::Vector3s& _t_ENU_ECEF)
+void SensorGnss::setEnuEcef(const Eigen::Matrix3d& _R_ENU_ECEF, const Eigen::Vector3d& _t_ENU_ECEF)
{
R_ENU_ECEF_ = _R_ENU_ECEF;
t_ENU_ECEF_ = _t_ENU_ECEF;
ENU_defined_ = true;
}
-void SensorGnss::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 SensorGnss::initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, const Eigen::Vector3d& _t_ECEF_antenaENU,
+ const Eigen::VectorXd& _pose_MAP_frame2, const Eigen::Vector3d& _t_ECEF_antena2)
{
- Eigen::Matrix3s R_ENU_ECEF;//, R_ENU2_ECEF;
- Eigen::Vector3s t_ENU_ECEF;//, t_ENU2_ECEF;
+ Eigen::Matrix3d R_ENU_ECEF;//, R_ENU2_ECEF;
+ Eigen::Vector3d t_ENU_ECEF;//, t_ENU2_ECEF;
computeEnuEcef(_t_ECEF_antenaENU, R_ENU_ECEF, t_ENU_ECEF);
//computeENU_ECEF(_t_ECEF_antena2, R_ENU2_ECEF, t_ENU2_ECEF);
// compute fix vector (from 1 to 2) in ENU coordinates
- Eigen::Vector3s v_ENU = R_ENU_ECEF * (_t_ECEF_antena2 - _t_ECEF_antenaENU);
+ Eigen::Vector3d v_ENU = R_ENU_ECEF * (_t_ECEF_antena2 - _t_ECEF_antenaENU);
// 2D
if (getProblem()->getDim() == 2)
{
// compute antena vector (from 1 to 2) in MAP
- Eigen::Vector2s t_MAP_antenaENU = _pose_MAP_frameENU.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frameENU(2)) * getP()->getState().head<2>();
- Eigen::Vector2s t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
- Eigen::Vector2s v_MAP = t_MAP_antena2 - t_MAP_antenaENU;
+ Eigen::Vector2d t_MAP_antenaENU = _pose_MAP_frameENU.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frameENU(2)) * getP()->getState().head<2>();
+ Eigen::Vector2d t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
+ Eigen::Vector2d v_MAP = t_MAP_antena2 - t_MAP_antenaENU;
// initialize yaw
- Scalar theta_ENU = atan2(v_ENU(1),v_ENU(0));
- Scalar theta_MAP = atan2(v_MAP(1),v_MAP(0));
- Scalar yaw_ENU_MAP = theta_ENU-theta_MAP;
+ double theta_ENU = atan2(v_ENU(1),v_ENU(0));
+ double theta_MAP = atan2(v_MAP(1),v_MAP(0));
+ double yaw_ENU_MAP = theta_ENU-theta_MAP;
setEnuMapYawState(yaw_ENU_MAP);
// initialize translation
- Eigen::Vector3s t_ENU_MAP(Eigen::Vector3s::Zero());
- Eigen::Matrix2s R_ENU_MAP = computeREnuMap(getEnuMapRoll()->getState()(0),
+ Eigen::Vector3d t_ENU_MAP(Eigen::Vector3d::Zero());
+ Eigen::Matrix2d R_ENU_MAP = computeREnuMap(getEnuMapRoll()->getState()(0),
getEnuMapPitch()->getState()(0),
getEnuMapYaw()->getState()(0)).topLeftCorner<2,2>();
t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU;
@@ -167,13 +167,13 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con
//std::cout << "yaw set: " << yaw << std::endl;
//std::cout << "t_ENU1_origin: " << t_ENU_MAP.transpose() << std::endl;
//std::cout << "-----checks\n";
- //std::cout << "R(-yaw) * v_ENU: " << (Eigen::Rotation2D<Scalar>(-yaw) * v_ENU.head<2>()).transpose() << std::endl;
+ //std::cout << "R(-yaw) * v_ENU: " << (Eigen::Rotation2D<double>(-yaw) * v_ENU.head<2>()).transpose() << std::endl;
//std::cout << "should be: " << v_MAP.transpose() << std::endl;
- //std::cout << "atan2(R(-yaw) * v_ENU): " << atan2((Eigen::Rotation2D<Scalar>(-yaw) * v_ENU.head<2>())(1), (Eigen::Rotation2D<Scalar>(-yaw) * v_ENU.head<2>())(0)) << std::endl;
+ //std::cout << "atan2(R(-yaw) * v_ENU): " << atan2((Eigen::Rotation2D<double>(-yaw) * v_ENU.head<2>())(1), (Eigen::Rotation2D<double>(-yaw) * v_ENU.head<2>())(0)) << std::endl;
//std::cout << "should be: " << atan2(v_MAP(1),v_MAP(0)) << std::endl;
- //std::cout << "R(yaw) * v_MAP: " << (Eigen::Rotation2D<Scalar>(yaw) * v_MAP).transpose() << std::endl;
+ //std::cout << "R(yaw) * v_MAP: " << (Eigen::Rotation2D<double>(yaw) * v_MAP).transpose() << std::endl;
//std::cout << "should be: " << v_ENU.head<2>().transpose() << std::endl;
- //std::cout << "atan2(R(yaw) * v_MAP): " << atan2((Eigen::Rotation2D<Scalar>(yaw) * v_MAP)(1),(Eigen::Rotation2D<Scalar>(yaw) * v_MAP)(0)) << std::endl;
+ //std::cout << "atan2(R(yaw) * v_MAP): " << atan2((Eigen::Rotation2D<double>(yaw) * v_MAP)(1),(Eigen::Rotation2D<double>(yaw) * v_MAP)(0)) << std::endl;
//std::cout << "should be: " << atan2(v_ENU(1),v_ENU(0)) << std::endl;
//std::cout << "v_ENU.norm(): " << v_ENU.norm() << std::endl;
//std::cout << "v_MAP.norm(): " << v_MAP.norm() << std::endl;
@@ -188,60 +188,60 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con
}
}
-void SensorGnss::initializeEnuMapYaw(const Eigen::VectorXs& _pose_MAP_frame1, const Eigen::VectorXs& _pose_MAP_frame2,
- const Eigen::Vector3s& _v_ECEF_antena1_antena2)
+void SensorGnss::initializeEnuMapYaw(const Eigen::VectorXd& _pose_MAP_frame1, const Eigen::VectorXd& _pose_MAP_frame2,
+ const Eigen::Vector3d& _v_ECEF_antena1_antena2)
{
assert(ENU_defined_ && "initializing ENU-MAP yaw without ENU defined");
- Eigen::Vector2s t_MAP_antena1 = _pose_MAP_frame1.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame1(2)) * getP()->getState().head<2>();
- Eigen::Vector2s t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
- Eigen::Vector2s v_MAP_antena1_antena2 = t_MAP_antena2 - t_MAP_antena1;
- Eigen::Vector3s v_ENU_antena1_antena2 = R_ENU_ECEF_ * _v_ECEF_antena1_antena2;
+ Eigen::Vector2d t_MAP_antena1 = _pose_MAP_frame1.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frame1(2)) * getP()->getState().head<2>();
+ Eigen::Vector2d t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
+ Eigen::Vector2d v_MAP_antena1_antena2 = t_MAP_antena2 - t_MAP_antena1;
+ Eigen::Vector3d v_ENU_antena1_antena2 = R_ENU_ECEF_ * _v_ECEF_antena1_antena2;
- Scalar theta_ENU = atan2(v_ENU_antena1_antena2(1),v_ENU_antena1_antena2(0));
- Scalar theta_MAP = atan2(v_MAP_antena1_antena2(1),v_MAP_antena1_antena2(0));
- Scalar yaw_ENU_MAP = theta_ENU-theta_MAP;
+ double theta_ENU = atan2(v_ENU_antena1_antena2(1),v_ENU_antena1_antena2(0));
+ double theta_MAP = atan2(v_MAP_antena1_antena2(1),v_MAP_antena1_antena2(0));
+ double yaw_ENU_MAP = theta_ENU-theta_MAP;
setEnuMapYawState(yaw_ENU_MAP);
}
-void SensorGnss::setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP)
+void SensorGnss::setEnuMapTranslationState(const Eigen::Vector3d& t_ENU_MAP)
{
getEnuMapTranslation()->setState(t_ENU_MAP);
ENU_MAP_initialized_ = true;
}
-void SensorGnss::setEnuMapRollState(const Scalar& roll_ENU_MAP)
+void SensorGnss::setEnuMapRollState(const double& roll_ENU_MAP)
{
- getEnuMapRoll()->setState(Eigen::Vector1s(roll_ENU_MAP));
+ getEnuMapRoll()->setState(Eigen::Vector1d(roll_ENU_MAP));
ENU_MAP_initialized_ = true;
}
-void SensorGnss::setEnuMapPitchState(const Scalar& pitch_ENU_MAP)
+void SensorGnss::setEnuMapPitchState(const double& pitch_ENU_MAP)
{
- getEnuMapPitch()->setState(Eigen::Vector1s(pitch_ENU_MAP));
+ getEnuMapPitch()->setState(Eigen::Vector1d(pitch_ENU_MAP));
ENU_MAP_initialized_ = true;
}
-void SensorGnss::setEnuMapYawState(const Scalar& yaw_ENU_MAP)
+void SensorGnss::setEnuMapYawState(const double& yaw_ENU_MAP)
{
- getEnuMapYaw()->setState(Eigen::Vector1s(yaw_ENU_MAP));
+ getEnuMapYaw()->setState(Eigen::Vector1d(yaw_ENU_MAP));
ENU_MAP_initialized_ = true;
}
-Eigen::Matrix3s SensorGnss::getREnuMap() const
+Eigen::Matrix3d 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()));
+ return Eigen::Matrix3d(Eigen::AngleAxis<double>(getEnuMapRoll() ->getState()(0), Eigen::Vector3d::UnitX()) *
+ Eigen::AngleAxis<double>(getEnuMapPitch()->getState()(0), Eigen::Vector3d::UnitY()) *
+ Eigen::AngleAxis<double>(getEnuMapYaw() ->getState()(0), Eigen::Vector3d::UnitZ()));
}
// Define the factory method
-SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics,
+SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics,
const IntrinsicsBasePtr _intrinsics)
{
assert((_extrinsics.size() == 3 || _extrinsics.size() == 9) && "Bad extrinsics vector length. Should be 3 (antena position) or 9 (antena position and MAP-ENU initialization: position XYZ and orientation RPY)");
diff --git a/test/gtest_factor_gnss_fix_2D.cpp b/test/gtest_factor_gnss_fix_2D.cpp
index 61a1780a8626d9618c3551f8a712c7adf4ff2e7c..9eb98ed91b5b1615edc41ea0f1298a492d99d4d2 100644
--- a/test/gtest_factor_gnss_fix_2D.cpp
+++ b/test/gtest_factor_gnss_fix_2D.cpp
@@ -20,15 +20,15 @@ using namespace Eigen;
using namespace wolf;
void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr,
- const Vector1s& o_enu_map,
- const Vector3s& t_base_antena,
- const Vector3s& t_enu_map,
- const Vector3s& t_map_base,
- const Vector1s& o_map_base)
+ const Vector1d& o_enu_map,
+ const Vector3d& t_base_antena,
+ const Vector3d& t_enu_map,
+ const Vector3d& t_map_base,
+ const Vector1d& o_map_base)
{
// ENU-MAP
- gnss_sensor_ptr->getEnuMapRoll()->setState(Eigen::Vector1s::Zero());
- gnss_sensor_ptr->getEnuMapPitch()->setState(Eigen::Vector1s::Zero());
+ gnss_sensor_ptr->getEnuMapRoll()->setState(Eigen::Vector1d::Zero());
+ gnss_sensor_ptr->getEnuMapPitch()->setState(Eigen::Vector1d::Zero());
gnss_sensor_ptr->getEnuMapYaw()->setState(o_enu_map);
gnss_sensor_ptr->getEnuMapTranslation()->setState(t_enu_map);
// Antena
@@ -57,7 +57,7 @@ void computeParamSizes(const CeresManagerPtr& ceres_mgr_ptr, int& num_params_red
num_param_blocks_reduced = 0;
num_params_reduced = 0;
- std::vector<Scalar*> param_blocks;
+ std::vector<double*> param_blocks;
ceres_mgr_ptr->getCeresProblem()->GetParameterBlocks(¶m_blocks);
for (auto pb : param_blocks)
@@ -74,18 +74,18 @@ void computeParamSizes(const CeresManagerPtr& ceres_mgr_ptr, int& num_params_red
}
// groundtruth transformations
-Vector1s o_enu_map = (Vector1s() << 2.6).finished();
-Vector3s t_enu_map = (Vector3s() << 12, -34, 4).finished();
-Vector3s t_map_base = (Vector3s() << 32, -34, 0).finished(); // z=0
-Vector1s o_map_base = (Vector1s() << -0.4).finished();
-Vector3s t_base_antena = (Vector3s() << 3,-2,8).finished();// Antena extrinsics
-Vector3s t_ecef_enu = (Vector3s() << 100,30,50).finished();
-Matrix3s R_ecef_enu = (AngleAxis<Scalar>(1.3, Vector3s::UnitX()) *
- AngleAxis<Scalar>(-2.2, Vector3s::UnitY()) *
- AngleAxis<Scalar>(-1.8, Vector3s::UnitZ())).toRotationMatrix();
-Matrix3s R_enu_map = AngleAxis<Scalar>(o_enu_map(0), Vector3s::UnitZ()).toRotationMatrix();
-Matrix3s R_map_base = AngleAxis<Scalar>(o_map_base(0), Vector3s::UnitZ()).toRotationMatrix();
-Vector3s t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
+Vector1d o_enu_map = (Vector1d() << 2.6).finished();
+Vector3d t_enu_map = (Vector3d() << 12, -34, 4).finished();
+Vector3d t_map_base = (Vector3d() << 32, -34, 0).finished(); // z=0
+Vector1d o_map_base = (Vector1d() << -0.4).finished();
+Vector3d t_base_antena = (Vector3d() << 3,-2,8).finished();// Antena extrinsics
+Vector3d t_ecef_enu = (Vector3d() << 100,30,50).finished();
+Matrix3d R_ecef_enu = (AngleAxis<double>(1.3, Vector3d::UnitX()) *
+ AngleAxis<double>(-2.2, Vector3d::UnitY()) *
+ AngleAxis<double>(-1.8, Vector3d::UnitZ())).toRotationMatrix();
+Matrix3d R_enu_map = AngleAxis<double>(o_enu_map(0), Vector3d::UnitZ()).toRotationMatrix();
+Matrix3d R_map_base = AngleAxis<double>(o_map_base(0), Vector3d::UnitZ()).toRotationMatrix();
+Vector3d t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena + t_map_base) + t_enu_map ) + t_ecef_enu;
// WOLF
ProblemPtr problem_ptr = Problem::create("PO", 2);
@@ -112,12 +112,12 @@ TEST(FactorGnssFix2DTest, configure_tree)
problem_ptr->installProcessor("GNSS FIX", "gnss fix", gnss_sensor_ptr, gnss_params_ptr);
// Emplace a frame (FIXED)
- Vector3s frame_pose = (Vector3s() << t_map_base(0), t_map_base(1), o_map_base(0)).finished();
+ Vector3d frame_pose = (Vector3d() << t_map_base(0), t_map_base(1), o_map_base(0)).finished();
frame_ptr = problem_ptr->emplaceFrame(KEY, frame_pose, TimeStamp(0));
problem_ptr->keyFrameCallback(frame_ptr, nullptr, 1.0);
// Create & process GNSS Fix capture
- CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity());
+ CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3d::Identity());
gnss_sensor_ptr->process(cap_gnss_ptr);
// Checks
@@ -141,7 +141,7 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_position)
frame_ptr->getP()->unfix();
// --------------------------- distort: map base position
- Vector3s frm_dist = frame_ptr->getState();
+ Vector3d frm_dist = frame_ptr->getState();
frm_dist(0) += 0.18;
frm_dist(1) += -0.58;
frame_ptr->setState(frm_dist);
@@ -187,7 +187,7 @@ TEST(FactorGnssFix2DTest, gnss_1_map_base_orientation)
frame_ptr->getO()->unfix();
// --------------------------- distort: map base orientation
- Vector1s frm_dist = frame_ptr->getO()->getState();
+ Vector1d frm_dist = frame_ptr->getO()->getState();
frm_dist(0) += 0.18;
frame_ptr->getO()->setState(frm_dist);
@@ -230,7 +230,7 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_yaw)
gnss_sensor_ptr->getEnuMapYaw()->unfix();
// --------------------------- distort: yaw enu_map
- Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYaw()->getState();
+ Vector1d o_enu_map_dist = gnss_sensor_ptr->getEnuMapYaw()->getState();
o_enu_map_dist(0) += 0.18;
gnss_sensor_ptr->getEnuMapYaw()->setState(o_enu_map_dist);
@@ -273,7 +273,7 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_position)
gnss_sensor_ptr->getEnuMapTranslation()->unfix();// enu-map yaw translation
// --------------------------- distort: position enu_map
- Vector3s t_enu_map_dist = gnss_sensor_ptr->getEnuMapTranslation()->getState();
+ Vector3d t_enu_map_dist = gnss_sensor_ptr->getEnuMapTranslation()->getState();
t_enu_map_dist(0) += 0.86;
t_enu_map_dist(1) += -0.34;
gnss_sensor_ptr->getEnuMapTranslation()->setState(t_enu_map_dist);
@@ -317,7 +317,7 @@ TEST(FactorGnssFix2DTest, gnss_1_base_antena)
gnss_sensor_ptr->getP()->unfix();
// --------------------------- distort: base_antena
- Vector3s base_antena_dist = gnss_sensor_ptr->getP()->getState();
+ Vector3d base_antena_dist = gnss_sensor_ptr->getP()->getState();
base_antena_dist(0) += 1.68;
base_antena_dist(0) += -0.48;
gnss_sensor_ptr->getP()->setState(base_antena_dist);
diff --git a/test/gtest_factor_gnss_single_diff_2D.cpp b/test/gtest_factor_gnss_single_diff_2D.cpp
index 3ce5c0156b6a6416e6690d81bfaf9ca76e8aafb5..2e09493d415c3a22f955be027728d19746871eea 100644
--- a/test/gtest_factor_gnss_single_diff_2D.cpp
+++ b/test/gtest_factor_gnss_single_diff_2D.cpp
@@ -30,12 +30,12 @@ class FactorGnssSingleDiff2DTest : public testing::Test
public:
// groundtruth transformations
- Vector3s t_ecef_enu;
- Matrix3s R_ecef_enu, R_enu_map, R_map_base1, R_map_base2;
- Vector3s t_base_antena, t_ecef_antena1, t_ecef_antena2;
- Vector1s o_enu_map;
- Vector3s t_map_base1, t_map_base2;
- Vector1s o_map_base1, o_map_base2;
+ Vector3d t_ecef_enu;
+ Matrix3d R_ecef_enu, R_enu_map, R_map_base1, R_map_base2;
+ Vector3d t_base_antena, t_ecef_antena1, t_ecef_antena2;
+ Vector1d o_enu_map;
+ Vector3d t_map_base1, t_map_base2;
+ Vector1d o_map_base1, o_map_base2;
// WOLF
ProblemPtr problem_ptr;
@@ -53,9 +53,9 @@ class FactorGnssSingleDiff2DTest : public testing::Test
o_map_base2 << 0.7;
t_base_antena << 3,-2,8;// Antena extrinsics
t_ecef_enu << 100,30,50;
- R_ecef_enu = (AngleAxis<Scalar>(1.3, Vector3s::UnitX())
- *AngleAxis<Scalar>(-2.2, Vector3s::UnitY())
- *AngleAxis<Scalar>(-1.8, Vector3s::UnitZ())).toRotationMatrix();
+ R_ecef_enu = (AngleAxis<double>(1.3, Vector3d::UnitX())
+ *AngleAxis<double>(-2.2, Vector3d::UnitY())
+ *AngleAxis<double>(-1.8, Vector3d::UnitZ())).toRotationMatrix();
// ----------------------------------------------------
// Problem and solver
@@ -80,7 +80,7 @@ class FactorGnssSingleDiff2DTest : public testing::Test
IntrinsicsOdom2DPtr odom_intrinsics_ptr = std::make_shared<IntrinsicsOdom2D>();
odom_intrinsics_ptr->k_disp_to_disp = 0.1;
odom_intrinsics_ptr->k_rot_to_rot = 0.1;
- odom_sensor_ptr = std::static_pointer_cast<SensorOdom2D>(problem_ptr->installSensor("ODOM 2D", "odometer", VectorXs::Zero(3), odom_intrinsics_ptr));
+ odom_sensor_ptr = std::static_pointer_cast<SensorOdom2D>(problem_ptr->installSensor("ODOM 2D", "odometer", VectorXd::Zero(3), odom_intrinsics_ptr));
ProcessorParamsOdom2DPtr odom_params_ptr = std::make_shared<ProcessorParamsOdom2D>();
odom_params_ptr->voting_active = true;
odom_params_ptr->dist_traveled = 1;
@@ -91,20 +91,20 @@ class FactorGnssSingleDiff2DTest : public testing::Test
//problem_ptr->setProcessorMotion("main odometry");
// set prior (FIXED)
- Vector3s frame1state = t_map_base1;
+ Vector3d frame1state = t_map_base1;
frame1state(2) = o_map_base1(0);
- prior_frame_ptr = problem_ptr->setPrior(frame1state, Matrix3s::Identity(), TimeStamp(0), Scalar(0.1));
+ prior_frame_ptr = problem_ptr->setPrior(frame1state, Matrix3d::Identity(), TimeStamp(0), double(0.1));
prior_frame_ptr->fix();
};
virtual void SetUp()
{
// reset grountruth parameters
- R_enu_map = AngleAxis<Scalar>(o_enu_map(0), Vector3s::UnitZ()).toRotationMatrix();
- R_map_base1 = Matrix3s::Identity();
- R_map_base1.topLeftCorner(2,2) = Rotation2D<Scalar>(o_map_base1(0)).matrix();
- R_map_base2 = Matrix3s::Identity();
- R_map_base2.topLeftCorner(2,2) = Rotation2D<Scalar>(o_map_base2(0)).matrix();
+ R_enu_map = AngleAxis<double>(o_enu_map(0), Vector3d::UnitZ()).toRotationMatrix();
+ R_map_base1 = Matrix3d::Identity();
+ R_map_base1.topLeftCorner(2,2) = Rotation2D<double>(o_map_base1(0)).matrix();
+ R_map_base2 = Matrix3d::Identity();
+ R_map_base2.topLeftCorner(2,2) = Rotation2D<double>(o_map_base2(0)).matrix();
t_ecef_antena1 = R_ecef_enu * R_enu_map * (R_map_base1 * t_base_antena + t_map_base1) + t_ecef_enu;
t_ecef_antena2 = R_ecef_enu * R_enu_map * (R_map_base2 * t_base_antena + t_map_base2) + t_ecef_enu;
@@ -133,7 +133,7 @@ TEST_F(FactorGnssSingleDiff2DTest, check_tree)
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position)
{
// Create GNSS Fix capture
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
+ CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3d::Identity(), prior_frame_ptr);
gnss_sensor_ptr->process(cap_gnss_ptr);
// fixing things
@@ -145,7 +145,7 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position)
std::cout << "frame1: " << prior_frame_ptr->getState().transpose() << std::endl;
// distort frm position
- Vector3s frm_dist = cap_gnss_ptr->getFrame()->getState();
+ Vector3d frm_dist = cap_gnss_ptr->getFrame()->getState();
frm_dist(0) += 18;
frm_dist(1) += -58;
cap_gnss_ptr->getFrame()->setState(frm_dist);
@@ -166,7 +166,7 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position)
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_orientation)
{
// Create GNSS Fix capture
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
+ CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3d::Identity(), prior_frame_ptr);
gnss_sensor_ptr->process(cap_gnss_ptr);
// fixing things
@@ -176,7 +176,7 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_orientation)
cap_gnss_ptr->getFrame()->getP()->fix();
// distort frm position
- Vector1s frm_dist = cap_gnss_ptr->getFrame()->getO()->getState();
+ Vector1d frm_dist = cap_gnss_ptr->getFrame()->getO()->getState();
frm_dist(0) += 1.8;
cap_gnss_ptr->getFrame()->getO()->setState(frm_dist);
@@ -196,7 +196,7 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_orientation)
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_enu_map_yaw)
{
// Create GNSS Fix capture
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
+ CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3d::Identity(), prior_frame_ptr);
gnss_sensor_ptr->process(cap_gnss_ptr);
// unfixing things
@@ -208,7 +208,7 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_enu_map_yaw)
cap_gnss_ptr->getFrame()->getO()->fix();
// distort yaw enu_map
- Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYaw()->getState();
+ Vector1d o_enu_map_dist = gnss_sensor_ptr->getEnuMapYaw()->getState();
o_enu_map_dist(0) += 1.8;
gnss_sensor_ptr->setEnuMapYawState(o_enu_map_dist(0));
@@ -228,7 +228,7 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_enu_map_yaw)
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_base_antena)
{
// Create GNSS Fix capture
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
+ CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3d::Identity(), prior_frame_ptr);
gnss_sensor_ptr->process(cap_gnss_ptr);
// unfixing things
@@ -242,7 +242,7 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_base_antena)
cap_gnss_ptr->getFrame()->getO()->fix();
// distort base_antena
- Vector3s base_antena_dist = gnss_sensor_ptr->getP()->getState();
+ Vector3d base_antena_dist = gnss_sensor_ptr->getP()->getState();
base_antena_dist(0) += 16.8;
base_antena_dist(0) += -40.8;
gnss_sensor_ptr->getP()->setState(base_antena_dist);