diff --git a/include/gnss/factor/factor_gnss_fix_2D.h b/include/gnss/factor/factor_gnss_fix_2D.h
index 0a1e49c45a68d7039e451377ba1f4d7abf3da002..9adb73314ac0cc8776579d0359855395fa22dc27 100644
--- a/include/gnss/factor/factor_gnss_fix_2D.h
+++ b/include/gnss/factor/factor_gnss_fix_2D.h
@@ -31,10 +31,10 @@ class FactorGnssFix2D : public FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_sensor_gnss_ptr->getStateBlock(0),
- _sensor_gnss_ptr->getEnuMapTranslationState(),
- _sensor_gnss_ptr->getEnuMapRollState(),
- _sensor_gnss_ptr->getEnuMapPitchState(),
- _sensor_gnss_ptr->getEnuMapYawState()),
+ _sensor_gnss_ptr->getEnuMapTranslation(),
+ _sensor_gnss_ptr->getEnuMapRoll(),
+ _sensor_gnss_ptr->getEnuMapPitch(),
+ _sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an active GNSS Fix 2D factor without initializing ENU");
diff --git a/include/gnss/factor/factor_gnss_fix_3D.h b/include/gnss/factor/factor_gnss_fix_3D.h
index 89628d4c8e7ba7fe3355f7816ca30bc2ad390892..4f47dad9fe30e61dda3c60ccfb70ccee12ac8155 100644
--- a/include/gnss/factor/factor_gnss_fix_3D.h
+++ b/include/gnss/factor/factor_gnss_fix_3D.h
@@ -30,10 +30,10 @@ class FactorGnssFix3D : public FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_sensor_gnss_ptr->getStateBlock(0),
- _sensor_gnss_ptr->getEnuMapTranslationState(),
- _sensor_gnss_ptr->getEnuMapRollState(),
- _sensor_gnss_ptr->getEnuMapPitchState(),
- _sensor_gnss_ptr->getEnuMapYawState()),
+ _sensor_gnss_ptr->getEnuMapTranslation(),
+ _sensor_gnss_ptr->getEnuMapRoll(),
+ _sensor_gnss_ptr->getEnuMapPitch(),
+ _sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an ACTIVE GNSS Fix 3D factor without initializing ENU");
diff --git a/include/gnss/factor/factor_gnss_single_diff_2D.h b/include/gnss/factor/factor_gnss_single_diff_2D.h
index 1d52171a42388e0030848fe8fd5fa5f28f164d83..3cadfcb2413f6d94429cbd04c94cb94bad90d353 100644
--- a/include/gnss/factor/factor_gnss_single_diff_2D.h
+++ b/include/gnss/factor/factor_gnss_single_diff_2D.h
@@ -33,9 +33,9 @@ class FactorGnssSingleDiff2D : public FactorAutodiff<FactorGnssSingleDiff2D, 3,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_sensor_gnss_ptr->getStateBlock(0),
- _sensor_gnss_ptr->getEnuMapRollState(),
- _sensor_gnss_ptr->getEnuMapPitchState(),
- _sensor_gnss_ptr->getEnuMapYawState()),
+ _sensor_gnss_ptr->getEnuMapRoll(),
+ _sensor_gnss_ptr->getEnuMapPitch(),
+ _sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 2D factor without initializing ENU");
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index 2bbbe94fb171e16b5be021b99afa987904ea6a94..3c5ed1466e020c1903dfe803706f5e83ff19a617 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -63,15 +63,18 @@ class SensorGnss : public SensorBase
bool isEnuMapInitialized() const;
// ENU_MAP
- StateBlockPtr getEnuMapTranslationState() const;
- StateBlockPtr getEnuMapRollState() const;
- StateBlockPtr getEnuMapPitchState() const;
- StateBlockPtr getEnuMapYawState() const;
+ 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);
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);
// Gets
const Eigen::Matrix3s& getREnuEcef() const;
@@ -95,22 +98,22 @@ inline bool SensorGnss::isEnuMapInitialized() const
return ENU_MAP_initialized_;
}
-inline StateBlockPtr SensorGnss::getEnuMapTranslationState() const
+inline StateBlockPtr SensorGnss::getEnuMapTranslation() const
{
return getStateBlockPtrStatic(3);
}
-inline StateBlockPtr SensorGnss::getEnuMapRollState() const
+inline StateBlockPtr SensorGnss::getEnuMapRoll() const
{
return getStateBlockPtrStatic(4);
}
-inline StateBlockPtr SensorGnss::getEnuMapPitchState() const
+inline StateBlockPtr SensorGnss::getEnuMapPitch() const
{
return getStateBlockPtrStatic(5);
}
-inline StateBlockPtr SensorGnss::getEnuMapYawState() const
+inline StateBlockPtr SensorGnss::getEnuMapYaw() const
{
return getStateBlockPtrStatic(6);
}
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index a12da6a372a1d77000b14c38e2d4e6290342d48a..92acb31151678c77efdbc81c471499bb8b2b2d40 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -122,10 +122,10 @@ bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac_ptr)
Eigen::VectorXs x(gnss_ftr_ptr->getCapture()->getFrame()->getP()->getState());
Eigen::VectorXs o(gnss_ftr_ptr->getCapture()->getFrame()->getP()->getState());
Eigen::VectorXs x_antena(sensor_gnss_ptr_->getStateBlock(0)->getState());
- Eigen::VectorXs t_ENU_map(sensor_gnss_ptr_->getEnuMapTranslationState()->getState());
- Eigen::VectorXs roll_ENU_map(sensor_gnss_ptr_->getEnuMapRollState()->getState());
- Eigen::VectorXs pitch_ENU_map(sensor_gnss_ptr_->getEnuMapPitchState()->getState());
- Eigen::VectorXs yaw_ENU_map(sensor_gnss_ptr_->getEnuMapYawState()->getState());
+ Eigen::VectorXs t_ENU_map(sensor_gnss_ptr_->getEnuMapTranslation()->getState());
+ Eigen::VectorXs roll_ENU_map(sensor_gnss_ptr_->getEnuMapRoll()->getState());
+ Eigen::VectorXs pitch_ENU_map(sensor_gnss_ptr_->getEnuMapPitch()->getState());
+ Eigen::VectorXs yaw_ENU_map(sensor_gnss_ptr_->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(),
pitch_ENU_map.data(), yaw_ENU_map.data()};
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index 35f0d57c63ade592d6e2c651c26dcf6a63d9964c..b048757fce4ca5c7f9f4eeb440a744066dfa9373 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -142,21 +142,19 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con
// initialize yaw
Scalar theta_ENU = atan2(v_ENU(1),v_ENU(0));
Scalar theta_MAP = atan2(v_MAP(1),v_MAP(0));
- Scalar yaw = theta_ENU-theta_MAP;
- this->getEnuMapYawState()->setState(Eigen::Vector1s(yaw));
+ Scalar 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(getEnuMapRollState()->getState()(0),
- getEnuMapPitchState()->getState()(0),
- getEnuMapYawState()->getState()(0)).topLeftCorner<2,2>();
+ Eigen::Matrix2s 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;
// set translation state
setEnuMapTranslationState(t_ENU_MAP);
- ENU_MAP_initialized_ = true;
-
//std::cout << "-----------------------------------------\n";
//std::cout << "t_ECEF_antenaENU: " << _t_ECEF_antenaENU.transpose() << std::endl;
//std::cout << "t_ECEF_antena2: " << _t_ECEF_antena2.transpose() << std::endl;
@@ -204,14 +202,35 @@ void SensorGnss::initializeEnuMapYaw(const Eigen::VectorXs& _pose_MAP_frame1, co
Scalar theta_MAP = atan2(v_MAP_antena1_antena2(1),v_MAP_antena1_antena2(0));
Scalar yaw_ENU_MAP = theta_ENU-theta_MAP;
- this->getEnuMapYawState()->setState(Eigen::Vector1s(yaw_ENU_MAP));
+ setEnuMapYawState(yaw_ENU_MAP);
+}
+
+void SensorGnss::setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP)
+{
+ getEnuMapTranslation()->setState(t_ENU_MAP);
ENU_MAP_initialized_ = true;
}
-void SensorGnss::setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP)
+void SensorGnss::setEnuMapRollState(const Scalar& roll_ENU_MAP)
+{
+ getEnuMapRoll()->setState(Eigen::Vector1s(roll_ENU_MAP));
+
+ ENU_MAP_initialized_ = true;
+}
+
+void SensorGnss::setEnuMapPitchState(const Scalar& pitch_ENU_MAP)
{
- this->getEnuMapTranslationState()->setState(t_ENU_MAP);
+ getEnuMapPitch()->setState(Eigen::Vector1s(pitch_ENU_MAP));
+
+ ENU_MAP_initialized_ = true;
+}
+
+void SensorGnss::setEnuMapYawState(const Scalar& yaw_ENU_MAP)
+{
+ getEnuMapYaw()->setState(Eigen::Vector1s(yaw_ENU_MAP));
+
+ ENU_MAP_initialized_ = true;
}
// Define the factory method
diff --git a/test/gtest_factor_gnss_fix_2D.cpp b/test/gtest_factor_gnss_fix_2D.cpp
index 1f3330a8987edfb2df8de89e1ac038a572fd8788..61a1780a8626d9618c3551f8a712c7adf4ff2e7c 100644
--- a/test/gtest_factor_gnss_fix_2D.cpp
+++ b/test/gtest_factor_gnss_fix_2D.cpp
@@ -27,10 +27,10 @@ void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr,
const Vector1s& o_map_base)
{
// ENU-MAP
- gnss_sensor_ptr->getEnuMapRollState()->setState(Eigen::Vector1s::Zero());
- gnss_sensor_ptr->getEnuMapPitchState()->setState(Eigen::Vector1s::Zero());
- gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map);
- gnss_sensor_ptr->getEnuMapTranslationState()->setState(t_enu_map);
+ gnss_sensor_ptr->getEnuMapRoll()->setState(Eigen::Vector1s::Zero());
+ gnss_sensor_ptr->getEnuMapPitch()->setState(Eigen::Vector1s::Zero());
+ gnss_sensor_ptr->getEnuMapYaw()->setState(o_enu_map);
+ gnss_sensor_ptr->getEnuMapTranslation()->setState(t_enu_map);
// Antena
gnss_sensor_ptr->getP()->setState(t_base_antena);
// Frame
@@ -41,10 +41,10 @@ void resetStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr,
void fixAllStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr)
{
// ENU-MAP
- gnss_sensor_ptr->getEnuMapRollState()->fix();
- gnss_sensor_ptr->getEnuMapPitchState()->fix();
- gnss_sensor_ptr->getEnuMapYawState()->fix();
- gnss_sensor_ptr->getEnuMapTranslationState()->fix();
+ gnss_sensor_ptr->getEnuMapRoll()->fix();
+ gnss_sensor_ptr->getEnuMapPitch()->fix();
+ gnss_sensor_ptr->getEnuMapYaw()->fix();
+ gnss_sensor_ptr->getEnuMapTranslation()->fix();
// Antena
gnss_sensor_ptr->getP()->fix();
// Frame
@@ -101,9 +101,9 @@ TEST(FactorGnssFix2DTest, configure_tree)
// Configure sensor and processor
gnss_sensor_ptr->setEnuMapTranslationState(t_enu_map);
- gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map);
- gnss_sensor_ptr->getEnuMapRollState()->fix();
- gnss_sensor_ptr->getEnuMapPitchState()->fix();
+ gnss_sensor_ptr->setEnuMapYawState(o_enu_map(0));
+ gnss_sensor_ptr->getEnuMapRoll()->fix();
+ gnss_sensor_ptr->getEnuMapPitch()->fix();
gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
ProcessorParamsGnssFixPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssFix>();
@@ -117,8 +117,7 @@ TEST(FactorGnssFix2DTest, configure_tree)
problem_ptr->keyFrameCallback(frame_ptr, nullptr, 1.0);
// Create & process GNSS Fix capture
- auto cap = CaptureBase::emplace<CaptureGnssFix>(nullptr, TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity());
- CaptureGnssFixPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssFix>(cap);
+ CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity());
gnss_sensor_ptr->process(cap_gnss_ptr);
// Checks
@@ -228,12 +227,12 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_yaw)
// --------------------------- fixing/unfixing states
fixAllStates(gnss_sensor_ptr, frame_ptr);
- gnss_sensor_ptr->getEnuMapYawState()->unfix();
+ gnss_sensor_ptr->getEnuMapYaw()->unfix();
// --------------------------- distort: yaw enu_map
- Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYawState()->getState();
+ Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYaw()->getState();
o_enu_map_dist(0) += 0.18;
- gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map_dist);
+ gnss_sensor_ptr->getEnuMapYaw()->setState(o_enu_map_dist);
// --------------------------- update solver
ceres_mgr_ptr->update();
@@ -255,7 +254,7 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_yaw)
ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
// --------------------------- check solver solution
- ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYawState()->getState(), o_enu_map, 1e-6);
+ ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYaw()->getState(), o_enu_map, 1e-6);
}
/*
@@ -271,13 +270,13 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_position)
// --------------------------- fixing/unfixing states
fixAllStates(gnss_sensor_ptr, frame_ptr);
- gnss_sensor_ptr->getEnuMapTranslationState()->unfix();// enu-map yaw translation
+ gnss_sensor_ptr->getEnuMapTranslation()->unfix();// enu-map yaw translation
// --------------------------- distort: position enu_map
- Vector3s t_enu_map_dist = gnss_sensor_ptr->getEnuMapTranslationState()->getState();
+ Vector3s 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->getEnuMapTranslationState()->setState(t_enu_map_dist);
+ gnss_sensor_ptr->getEnuMapTranslation()->setState(t_enu_map_dist);
// --------------------------- update solver
ceres_mgr_ptr->update();
@@ -299,7 +298,7 @@ TEST(FactorGnssFix2DTest, gnss_1_enu_map_position)
ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
// --------------------------- check solver solution
- ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslationState()->getState().head(2), t_enu_map.head(2), 1e-6);
+ ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslation()->getState().head(2), t_enu_map.head(2), 1e-6);
}
/*
diff --git a/test/gtest_factor_gnss_single_diff_2D.cpp b/test/gtest_factor_gnss_single_diff_2D.cpp
index 2c70d8c0cd705aa0a70176e9fa6a2d92aefe4bd0..4b4ad70cc739f2af3c67a5a586b386b5c92b18ce 100644
--- a/test/gtest_factor_gnss_single_diff_2D.cpp
+++ b/test/gtest_factor_gnss_single_diff_2D.cpp
@@ -65,15 +65,13 @@ class FactorGnssSingleDiff2DTest : public testing::Test
// gnss sensor
gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("GNSS", "gnss", t_base_antena, std::make_shared<IntrinsicsBase>()));
- gnss_sensor_ptr->getEnuMapRollState()->fix();
- gnss_sensor_ptr->getEnuMapPitchState()->fix();
+ gnss_sensor_ptr->getEnuMapRoll()->fix();
+ gnss_sensor_ptr->getEnuMapPitch()->fix();
gnss_sensor_ptr->setEnuEcef(R_ecef_enu.transpose(), R_ecef_enu.transpose()*(-t_ecef_enu));
- std::cout << "gnss sensor installed" << std::endl;
// gnss processor
ProcessorParamsBasePtr gnss_params_ptr = std::make_shared<ProcessorParamsBase>(true, 1.0);
problem_ptr->installProcessor("GNSS SINGLE DIFFERENCES", "gnss single difference", gnss_sensor_ptr, gnss_params_ptr);
- std::cout << "gnss processor installed" << std::endl;
// odom sensor & processor
IntrinsicsOdom2DPtr odom_intrinsics_ptr = std::make_shared<IntrinsicsOdom2D>();
@@ -132,13 +130,12 @@ TEST_F(FactorGnssSingleDiff2DTest, check_tree)
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position)
{
// Create GNSS Fix capture
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssSingleDiff>(CaptureBase::emplace<CaptureGnssSingleDiff>(nullptr, 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*Matrix3s::Identity(), prior_frame_ptr);
gnss_sensor_ptr->process(cap_gnss_ptr);
// fixing things
- gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map); // enu-map yaw orientation
- gnss_sensor_ptr->getEnuMapYawState()->fix();
+ gnss_sensor_ptr->setEnuMapYawState(o_enu_map(0)); // enu-map yaw orientation
+ gnss_sensor_ptr->getEnuMapYaw()->fix();
cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation
cap_gnss_ptr->getFrame()->getO()->fix();
@@ -152,7 +149,6 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position)
// solve for frm
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
- std::cout << report << std::endl;
ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
ASSERT_MATRIX_APPROX(cap_gnss_ptr->getFrame()->getState().head(2), t_map_base2.head(2), 1e-6);
@@ -167,12 +163,12 @@ 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::static_pointer_cast<CaptureGnssSingleDiff>(CaptureBase::emplace<CaptureGnssSingleDiff>(nullptr, 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*Matrix3s::Identity(), prior_frame_ptr);
gnss_sensor_ptr->process(cap_gnss_ptr);
// fixing things
- gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map); // enu-map yaw orientation
- gnss_sensor_ptr->getEnuMapYawState()->fix();
+ gnss_sensor_ptr->setEnuMapYawState(o_enu_map(0)); // enu-map yaw orientation
+ gnss_sensor_ptr->getEnuMapYaw()->fix();
cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position
cap_gnss_ptr->getFrame()->getP()->fix();
@@ -197,11 +193,11 @@ 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::static_pointer_cast<CaptureGnssSingleDiff>(CaptureBase::emplace<CaptureGnssSingleDiff>(nullptr, 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*Matrix3s::Identity(), prior_frame_ptr);
gnss_sensor_ptr->process(cap_gnss_ptr);
// unfixing things
- gnss_sensor_ptr->getEnuMapYawState()->unfix();
+ gnss_sensor_ptr->getEnuMapYaw()->unfix();
// fixing things
cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position
cap_gnss_ptr->getFrame()->getP()->fix();
@@ -209,15 +205,15 @@ 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->getEnuMapYawState()->getState();
+ Vector1s o_enu_map_dist = gnss_sensor_ptr->getEnuMapYaw()->getState();
o_enu_map_dist(0) += 1.8;
- gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map_dist);
+ gnss_sensor_ptr->setEnuMapYawState(o_enu_map_dist(0));
// solve for frm
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
ASSERT_TRUE(cap_gnss_ptr->getFrame()->isKey());
- ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYawState()->getState(), o_enu_map, 1e-6);
+ ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYaw()->getState(), o_enu_map, 1e-6);
}
/*
@@ -229,14 +225,14 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_enu_map_yaw)
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_base_antena)
{
// Create GNSS Fix capture
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssSingleDiff>(CaptureBase::emplace<CaptureGnssSingleDiff>(nullptr, 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*Matrix3s::Identity(), prior_frame_ptr);
gnss_sensor_ptr->process(cap_gnss_ptr);
// unfixing things
gnss_sensor_ptr->getP()->unfix();
// fixing things
- gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map); // enu-map yaw orientation
- gnss_sensor_ptr->getEnuMapYawState()->fix();
+ gnss_sensor_ptr->setEnuMapYawState(o_enu_map(0)); // enu-map yaw orientation
+ gnss_sensor_ptr->getEnuMapYaw()->fix();
cap_gnss_ptr->getFrame()->getP()->setState(t_map_base2.head(2)); // frame position
cap_gnss_ptr->getFrame()->getP()->fix();
cap_gnss_ptr->getFrame()->getO()->setState(o_map_base2); // frame orientation