diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
index fce80394e5c29fb52501b312d970a25b946052c5..e8dae37ab33d2d741f70567e63a296b149e86240 100644
--- a/include/gnss/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -43,8 +43,7 @@ class ProcessorGnssFix : public ProcessorBase
protected:
SensorGnssPtr sensor_gnss_;
ProcessorParamsGnssFixPtr params_gnss_;
- CaptureGnssFixPtr first_capture_, incoming_capture_;
- FrameBasePtr last_KF_;
+ CaptureGnssFixPtr first_capture_, last_KF_capture_, incoming_capture_;
public:
ProcessorGnssFix(ProcessorParamsGnssFixPtr _params, SensorGnssPtr _sensor_gnss_ptr);
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index dd8ba5aeb905616f7f2a768ed4b0d9d842da542a..13686e0321d7beadab8d5ded80a2e59ea190ebbc 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -45,7 +45,7 @@ class SensorGnss : public SensorBase
{
protected:
IntrinsicsGnssPtr params_;
- bool ENU_defined_, ENU_MAP_initialized_;
+ bool ENU_defined_, t_ENU_MAP_initialized_, R_ENU_MAP_initialized_;
Eigen::Matrix3s R_ENU_ECEF_;
Eigen::Vector3s t_ENU_ECEF_;
@@ -66,6 +66,8 @@ class SensorGnss : public SensorBase
Eigen::Vector3s gettEnuMap() const;
bool isEnuDefined() const;
bool isEnuMapInitialized() const;
+ bool isEnuMapTranslationInitialized() const;
+ bool isEnuMapRotationInitialized() const;
// Sets
void setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP);
@@ -97,7 +99,17 @@ inline bool SensorGnss::isEnuDefined() const
inline bool SensorGnss::isEnuMapInitialized() const
{
- return ENU_MAP_initialized_;
+ return t_ENU_MAP_initialized_ && R_ENU_MAP_initialized_;
+}
+
+inline bool SensorGnss::isEnuMapTranslationInitialized() const
+{
+ return t_ENU_MAP_initialized_;
+}
+
+inline bool SensorGnss::isEnuMapRotationInitialized() const
+{
+ return R_ENU_MAP_initialized_;
}
inline StateBlockPtr SensorGnss::getEnuMapTranslation() const
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 89d42df2a49051cc205a494db2334c948c3b204b..4800b73b3968e68e2046b17d244ccb6856012425 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -32,7 +32,7 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
// ALREADY CREATED KF
PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( incoming_capture_, params_gnss_->time_tolerance);
- if (KF_pack && KF_pack->key_frame != last_KF_)
+ if (KF_pack && (last_KF_capture_!=nullptr || KF_pack->key_frame != last_KF_capture_->getFrame()))
{
WOLF_DEBUG( "PR ", getName()," - capture ", incoming_capture_->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
new_frame = KF_pack->key_frame;
@@ -69,7 +69,7 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
}
// store last KF
- last_KF_= new_frame;
+ last_KF_capture_ = incoming_capture_;
// Define ENU (if not defined)
if (!sensor_gnss_->isEnuDefined())
@@ -142,29 +142,34 @@ bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac)
bool ProcessorGnssFix::voteForKeyFrame()
{
// Depending on time since the last KF with gnssfix capture
- if (!last_KF_ || (incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) > params_gnss_->time_th)
- return true;
+ if (!last_KF_capture_ || (incoming_capture_->getTimeStamp() - last_KF_capture_->getTimeStamp()) > params_gnss_->time_th)
+ {
+ WOLF_DEBUG("KF because of null lastKF or time since last KF");
+ return true;
+ }
// ENU not defined
if (!sensor_gnss_->isEnuDefined())
+ {
+ WOLF_DEBUG("KF because of enu not defined");
return true;
- // ENU-MAP not initialized
- if (!sensor_gnss_->isEnuMapInitialized())
+ }
+ // ENU-MAP not initialized and can be initialized
+ if ( sensor_gnss_->isEnuDefined() &&
+ !sensor_gnss_->isEnuMapInitialized() &&
+ (first_capture_->getData()-incoming_capture_->getData()).norm() > params_gnss_->enu_map_init_dist_min)
{
+ WOLF_DEBUG("KF because of enu map not initialized");
assert(first_capture_ != nullptr);
- // if distance enough to initialize: make KF
- if ((first_capture_->getData()-incoming_capture_->getData()).norm() > params_gnss_->enu_map_init_dist_min)
- return true;
- else
- return false;
+ return true;
}
// Distance criterion (ENU defined and ENU-MAP initialized)
- // convert fix data from ECEF to MAP reference frame
- Eigen::Vector3s incoming_fix_map = sensor_gnss_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * incoming_capture_->getData() + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap());
- if ((incoming_fix_map.head(2) - last_KF_->getP()->getState()).norm() > params_gnss_->dist_traveled)
- return true;
-
+ if (last_KF_capture_ != nullptr && (incoming_capture_->getData() - last_KF_capture_->getData()).norm() > params_gnss_->dist_traveled)
+ {
+ WOLF_DEBUG("KF because of distance criterion: ", (incoming_capture_->getData() - last_KF_capture_->getData()).norm());
+ return true;
+ }
// TODO: more alternatives?
// otherwise
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index c1e0e65eadef6d9ae9dee21ade9d54a3bea3fe2d..b1aaba28dab8a5acf51e3d4ca3ba5f266408c60d 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -23,7 +23,7 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture)
{
// TODO: keep captures in a buffer and deal with KFpacks
- //WOLF_DEBUG("ProcessorGnssSingleDiff::process()");
+ WOLF_INFO("ProcessorGnssSingleDiff::process()");
incoming_capture_ = std::static_pointer_cast<CaptureGnssSingleDiff>(_capture);
// discard capture with null or non-key origin frame
@@ -76,7 +76,7 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture)
// 3 - current capture in key-frame with factor
// 4 - frames constained by the factor separated enough ( > enu_map_init_dist_min)
if ( sensor_gnss_->isEnuDefined() &&
- !sensor_gnss_->isEnuMapInitialized() &&
+ !sensor_gnss_->isEnuMapRotationInitialized() &&
new_frame != nullptr &&
incoming_capture_->getFrame() != nullptr && incoming_capture_->getFrame()->isKey() &&
incoming_capture_->getData().norm() > params_gnss_->enu_map_init_dist_min)
@@ -109,11 +109,11 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame()
//std::cout << "params_gnss_->time_th" << params_gnss_->time_th << std::endl;
//std::cout << "(incoming_capture_->getTimeStamp() - incoming_capture_->getTimeStamp())" << (incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) << std::endl;
- // Depending on time since the last KF with gnssfix capture
+ // Elapsed time criterion: From the last KF with gnssfix capture
if ((incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) > params_gnss_->time_th)
return true;
- // Distance criterion
+ // Distance criterion: From the last KF with gnssfix capture
Eigen::Vector2s v_origin_current = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
Eigen::Vector2s v_lastKF_origin = incoming_capture_->getOriginFrame()->getP()->getState() - last_KF_->getP()->getState();
//std::cout << "params_gnss_->dist_traveled" << params_gnss_->dist_traveled << std::endl;
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index d422362c3300eed41696dd290a164dc9a2b01505..34f0e73e85e56c8c62b969ec42e55b10f863ae4c 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -18,7 +18,8 @@ SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D positi
SensorBase("GNSS", _p_ptr, nullptr, _bias_ptr, 0), // antena orientation has no sense in GNSS
params_(params),
ENU_defined_(false),
- ENU_MAP_initialized_(false)
+ t_ENU_MAP_initialized_(false),
+ R_ENU_MAP_initialized_(false)
{
assert(_p_ptr->getSize() == 3 && "Bad extrinsics size");
assert((_bias_ptr == nullptr || _bias_ptr->getSize() == 1 )&& "Bad bias size");
@@ -41,7 +42,8 @@ SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS sensor position
SensorBase("GNSS", _p_ptr, nullptr, _bias_ptr, 0),// antena orientation has no sense in GNSS
params_(params),
ENU_defined_(false),
- ENU_MAP_initialized_(false)
+ t_ENU_MAP_initialized_(false),
+ R_ENU_MAP_initialized_(false)
{
assert(_p_ptr->getSize() == 3 && "Bad extrinsics size");
assert((_bias_ptr == nullptr || _bias_ptr->getSize() == 1 )&& "Bad bias size");
@@ -139,22 +141,32 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con
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_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),
- getEnuMapPitch()->getState()(0),
- getEnuMapYaw()->getState()(0)).topLeftCorner<2,2>();
+ // ENU-MAP Rotation
+ Eigen::Matrix2s R_ENU_MAP;
+ // get it if already initialized
+ if (R_ENU_MAP_initialized_)
+ R_ENU_MAP = getREnuMap().topLeftCorner<2,2>();
+ // initialize yaw if not initialized
+ else
+ {
+ 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;
+ setEnuMapYawState(yaw_ENU_MAP);
+
+ R_ENU_MAP = computeREnuMap(getEnuMapRoll()->getState()(0),
+ getEnuMapPitch()->getState()(0),
+ getEnuMapYaw()->getState()(0)).topLeftCorner<2,2>();
+
+ WOLF_INFO("SensorGnss: ENU-MAP rotation initialized.");
+ }
+ // ENU-MAP translation
+ Eigen::Vector3s t_ENU_MAP(Eigen::Vector3s::Zero());
t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU;
-
- // set translation state
setEnuMapTranslationState(t_ENU_MAP);
+ WOLF_INFO("SensorGnss: ENU-MAP translation initialized.");
+
//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;
@@ -177,8 +189,6 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con
//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
@@ -203,34 +213,36 @@ void SensorGnss::initializeEnuMapYaw(const Eigen::VectorXs& _pose_MAP_frame1, co
Scalar yaw_ENU_MAP = theta_ENU-theta_MAP;
setEnuMapYawState(yaw_ENU_MAP);
+
+ WOLF_INFO("SensorGnss: ENU-MAP Rotation initialized.")
}
void SensorGnss::setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP)
{
getEnuMapTranslation()->setState(t_ENU_MAP);
- ENU_MAP_initialized_ = true;
+ t_ENU_MAP_initialized_ = true;
}
void SensorGnss::setEnuMapRollState(const Scalar& roll_ENU_MAP)
{
getEnuMapRoll()->setState(Eigen::Vector1s(roll_ENU_MAP));
- ENU_MAP_initialized_ = true;
+ R_ENU_MAP_initialized_ = true;
}
void SensorGnss::setEnuMapPitchState(const Scalar& pitch_ENU_MAP)
{
getEnuMapPitch()->setState(Eigen::Vector1s(pitch_ENU_MAP));
- ENU_MAP_initialized_ = true;
+ R_ENU_MAP_initialized_ = true;
}
void SensorGnss::setEnuMapYawState(const Scalar& yaw_ENU_MAP)
{
getEnuMapYaw()->setState(Eigen::Vector1s(yaw_ENU_MAP));
- ENU_MAP_initialized_ = true;
+ R_ENU_MAP_initialized_ = true;
}
Eigen::Matrix3s SensorGnss::getREnuMap() const