hotfix

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

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_;

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