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

working!

parent 6f97b6dd
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 206 additions and 55 deletions
include/gnss/processor/processor_gnss_fix.h
+ 5
− 2
View file @ c9fc28f9
...@@ -18,16 +18,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssFix); ...@@ -18,16 +18,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssFix);
struct ProcessorParamsGnssFix : public ProcessorParamsBase struct ProcessorParamsGnssFix : public ProcessorParamsBase
{ {
Scalar time_th; Scalar time_th;
Scalar enu_map_init_dist_min;
ProcessorParamsGnssFix() = default; ProcessorParamsGnssFix() = default;
ProcessorParamsGnssFix(std::string _unique_name, const paramsServer& _server): ProcessorParamsGnssFix(std::string _unique_name, const paramsServer& _server):
ProcessorParamsBase(_unique_name, _server) ProcessorParamsBase(_unique_name, _server)
{ {
time_th = _server.getParam<Scalar>(_unique_name + "/time_th"); time_th = _server.getParam<Scalar>(_unique_name + "/time_th");
enu_map_init_dist_min = _server.getParam<Scalar>(_unique_name + "/enu_map_init_dist_min");
} }
std::string print() std::string print()
{ {
return "\n" + ProcessorParamsBase::print() return "\n" + ProcessorParamsBase::print()
+ "time_th: " + std::to_string(time_th) + "\n"; + "time_th: " + std::to_string(time_th) + "\n";
+ "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n";
} }
}; };
...@@ -36,7 +39,7 @@ class ProcessorGnssFix : public ProcessorBase ...@@ -36,7 +39,7 @@ class ProcessorGnssFix : public ProcessorBase
{ {
protected: protected:
SensorGnssPtr sensor_gnss_ptr_; SensorGnssPtr sensor_gnss_ptr_;
ProcessorParamsGnssFixPtr params_; ProcessorParamsGnssFixPtr params_gnss_;
CaptureGnssFixPtr first_capture_ptr_, last_capture_ptr_; CaptureGnssFixPtr first_capture_ptr_, last_capture_ptr_;
FrameBasePtr last_gnss_fix_KF_; FrameBasePtr last_gnss_fix_KF_;
......
include/gnss/processor/processor_gnss_single_diff.h
+ 24
− 3
View file @ c9fc28f9
...@@ -12,17 +12,38 @@ ...@@ -12,17 +12,38 @@
namespace wolf { namespace wolf {
WOLF_PTR_TYPEDEFS(ProcessorGnssSingleDiff); WOLF_PTR_TYPEDEFS(ProcessorGnssSingleDiff);
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssSingleDiff);
struct ProcessorParamsGnssSingleDiff : public ProcessorParamsBase
{
Scalar time_th;
Scalar enu_map_init_dist_min;
ProcessorParamsGnssSingleDiff() = default;
ProcessorParamsGnssSingleDiff(std::string _unique_name, const paramsServer& _server):
ProcessorParamsBase(_unique_name, _server)
{
time_th = _server.getParam<Scalar>(_unique_name + "/time_th");
enu_map_init_dist_min = _server.getParam<Scalar>(_unique_name + "/enu_map_init_dist_min");
}
std::string print()
{
return "\n" + ProcessorParamsBase::print()
+ "time_th: " + std::to_string(time_th) + "\n";
+ "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n";
}
};
//class //class
class ProcessorGnssSingleDiff : public ProcessorBase class ProcessorGnssSingleDiff : public ProcessorBase
{ {
protected: protected:
SensorGnssPtr sensor_gnss_ptr_; SensorGnssPtr sensor_gnss_ptr_;
ProcessorParamsBasePtr params_; ProcessorParamsGnssSingleDiffPtr params_gnss_;
CaptureGnssSingleDiffPtr last_capture_ptr_; CaptureGnssSingleDiffPtr last_capture_ptr_, incoming_capture_ptr_;
public: public:
ProcessorGnssSingleDiff(ProcessorParamsBasePtr _params, SensorGnssPtr _sensor_gnss_ptr); ProcessorGnssSingleDiff(ProcessorParamsGnssSingleDiffPtr _params_gnss, SensorGnssPtr _sensor_gnss_ptr);
virtual ~ProcessorGnssSingleDiff(); virtual ~ProcessorGnssSingleDiff();
virtual void configure(SensorBasePtr _sensor); virtual void configure(SensorBasePtr _sensor);
......
include/gnss/sensor/sensor_gnss.h
+ 4
− 2
View file @ c9fc28f9
...@@ -44,13 +44,14 @@ struct IntrinsicsGnss : public IntrinsicsBase ...@@ -44,13 +44,14 @@ struct IntrinsicsGnss : public IntrinsicsBase
class SensorGnss : public SensorBase class SensorGnss : public SensorBase
{ {
protected: protected:
IntrinsicsGnssPtr params_;
bool ENU_defined_, ENU_MAP_initialized_; bool ENU_defined_, ENU_MAP_initialized_;
Eigen::Matrix3s R_ENU_ECEF_; Eigen::Matrix3s R_ENU_ECEF_;
Eigen::Vector3s t_ENU_ECEF_; Eigen::Vector3s t_ENU_ECEF_;
public: public:
SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr); SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, IntrinsicsGnssPtr params);
SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, StateBlockPtr _t_ENU_MAP_ptr, StateBlockPtr _roll_ENU_MAP_ptr, StateBlockPtr _pitch_ENU_MAP_ptr, StateBlockPtr _yaw_ENU_MAP_ptr); SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, IntrinsicsGnssPtr params, StateBlockPtr _t_ENU_MAP_ptr, StateBlockPtr _roll_ENU_MAP_ptr, StateBlockPtr _pitch_ENU_MAP_ptr, StateBlockPtr _yaw_ENU_MAP_ptr);
virtual ~SensorGnss(); virtual ~SensorGnss();
...@@ -68,6 +69,7 @@ class SensorGnss : public SensorBase ...@@ -68,6 +69,7 @@ class SensorGnss : public SensorBase
StateBlockPtr getEnuMapYawState() const; StateBlockPtr getEnuMapYawState() const;
void initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, const Eigen::Vector3s& _t_ECEF_antenaENU, 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); const Eigen::VectorXs& _pose_MAP_frame2, const Eigen::Vector3s& _t_ECEF_antena2);
void setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP);
// Gets // Gets
const Eigen::Matrix3s& getREnuEcef() const; const Eigen::Matrix3s& getREnuEcef() const;
......
src/processor/processor_gnss_fix.cpp
+ 27
− 27
View file @ c9fc28f9
...@@ -7,10 +7,10 @@ ...@@ -7,10 +7,10 @@
namespace wolf namespace wolf
{ {
ProcessorGnssFix::ProcessorGnssFix(ProcessorParamsGnssFixPtr _params, SensorGnssPtr _sensor_gnss_ptr) : ProcessorGnssFix::ProcessorGnssFix(ProcessorParamsGnssFixPtr _params_gnss, SensorGnssPtr _sensor_gnss_ptr) :
ProcessorBase("GNSS FIX", _params), ProcessorBase("GNSS FIX", _params_gnss),
sensor_gnss_ptr_(_sensor_gnss_ptr), sensor_gnss_ptr_(_sensor_gnss_ptr),
params_(_params), params_gnss_(_params_gnss),
first_capture_ptr_(nullptr) first_capture_ptr_(nullptr)
{ {
// //
...@@ -34,24 +34,24 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr) ...@@ -34,24 +34,24 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr)
//std::cout << "WOLF: gnss fix in ENU coordinates: " << p_enu.transpose() << std::endl; //std::cout << "WOLF: gnss fix in ENU coordinates: " << p_enu.transpose() << std::endl;
FrameBasePtr new_frame_ptr = nullptr; FrameBasePtr new_frame_ptr = nullptr;
bool new_fac_created = false; FactorBasePtr new_fac = nullptr;
// ALREADY CREATED KF // ALREADY CREATED KF
PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( _capture_ptr, params_->time_tolerance); PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( _capture_ptr, params_gnss_->time_tolerance);
if (KF_pack && KF_pack->key_frame != last_gnss_fix_KF_) if (KF_pack && KF_pack->key_frame != last_gnss_fix_KF_)
{ {
WOLF_DEBUG( "PR ", getName()," - capture ", _capture_ptr->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
new_frame_ptr = KF_pack->key_frame; new_frame_ptr = KF_pack->key_frame;
//WOLF_DEBUG( "PR ", getName()," - capture ", _capture_ptr->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
} }
// MAKE KF // MAKE KF
else if (voteForKeyFrame() && permittedKeyFrame()) else if (voteForKeyFrame() && permittedKeyFrame())
{ {
new_frame_ptr = getProblem()->emplaceFrame(KEY, _capture_ptr->getTimeStamp()); new_frame_ptr = getProblem()->emplaceFrame(KEY, _capture_ptr->getTimeStamp());
getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), params_->time_tolerance); getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), params_gnss_->time_tolerance);
//WOLF_DEBUG( "PR ", getName()," - capture ", _capture_ptr->id(), " appended to new KF " , new_frame_ptr->id() , " TS: ", new_frame_ptr->getTimeStamp()); WOLF_DEBUG( "PR ", getName()," - capture ", _capture_ptr->id(), " appended to new KF " , new_frame_ptr->id() , " TS: ", new_frame_ptr->getTimeStamp());
} }
// ESTABLISH CONSTRAINT // ESTABLISH FACTOR
if (new_frame_ptr) if (new_frame_ptr)
{ {
// LINK CAPTURE // LINK CAPTURE
...@@ -62,7 +62,7 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr) ...@@ -62,7 +62,7 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr)
auto ftr_ptr = FeatureBase::emplace<FeatureGnssFix>(last_capture_ptr_, last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance()); auto ftr_ptr = FeatureBase::emplace<FeatureGnssFix>(last_capture_ptr_, last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance());
// EMPLACE FACTOR // EMPLACE FACTOR
auto new_fac = emplaceFactor(ftr_ptr); new_fac = emplaceFactor(ftr_ptr);
// outlier rejection // outlier rejection
if (sensor_gnss_ptr_->isEnuDefined() && sensor_gnss_ptr_->isEnuMapInitialized()) if (sensor_gnss_ptr_->isEnuDefined() && sensor_gnss_ptr_->isEnuMapInitialized())
...@@ -72,34 +72,34 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr) ...@@ -72,34 +72,34 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr)
// store last KF // store last KF
if (new_fac) if (new_fac)
last_gnss_fix_KF_= new_frame_ptr; last_gnss_fix_KF_= new_frame_ptr;
//WOLF_DEBUG("Factor established");
} }
// INITIALIZE ENU IF: // INITIALIZE ENU IF:
// 1 - not initialized // 1 - not initialized
// 2 - factor established // 2 - factor established
if (!sensor_gnss_ptr_->isEnuDefined() && new_fac_created) if (!sensor_gnss_ptr_->isEnuDefined() && new_fac != nullptr)
{ {
sensor_gnss_ptr_->setEcefEnu(last_capture_ptr_->getData()); sensor_gnss_ptr_->setEcefEnu(last_capture_ptr_->getData());
WOLF_INFO("ProcessorGnssFix: ECEF-ENU initialized.") // Store the first capture that established a factor
first_capture_ptr_ = last_capture_ptr_;
} }
// INITIALIZE ENU_MAP IF 2 NECESSARY CONDITIONS: // INITIALIZE ENU_MAP IF 4 NECESSARY CONDITIONS:
// 1 - not initialized // 1 - ENU-ECEF defined
// 2 - two factors established (first and current) // 2 - not initialized
if (!sensor_gnss_ptr_->isEnuMapInitialized() && first_capture_ptr_ && new_fac_created) // 3 - current capture in key-frame with factor
// 4 - two factors established (first and current) in frames separated enough ( > enu_map_init_dist_min)
if ( sensor_gnss_ptr_->isEnuDefined() &&
!sensor_gnss_ptr_->isEnuMapInitialized() &&
new_fac != nullptr &&
first_capture_ptr_->getFrame() != nullptr && last_capture_ptr_->getFrame() != nullptr &&
(first_capture_ptr_->getFrame()->getState()-last_capture_ptr_->getFrame()->getState()).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(), sensor_gnss_ptr_->initializeEnuMap(first_capture_ptr_->getFrame()->getState(), first_capture_ptr_->getData(),
last_capture_ptr_->getFrame()->getState(), last_capture_ptr_->getData()); last_capture_ptr_->getFrame()->getState(), last_capture_ptr_->getData());
WOLF_INFO("ProcessorGnssFix: ENU-MAP initialized.")
} }
// Store the first capture that established a factor
if (new_fac_created && first_capture_ptr_ == nullptr)
first_capture_ptr_ = last_capture_ptr_;
} }
FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr_ptr) FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr_ptr)
...@@ -108,10 +108,10 @@ FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr_ptr) ...@@ -108,10 +108,10 @@ FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr_ptr)
//WOLF_DEBUG("creating the factor..."); //WOLF_DEBUG("creating the factor...");
// 2D // 2D
if (getProblem()->getDim() == 2) if (getProblem()->getDim() == 2)
return FactorBase::emplace<FactorGnssFix2D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false); return FactorBase::emplace<FactorGnssFix2D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false, FAC_ACTIVE);
// 3D // 3D
else else
return FactorBase::emplace<FactorGnssFix3D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false); return FactorBase::emplace<FactorGnssFix3D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false, FAC_ACTIVE);
} }
bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac_ptr) bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac_ptr)
...@@ -147,7 +147,7 @@ bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac_ptr) ...@@ -147,7 +147,7 @@ bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac_ptr)
bool ProcessorGnssFix::voteForKeyFrame() bool ProcessorGnssFix::voteForKeyFrame()
{ {
// Depending on time since the last KF with gnssfix capture // Depending on time since the last KF with gnssfix capture
if (!last_gnss_fix_KF_ || (last_capture_ptr_->getTimeStamp() - last_gnss_fix_KF_->getTimeStamp()) > params_->time_th) if (!last_gnss_fix_KF_ || (last_capture_ptr_->getTimeStamp() - last_gnss_fix_KF_->getTimeStamp()) > params_gnss_->time_th)
return true; return true;
// TODO: more alternatives? // TODO: more alternatives?
......
src/processor/processor_gnss_single_diff.cpp
+ 39
− 10
View file @ c9fc28f9
...@@ -6,10 +6,10 @@ ...@@ -6,10 +6,10 @@
namespace wolf namespace wolf
{ {
ProcessorGnssSingleDiff::ProcessorGnssSingleDiff(ProcessorParamsBasePtr _params, SensorGnssPtr _sensor_gnss_ptr) : ProcessorGnssSingleDiff::ProcessorGnssSingleDiff(ProcessorParamsGnssSingleDiffPtr _params_gnss, SensorGnssPtr _sensor_gnss_ptr) :
ProcessorBase("GNSS SINGLE DIFFERENCES", _params), ProcessorBase("GNSS SINGLE DIFFERENCES", _params_gnss),
sensor_gnss_ptr_(_sensor_gnss_ptr), sensor_gnss_ptr_(_sensor_gnss_ptr),
params_(_params) params_gnss_(_params_gnss)
{ {
// //
} }
...@@ -24,13 +24,17 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr) ...@@ -24,13 +24,17 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr)
// TODO: keep captures in a buffer and deal with KFpacks // TODO: keep captures in a buffer and deal with KFpacks
//WOLF_DEBUG("ProcessorGnssSingleDiff::process()"); //WOLF_DEBUG("ProcessorGnssSingleDiff::process()");
last_capture_ptr_ = std::static_pointer_cast<CaptureGnssSingleDiff>(_capture_ptr); incoming_capture_ptr_ = std::static_pointer_cast<CaptureGnssSingleDiff>(_capture_ptr);
// discard capture with null or non-key origin frame
if (incoming_capture_ptr_->getOriginFrame() == nullptr || incoming_capture_ptr_->getOriginFrame()->isKey())
return;
FrameBasePtr new_frame_ptr = nullptr; FrameBasePtr new_frame_ptr = nullptr;
// ALREADY CREATED KF // ALREADY CREATED KF
PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( _capture_ptr, params_->time_tolerance); PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( _capture_ptr, params_->time_tolerance);
if (KF_pack && KF_pack->key_frame != last_capture_ptr_->getOriginFrame()) if (KF_pack && KF_pack->key_frame != incoming_capture_ptr_->getOriginFrame())
{ {
new_frame_ptr = KF_pack->key_frame; new_frame_ptr = KF_pack->key_frame;
WOLF_DEBUG( "PR ",getName()," - capture ", _capture_ptr->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp()); WOLF_DEBUG( "PR ",getName()," - capture ", _capture_ptr->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
...@@ -51,7 +55,7 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr) ...@@ -51,7 +55,7 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr)
// EMPLACE FEATURE // EMPLACE FEATURE
//WOLF_DEBUG("emplacing feature..."); //WOLF_DEBUG("emplacing feature...");
auto ftr_ptr = FeatureBase::emplace<FeatureGnssSingleDiff>(last_capture_ptr_, last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance()); auto ftr_ptr = FeatureBase::emplace<FeatureGnssSingleDiff>(incoming_capture_ptr_, incoming_capture_ptr_->getData(),incoming_capture_ptr_->getDataCovariance());
// ADD CONSTRAINT // ADD CONSTRAINT
//WOLF_DEBUG("emplacing factor..."); //WOLF_DEBUG("emplacing factor...");
...@@ -59,6 +63,25 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr) ...@@ -59,6 +63,25 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr)
//WOLF_DEBUG("Factor established"); //WOLF_DEBUG("Factor established");
} }
// INITIALIZE ENU_MAP IF 4 NECESSARY CONDITIONS:
// 1 - ENU-ECEF defined
// 2 - not initialized
// 3 - current capture in key-frame with factor
// 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 &&
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());
}
last_capture_ptr_ = incoming_capture_ptr_;
incoming_capture_ptr_ = nullptr;
} }
FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr_ptr) FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr_ptr)
...@@ -67,7 +90,7 @@ FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr_ptr) ...@@ -67,7 +90,7 @@ FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr_ptr)
//WOLF_DEBUG("creating the factor..."); //WOLF_DEBUG("creating the factor...");
// 2D // 2D
if (getProblem()->getDim() == 2) if (getProblem()->getDim() == 2)
return FactorBase::emplace<FactorGnssSingleDiff2D>(ftr_ptr, ftr_ptr, last_capture_ptr_->getOriginFrame(), sensor_gnss_ptr_, shared_from_this()); return FactorBase::emplace<FactorGnssSingleDiff2D>(ftr_ptr, ftr_ptr, incoming_capture_ptr_->getOriginFrame(), sensor_gnss_ptr_, shared_from_this());
// 3D TODO // 3D TODO
else else
std::runtime_error("Single Differences in 3D not implemented yet."); std::runtime_error("Single Differences in 3D not implemented yet.");
...@@ -77,8 +100,14 @@ FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr_ptr) ...@@ -77,8 +100,14 @@ FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr_ptr)
bool ProcessorGnssSingleDiff::voteForKeyFrame() bool ProcessorGnssSingleDiff::voteForKeyFrame()
{ {
// TODO // Depending on time since the last KF with gnssfix capture
return true; if (last_capture_ptr_==nullptr || (last_capture_ptr_->getTimeStamp() - incoming_capture_ptr_->getTimeStamp()) > params_gnss_->time_th)
return true;
// TODO: more alternatives?
// otherwise
return false;
} }
void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor) void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor)
...@@ -88,7 +117,7 @@ void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor) ...@@ -88,7 +117,7 @@ void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor)
ProcessorBasePtr ProcessorGnssSingleDiff::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr) ProcessorBasePtr ProcessorGnssSingleDiff::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
{ {
ProcessorGnssSingleDiffPtr prc_ptr = std::make_shared<ProcessorGnssSingleDiff>(_params, std::static_pointer_cast<SensorGnss>(sensor_ptr)); ProcessorGnssSingleDiffPtr prc_ptr = std::make_shared<ProcessorGnssSingleDiff>(std::static_pointer_cast<ProcessorParamsGnssSingleDiff>(_params), std::static_pointer_cast<SensorGnss>(sensor_ptr));
prc_ptr->setName(_unique_name); prc_ptr->setName(_unique_name);
return prc_ptr; return prc_ptr;
} }
......
src/sensor/sensor_gnss.cpp
+ 104
− 8
View file @ c9fc28f9
...@@ -12,9 +12,11 @@ static Scalar kSecondEccentricitySquared = 6.73949674228 * 0.001; ...@@ -12,9 +12,11 @@ static Scalar kSecondEccentricitySquared = 6.73949674228 * 0.001;
//static Scalar kFlattening = 1 / 298.257223563; //static Scalar kFlattening = 1 / 298.257223563;
SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D position with respect to the car frame (base frame) SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D position with respect to the car frame (base frame)
StateBlockPtr _bias_ptr) //GNSS sensor time bias StateBlockPtr _bias_ptr, //GNSS sensor time bias
IntrinsicsGnssPtr params) //sensor params
: :
SensorBase("GNSS", _p_ptr, nullptr, _bias_ptr, 0), // antena orientation has no sense in GNSS SensorBase("GNSS", _p_ptr, nullptr, _bias_ptr, 0), // antena orientation has no sense in GNSS
params_(params),
ENU_defined_(false), ENU_defined_(false),
ENU_MAP_initialized_(false) ENU_MAP_initialized_(false)
{ {
...@@ -22,20 +24,22 @@ SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D positi ...@@ -22,20 +24,22 @@ SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS antena 3D positi
assert((_bias_ptr == nullptr || _bias_ptr->getSize() == 1 )&& "Bad bias size"); assert((_bias_ptr == nullptr || _bias_ptr->getSize() == 1 )&& "Bad bias size");
getStateBlockVec().resize(7); getStateBlockVec().resize(7);
setStateBlockPtrStatic(3, std::make_shared<StateBlock>(3, true)); setStateBlockPtrStatic(3, std::make_shared<StateBlock>(3, params_->translation_fixed_));
setStateBlockPtrStatic(4, std::make_shared<StateBlock>(1, true)); setStateBlockPtrStatic(4, std::make_shared<StateBlock>(1, params_->roll_fixed_));
setStateBlockPtrStatic(5, std::make_shared<StateBlock>(1, true)); setStateBlockPtrStatic(5, std::make_shared<StateBlock>(1, params_->pitch_fixed_));
setStateBlockPtrStatic(6, std::make_shared<StateBlock>(1, true)); setStateBlockPtrStatic(6, std::make_shared<StateBlock>(1, params_->yaw_fixed_));
} }
SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS sensor position with respect to the car frame (base frame) SensorGnss::SensorGnss(StateBlockPtr _p_ptr, //GNSS sensor position with respect to the car frame (base frame)
StateBlockPtr _bias_ptr, //GNSS sensor time bias StateBlockPtr _bias_ptr, //GNSS sensor time bias
IntrinsicsGnssPtr params, //sensor params
StateBlockPtr _t_ENU_MAP_ptr, //ENU_MAP translation StateBlockPtr _t_ENU_MAP_ptr, //ENU_MAP translation
StateBlockPtr _roll_ENU_MAP_ptr, //ENU_MAP Roll StateBlockPtr _roll_ENU_MAP_ptr, //ENU_MAP Roll
StateBlockPtr _pitch_ENU_MAP_ptr, //ENU_MAP Pitch StateBlockPtr _pitch_ENU_MAP_ptr, //ENU_MAP Pitch
StateBlockPtr _yaw_ENU_MAP_ptr) //ENU_MAP Yaw StateBlockPtr _yaw_ENU_MAP_ptr) //ENU_MAP Yaw
: :
SensorBase("GNSS", _p_ptr, nullptr, _bias_ptr, 0),// antena orientation has no sense in GNSS SensorBase("GNSS", _p_ptr, nullptr, _bias_ptr, 0),// antena orientation has no sense in GNSS
params_(params),
ENU_defined_(false), ENU_defined_(false),
ENU_MAP_initialized_(false) ENU_MAP_initialized_(false)
{ {
...@@ -105,6 +109,8 @@ void SensorGnss::setEcefEnu(const Eigen::Vector3s& _t_ECEF_ENU) ...@@ -105,6 +109,8 @@ void SensorGnss::setEcefEnu(const Eigen::Vector3s& _t_ECEF_ENU)
{ {
computeEnuEcef(_t_ECEF_ENU, R_ENU_ECEF_, t_ENU_ECEF_); computeEnuEcef(_t_ECEF_ENU, R_ENU_ECEF_, t_ENU_ECEF_);
ENU_defined_ = true; ENU_defined_ = true;
WOLF_INFO("SensorGnss: ECEF-ENU initialized.")
} }
void SensorGnss::setEnuEcef(const Eigen::Matrix3s& _R_ENU_ECEF, const Eigen::Vector3s& _t_ENU_ECEF) void SensorGnss::setEnuEcef(const Eigen::Matrix3s& _R_ENU_ECEF, const Eigen::Vector3s& _t_ENU_ECEF)
...@@ -145,7 +151,90 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con ...@@ -145,7 +151,90 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con
getEnuMapPitchState()->getState()(0), getEnuMapPitchState()->getState()(0),
getEnuMapYawState()->getState()(0)).topLeftCorner<2,2>(); getEnuMapYawState()->getState()(0)).topLeftCorner<2,2>();
t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU; t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU;
this->getEnuMapTranslationState()->setState(t_ENU_MAP);
// 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");
}
}
void SensorGnss::initializeEnuMapYaw(const Eigen::VectorXs& _pose_MAP_frame1, const Eigen::VectorXs& _pose_MAP_frame2,
const Eigen::Vector3s& _v_ECEF_antena1_antena2)
{
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);
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));
Scalar yaw_ENU_MAP = theta_ENU-theta_MAP;
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 << "-----------------------------------------\n";
//std::cout << "t_ECEF_antenaENU: " << _t_ECEF_antenaENU.transpose() << std::endl; //std::cout << "t_ECEF_antenaENU: " << _t_ECEF_antenaENU.transpose() << std::endl;
...@@ -169,6 +258,8 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con ...@@ -169,6 +258,8 @@ 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 << "should be: " << atan2(v_ENU(1),v_ENU(0)) << std::endl;
//std::cout << "v_ENU.norm(): " << v_ENU.norm() << std::endl; //std::cout << "v_ENU.norm(): " << v_ENU.norm() << std::endl;
//std::cout << "v_MAP.norm(): " << v_MAP.norm() << std::endl; //std::cout << "v_MAP.norm(): " << v_MAP.norm() << std::endl;
WOLF_INFO("SensorGnss: ENU-MAP initialized.")
} }
// 3D // 3D
else else
...@@ -176,6 +267,11 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con ...@@ -176,6 +267,11 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con
//TODO //TODO
std::runtime_error("not implemented yet"); std::runtime_error("not implemented yet");
} }
}
void SensorGnss::setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP)
{
this->getEnuMapTranslationState()->setState(t_ENU_MAP);
ENU_MAP_initialized_ = true; ENU_MAP_initialized_ = true;
} }
...@@ -191,7 +287,7 @@ SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::V ...@@ -191,7 +287,7 @@ SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::V
StateBlockPtr p_ptr = std::make_shared<StateBlock>(_extrinsics.head(3), intrinsics_gnss_ptr->extrinsics_fixed_); StateBlockPtr p_ptr = std::make_shared<StateBlock>(_extrinsics.head(3), intrinsics_gnss_ptr->extrinsics_fixed_);
if (_extrinsics.size() == 3) if (_extrinsics.size() == 3)
sen = std::make_shared<SensorGnss>(p_ptr, nullptr); sen = std::make_shared<SensorGnss>(p_ptr, nullptr, intrinsics_gnss_ptr);
else else
{ {
StateBlockPtr p_ptr = std::make_shared<StateBlock>(_extrinsics.head<3>(), intrinsics_gnss_ptr->extrinsics_fixed_); StateBlockPtr p_ptr = std::make_shared<StateBlock>(_extrinsics.head<3>(), intrinsics_gnss_ptr->extrinsics_fixed_);
...@@ -200,7 +296,7 @@ SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::V ...@@ -200,7 +296,7 @@ SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::V
StateBlockPtr pitch_ptr = std::make_shared<StateBlock>(_extrinsics.segment<1>(7), intrinsics_gnss_ptr->pitch_fixed_); StateBlockPtr pitch_ptr = std::make_shared<StateBlock>(_extrinsics.segment<1>(7), intrinsics_gnss_ptr->pitch_fixed_);
StateBlockPtr yaw_ptr = std::make_shared<StateBlock>(_extrinsics.segment<1>(8), intrinsics_gnss_ptr->yaw_fixed_); StateBlockPtr yaw_ptr = std::make_shared<StateBlock>(_extrinsics.segment<1>(8), intrinsics_gnss_ptr->yaw_fixed_);
sen = std::make_shared<SensorGnss>(p_ptr, nullptr, translation_ptr, roll_ptr, pitch_ptr, yaw_ptr); sen = std::make_shared<SensorGnss>(p_ptr, nullptr, intrinsics_gnss_ptr, translation_ptr, roll_ptr, pitch_ptr, yaw_ptr);
} }
sen->setName(_unique_name); sen->setName(_unique_name);
......
test/gtest_factor_gnss_fix_2D.cpp
+ 3
− 3
View file @ c9fc28f9
...@@ -100,7 +100,7 @@ TEST(FactorGnssFix2DTest, configure_tree) ...@@ -100,7 +100,7 @@ TEST(FactorGnssFix2DTest, configure_tree)
ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100; ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100;
// Configure sensor and processor // Configure sensor and processor
gnss_sensor_ptr->getEnuMapTranslationState()->setState(t_enu_map); gnss_sensor_ptr->setEnuMapTranslationState(t_enu_map);
gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map); gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map);
gnss_sensor_ptr->getEnuMapRollState()->fix(); gnss_sensor_ptr->getEnuMapRollState()->fix();
gnss_sensor_ptr->getEnuMapPitchState()->fix(); gnss_sensor_ptr->getEnuMapPitchState()->fix();
...@@ -117,8 +117,8 @@ TEST(FactorGnssFix2DTest, configure_tree) ...@@ -117,8 +117,8 @@ TEST(FactorGnssFix2DTest, configure_tree)
problem_ptr->keyFrameCallback(frame_ptr, nullptr, 1.0); problem_ptr->keyFrameCallback(frame_ptr, nullptr, 1.0);
// Create & process GNSS Fix capture // Create & process GNSS Fix capture
// CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity()); 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>(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);
gnss_sensor_ptr->process(cap_gnss_ptr); gnss_sensor_ptr->process(cap_gnss_ptr);
// Checks // Checks
......
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