diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index e7204f6fa033d0f08a97eeaa67aaac1ed2d44438..2bbbe94fb171e16b5be021b99afa987904ea6a94 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -69,6 +69,8 @@ class SensorGnss : public SensorBase
StateBlockPtr getEnuMapYawState() 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);
// Gets
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index c069eeb81c85c76ec154a34c9768bc28c1ee2dbe..231807dde851503c8f45f8f0ffc410a2cf64d633 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -71,13 +71,14 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr)
// 4 - frames constained by the factor separated enough ( > enu_map_init_dist_min)
if ( sensor_gnss_ptr_->isEnuDefined() &&
!sensor_gnss_ptr_->isEnuMapInitialized() &&
- new_fac != nullptr &&
+ new_frame_ptr != nullptr &&
incoming_capture_ptr_->getFrame() != nullptr && incoming_capture_ptr_->getFrame()->isKey() &&
incoming_capture_ptr_->getData().norm() > params_gnss_->enu_map_init_dist_min)
{
std::cout << "initializing enu map\n";
- sensor_gnss_ptr_->initializeEnuMap(first_capture_ptr_->getFrame()->getState(), first_capture_ptr_->getData(),
- last_capture_ptr_->getFrame()->getState(), last_capture_ptr_->getData());
+ sensor_gnss_ptr_->initializeEnuMapYaw(last_capture_ptr_->getOriginFrame()->getState(),
+ last_capture_ptr_->getFrame()->getState(),
+ last_capture_ptr_->getData());
}
last_capture_ptr_ = incoming_capture_ptr_;
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index 16f2ca347c7770390b8938cc34080b878d670d5d..35f0d57c63ade592d6e2c651c26dcf6a63d9964c 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -155,6 +155,8 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con
// 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;
@@ -193,10 +195,10 @@ void SensorGnss::initializeEnuMapYaw(const Eigen::VectorXs& _pose_MAP_frame1, co
{
assert(ENU_defined_ && "initializing ENU-MAP yaw without ENU defined");
- Eigen::Vector3s t_MAP_antena1 = _pose_MAP_frame1.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame1(2)) * getP()->getState().head<2>();
- Eigen::Vector3s t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
- Eigen::Vector3s v_MAP_antena1_antena2 = t_MAP_antena2 - t_MAP_antena1;
- Eigen::Vector3s v_ENU_antena1_antena2 = R_ENU_ECEF_ * (_v_ECEF_antena1_antena2);
+ 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;
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));
@@ -204,76 +206,12 @@ void SensorGnss::initializeEnuMapYaw(const Eigen::VectorXs& _pose_MAP_frame1, co
this->getEnuMapYawState()->setState(Eigen::Vector1s(yaw_ENU_MAP));
- Eigen::Matrix3s R_ENU_ECEF;//, R_ENU2_ECEF;
- Eigen::Vector3s 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);
-
- // 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;
-
- // 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));
-
- // 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>();
- t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU;
-
- // set translation state
- setEnuMapTranslationState(t_ENU_MAP);
-
- //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;
- //std::cout << "v_ENU: " << v_ENU.transpose() << std::endl;
- //std::cout << "theta_ENU: " << theta_ENU << std::endl;
- //std::cout << "t_MAP_antena1: " << t_MAP_antenaENU.transpose() << std::endl;
- //std::cout << "t_MAP_antena2: " << t_MAP_antena2.transpose() << std::endl;
- //std::cout << "v_MAP: " << v_MAP.transpose() << std::endl;
- //std::cout << "theta_MAP: " << theta_MAP << std::endl;
- //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 << "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 << "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 << "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 << "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;
-
- WOLF_INFO("SensorGnss: ENU-MAP initialized.")
- }
- // 3D
- else
- {
- //TODO
- std::runtime_error("not implemented yet");
- }
+ ENU_MAP_initialized_ = true;
}
void SensorGnss::setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP)
{
this->getEnuMapTranslationState()->setState(t_ENU_MAP);
-
- ENU_MAP_initialized_ = true;
}
// Define the factory method