Skip to content
Snippets Groups Projects
Commit 6a36f645 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

improved voteForKF and enu-map initialization

parent d826eed6
No related branches found
No related tags found
3 merge requests!28release after RAL,!27After 2nd RAL submission,!10Resolve "GAUSS Project"
Hide whitespace changes
Inline Side-by-side
include/gnss/processor/processor_gnss_fix.h
+ 1
− 2
View file @ 6a36f645
......@@ -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);
......
include/gnss/sensor/sensor_gnss.h
+ 14
− 2
View file @ 6a36f645
......@@ -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
......
src/processor/processor_gnss_fix.cpp
+ 21
− 16
View file @ 6a36f645
......@@ -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
......
src/processor/processor_gnss_single_diff.cpp
+ 4
− 4
View file @ 6a36f645
......@@ -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;
......
src/sensor/sensor_gnss.cpp
+ 33
− 21
View file @ 6a36f645
......@@ -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
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment