diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
index 1fa78ddd321ef08d9c1d557a7bd2ea7a5fac703a..a2bdf9edc92a504082fc1b5fbecabec311c10687 100644
--- a/include/gnss/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -23,8 +23,8 @@
#define WOLF_PROCESSOR_GNSS_FIX_H
// Wolf includes
+#include "gnss/internal/config.h"
#include "core/processor/processor_base.h"
-#include "gnss/capture/capture_gnss.h"
#include "gnss/sensor/sensor_gnss.h"
// Std includes
@@ -127,9 +127,8 @@ class ProcessorGnssFix : public ProcessorBase
SensorGnssPtr sensor_gnss_;
ParamsProcessorGnssFixPtr params_gnss_;
CaptureBasePtr last_KF_capture_, incoming_capture_;
- FeatureGnssFixPtr last_KF_feature_;
+ FeatureBasePtr last_KF_feature_, incoming_feature_;
FrameBasePtr last_KF_;
- GnssUtils::ComputePosOutput incoming_pos_out_;
Eigen::Vector3d first_pos_;
VectorComposite first_frame_state_;
@@ -155,43 +154,42 @@ class ProcessorGnssFix : public ProcessorBase
*/
void processKeyFrame(FrameBasePtr _keyframe_ptr) override {};
+
/** \brief trigger in capture
*
* Returns true if processCapture() should be called after the provided capture arrived.
*/
- bool triggerInCapture(CaptureBasePtr) const override;
+ bool triggerInCapture(CaptureBasePtr) const override {return true;};
/** \brief trigger in key-frame
*
* The ProcessorTracker only processes incoming captures, then it returns false.
*/
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;}
+ bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;};
/** \brief store key frame
*
* Returns true if the key frame should be stored
*/
- bool storeKeyFrame(FrameBasePtr) override;
+ bool storeKeyFrame(FrameBasePtr) override {return true;};
/** \brief store capture
*
* Returns true if the capture should be stored
*/
- bool storeCapture(CaptureBasePtr) override;
+ bool storeCapture(CaptureBasePtr) override {return false;};
bool voteForKeyFrame() const override;
private:
+ void processStoredFrames();
+ void handleEnuMap(FeatureBasePtr incoming_feature);
+ FeatureBasePtr emplaceFeature(CaptureBasePtr _capture);
FactorBasePtr emplaceFactor(FeatureBasePtr _ftr_ptr);
bool detectOutlier(FactorBasePtr ctr_ptr);
};
-inline bool ProcessorGnssFix::triggerInCapture(CaptureBasePtr) const
-{
- return true;
-}
-
} // namespace wolf
#endif //WOLF_PROCESSOR_GNSS_FIX_H
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 31f95fd152bb034143384d3d96ee03b5029c6479..6066c1f0aea1e80fbfb698fda952de48cd9bf872 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -19,12 +19,12 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+#include "gnss/processor/processor_gnss_fix.h"
#include "gnss/capture/capture_gnss.h"
#include "gnss/capture/capture_gnss_fix.h"
#include "gnss/factor/factor_gnss_fix_2d.h"
#include "gnss/factor/factor_gnss_fix_3d.h"
#include "gnss/feature/feature_gnss_fix.h"
-#include "gnss/processor/processor_gnss_fix.h"
namespace wolf
{
@@ -41,59 +41,148 @@ ProcessorGnssFix::~ProcessorGnssFix()
//
}
+void ProcessorGnssFix::processStoredFrames()
+{
+ // clean buffers (useless entries)
+ if (not buffer_frame_.empty() and not buffer_capture_.empty())
+ {
+ buffer_frame_ .removeUpToLower(buffer_capture_.getContainer().begin()->first-params_->time_tolerance);
+ buffer_capture_.removeUpToLower(buffer_frame_ .getContainer().begin()->first-params_->time_tolerance);
+ }
+
+ // iterate over stored frames
+ auto frm_it = buffer_frame_.getContainer().begin();
+ while (frm_it != buffer_frame_.getContainer().end())
+ {
+ // Search for any stored capture within time tolerance of frame
+ auto capture = buffer_capture_.select(frm_it->first, params_->time_tolerance);
+
+ // Capture found
+ if (capture)
+ {
+ // safety check: capture stored already linked
+ if (capture->getFrame())
+ {
+ WOLF_WARN("ProcessorGnssFix::processStoredFrames: any stored CaptureGnss was already linked to a frame");
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
+ frm_it++;
+ continue;
+ }
+ // link capture
+ capture->link(frm_it->second);
+
+ // get feature
+ FeatureGnssFixPtr feat_fix;
+ auto feature_list = capture->getFeatureList();
+ for (auto feat : feature_list)
+ {
+ feat_fix = std::dynamic_pointer_cast<FeatureGnssFix>(feat);
+ if (feat_fix)
+ break;
+ }
+ if (not feat_fix)
+ {
+ WOLF_WARN("ProcessorGnssFix::processStoredFrames: A stored CaptureGnss doesn't have any FrameGnssFix");
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
+ frm_it++;
+ continue;
+ }
+
+ // emplace factor
+ auto fac = emplaceFactor(feat_fix);
+
+ // outlier rejection (only can be evaluated if ENU defined and ENU-MAP well initialized: fixed)
+ if (params_gnss_->remove_outliers and detectOutlier(fac))
+ {
+ feat_fix->remove();
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
+ frm_it++;
+ continue;
+ }
+
+ // Handle ENU definition, ENU-MAP initialization etc.
+ handleEnuMap(feat_fix);
+
+ // store last KF (if more recent)
+ if (not last_KF_capture_ or last_KF_capture_->getTimeStamp() < capture->getTimeStamp())
+ {
+ last_KF_capture_ = capture;
+ last_KF_feature_ = feat_fix;
+ }
+
+ // remove capture from buffer
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
+
+ // remove frame from buffer
+ frm_it = buffer_frame_.getContainer().erase(frm_it);
+ }
+ else
+ frm_it++;
+ }
+}
+
+FeatureBasePtr ProcessorGnssFix::emplaceFeature(CaptureBasePtr _capture)
+{
+ GnssUtils::ComputePosOutput pos_output;
+ if (std::dynamic_pointer_cast<CaptureGnss>(_capture))
+ {
+ WOLF_DEBUG("ProcessorGnssFix: creating feature from incoming raw measurement. Type:", _capture->getType());
+ auto raw_capture = std::static_pointer_cast<CaptureGnss>(_capture);
+ pos_output = GnssUtils::computePos(*(raw_capture->getObservations()), *(raw_capture->getNavigation()), params_gnss_->compute_pos_opt);
+ if (!pos_output.success) // error
+ {
+ WOLF_DEBUG("computePos failed with msg: ", pos_output.msg);
+ return nullptr;
+ }
+ }
+ else if (std::dynamic_pointer_cast<CaptureGnssFix>(_capture))
+ {
+ WOLF_DEBUG("ProcessorGnssFix: creating feature from incoming navsatfix. Type:", _capture->getType());
+ auto fix_capture = std::static_pointer_cast<CaptureGnssFix>(_capture);
+ pos_output.time = fix_capture->getGpsTimeStamp().getSeconds(); // time_t time;
+ pos_output.sec = 1e-9 * fix_capture->getGpsTimeStamp().getSeconds(); // double sec;
+ pos_output.pos = fix_capture->getPositionEcef(); // Eigen::Vector3d pos; // position (m)
+ pos_output.vel = Eigen::Vector3d::Zero(); // Eigen::Vector3d vel; // velocity (m/s)
+ pos_output.pos_covar = fix_capture->getCovarianceEcef(); // Eigen::Matrix3d pos_covar; // position covariance (m^2)
+ pos_output.type = 0; // int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
+ }
+ else
+ throw std::runtime_error("ProcessorGnssFix::emplaceFeature wrong capture type");
+
+ return FeatureBase::emplace<FeatureGnssFix>(_capture, pos_output);
+}
+
void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
{
- // TODO: keep captures in a buffer and deal with KFpacks
WOLF_DEBUG("PR ", getName()," process() capture type: ", _capture->getType());
- incoming_capture_ = _capture;
- bool KF_created = false;
+ // Check capture type
+ if (not std::dynamic_pointer_cast<CaptureGnss>(_capture) and not std::dynamic_pointer_cast<CaptureGnssFix>(_capture))
+ throw std::runtime_error("ProcessorGnssFix::processCapture capture of bad type. Accepted types: CaptureGnss and CaptureGnssFix");
- // Check type of capture: GNSS raw
- bool israw = false;
- if (std::dynamic_pointer_cast<CaptureGnss>(_capture)!=nullptr)
- israw = true;
- // if not raw nor Fix, not processable
- else if (std::dynamic_pointer_cast<CaptureGnssFix>(_capture)==nullptr)
- return;
+ // First process stored frames
+ processStoredFrames();
- // Only process raw if fix_from_raw
- if (israw and !params_gnss_->fix_from_raw)
+ // Only process raw if fix_from_raw and fix if not fix_from_raw
+ bool israw = std::dynamic_pointer_cast<CaptureGnss>(_capture)!=nullptr;
+ if (israw and not params_gnss_->fix_from_raw)
return;
-
- // Only process fix if not fix_from_raw
- if (!israw and params_gnss_->fix_from_raw)
+ if (not israw and params_gnss_->fix_from_raw)
return;
+ // Process incoming capture
+ incoming_capture_ = _capture;
+ bool KF_created = false;
FrameBasePtr new_frame = nullptr;
FactorBasePtr new_fac = nullptr;
- // emplace features
- if (israw)
- {
- auto raw_capture = std::static_pointer_cast<CaptureGnss>(incoming_capture_);
- incoming_pos_out_ = GnssUtils::computePos(*(raw_capture->getObservations()), *(raw_capture->getNavigation()), params_gnss_->compute_pos_opt);
- if (!incoming_pos_out_.success) // error
- {
- WOLF_DEBUG("computePos failed with msg: ", incoming_pos_out_.msg);
- return;
- }
- }
- else
- {
- WOLF_DEBUG("ProcessorGnssFix: creating feature from incoming navsatfix. Type:", incoming_capture_->getType());
- auto fix_capture = std::static_pointer_cast<CaptureGnssFix>(incoming_capture_);
- incoming_pos_out_.time = fix_capture->getGpsTimeStamp().getSeconds(); // time_t time;
- incoming_pos_out_.sec = 1e-9 * fix_capture->getGpsTimeStamp().getSeconds(); // double sec;
- incoming_pos_out_.pos = fix_capture->getPositionEcef(); // Eigen::Vector3d pos; // position (m)
- incoming_pos_out_.vel = Eigen::Vector3d::Zero(); // Eigen::Vector3d vel; // velocity (m/s)
- incoming_pos_out_.pos_covar = fix_capture->getCovarianceEcef(); // Eigen::Matrix3d pos_covar; // position covariance (m^2)
- incoming_pos_out_.type = 0; // int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
- }
- auto incoming_feature = FeatureBase::emplace<FeatureGnssFix>(incoming_capture_, incoming_pos_out_);
+ // emplace feature
+ incoming_feature_ = emplaceFeature(incoming_capture_);
+ if (not incoming_feature_)
+ return;
// ALREADY CREATED KF
- FrameBasePtr keyframe = buffer_frame_.select( incoming_capture_->getTimeStamp(), params_gnss_->time_tolerance);
+ FrameBasePtr keyframe = buffer_frame_.select(incoming_capture_->getTimeStamp(), params_gnss_->time_tolerance);
if (keyframe and last_KF_capture_ and keyframe == last_KF_capture_->getFrame())
keyframe = nullptr;
if (keyframe)
@@ -108,70 +197,82 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
new_frame = getProblem()->emplaceFrame(incoming_capture_->getTimeStamp());
KF_created = true;
}
- // OTHERWISE return
+ // OTHERWISE store capture
else
+ {
+ buffer_capture_.emplace(_capture->getTimeStamp(), _capture);
return;
+ }
- // ESTABLISH FACTOR
// link capture
incoming_capture_->link(new_frame);
+
// emplace factor
- new_fac = emplaceFactor(incoming_feature);
+ new_fac = emplaceFactor(incoming_feature_);
// outlier rejection (only can be evaluated if ENU defined and ENU-MAP well initialized: fixed)
- if (params_gnss_->remove_outliers and
- sensor_gnss_->isEnuDefined() and
- sensor_gnss_->isEnuMapFixed() and
- detectOutlier(new_fac))
+ if (params_gnss_->remove_outliers and detectOutlier(new_fac))
{
- new_frame->remove();
+ if (KF_created)
+ new_frame->remove();
+ else
+ incoming_feature_->remove();
return;
}
+ // Handle ENU definition, ENU-MAP initialization etc.
+ handleEnuMap(incoming_feature_);
+
// store last KF
last_KF_capture_ = incoming_capture_;
- last_KF_feature_ = incoming_feature;
+ last_KF_feature_ = incoming_feature_;
+
+ // Notify if KF created
+ if (KF_created)
+ getProblem()->keyFrameCallback(new_frame, shared_from_this());
+}
+void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature)
+{
// Define ENU (if not defined)
if (!sensor_gnss_->isEnuDefined())
{
- WOLF_DEBUG("Defining ecef enu");
- sensor_gnss_->setEcefEnu(incoming_feature->getMeasurement());
+ WOLF_INFO("Defining ecef enu with first fix");
+ sensor_gnss_->setEcefEnu(feature->getMeasurement());
}
- // Store the first capture that established a factor (for initializing ENU-MAP)
+ // Store the first capture that established a factor (for later initialization ENU-MAP)
if (first_frame_state_.empty() and
not sensor_gnss_->isEnuMapFixed())
{
- first_frame_state_ = incoming_capture_->getFrame()->getState();
- first_pos_ = incoming_feature->getMeasurement();
+ first_frame_state_ = feature->getCapture()->getFrame()->getState();
+ first_pos_ = feature->getMeasurement();
}
+
// Initialize ENU-MAP if: ENU defined and ENU-MAP not initialized (and not fixed) and far enough
if (params_gnss_->init_enu_map and
not first_frame_state_.empty() and
sensor_gnss_->isEnuDefined() and
not sensor_gnss_->isEnuMapInitialized() and
not sensor_gnss_->isEnuMapFixed() and
- (first_pos_-incoming_pos_out_.pos).norm() > params_gnss_->enu_map_init_dist_min)
+ (first_pos_-feature->getMeasurement()).norm() > params_gnss_->enu_map_init_dist_min)
{
- assert(first_frame_state_.count('P') and first_frame_state_.count('O') and incoming_capture_->getFrame() != nullptr);
+ assert(first_frame_state_.count('P') and
+ first_frame_state_.count('O') and
+ feature->getCapture()->getFrame() != nullptr);
sensor_gnss_->initializeEnuMap(first_frame_state_.vector("PO"),
first_pos_,
- incoming_capture_->getFrame()->getState().vector("PO"),
- incoming_feature->getMeasurement());
+ feature->getCapture()->getFrame()->getState().vector("PO"),
+ feature->getMeasurement());
// Set as not-initialized if factors not separated enough ( < enu_map_init_dist_max)
- if ((first_pos_ - incoming_feature->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_max)
+ if ((first_pos_ - feature->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_max)
sensor_gnss_->setEnuMapInitialized(false);
}
// Fix ENU-MAP if separated enough from first pos ( > enu_map_fix_dist )
- if ((first_pos_ - incoming_feature->getMeasurement()).norm() > params_gnss_->enu_map_fix_dist)
+ if ((first_pos_ - feature->getMeasurement()).norm() > params_gnss_->enu_map_fix_dist)
sensor_gnss_->fixEnuMap();
-
- // Notify if KF created
- if (KF_created)
- getProblem()->keyFrameCallback(new_frame, shared_from_this());
}
FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr _ftr)
@@ -216,8 +317,8 @@ bool ProcessorGnssFix::voteForKeyFrame() const
sensor_gnss_->isEnuDefined() and
not sensor_gnss_->isEnuMapInitialized() and
not sensor_gnss_->isEnuMapFixed() and
- (first_pos_-incoming_pos_out_.pos).norm() > params_gnss_->enu_map_init_dist_min and
- (first_pos_-incoming_pos_out_.pos).norm() < params_gnss_->enu_map_init_dist_max)
+ (first_pos_-incoming_feature_->getMeasurement()).norm() > params_gnss_->enu_map_init_dist_min and
+ (first_pos_-incoming_feature_->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_max)
{
WOLF_DEBUG("KF because of enu map not initialized");
return true;
@@ -225,9 +326,9 @@ bool ProcessorGnssFix::voteForKeyFrame() const
// Distance criterion (ENU defined and ENU-MAP initialized)
if (last_KF_capture_ != nullptr and
- (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm() > params_gnss_->dist_traveled)
+ (incoming_feature_->getMeasurement() - last_KF_feature_->getMeasurement()).norm() > params_gnss_->dist_traveled)
{
- WOLF_DEBUG("KF because of distance criterion: ", (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm());
+ WOLF_DEBUG("KF because of distance criterion: ", (incoming_feature_->getMeasurement() - last_KF_feature_->getMeasurement()).norm());
return true;
}
@@ -239,6 +340,9 @@ bool ProcessorGnssFix::detectOutlier(FactorBasePtr fac)
{
WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
+ if (not sensor_gnss_->isEnuDefined() or not sensor_gnss_->isEnuMapFixed())
+ return false;
+
// Cast feature
auto gnss_ftr = std::static_pointer_cast<FeatureGnssFix>(fac->getFeature());
@@ -273,14 +377,6 @@ bool ProcessorGnssFix::detectOutlier(FactorBasePtr fac)
return false;
}
-bool ProcessorGnssFix::storeKeyFrame(FrameBasePtr _frame_ptr)
-{
- return true;
-}
-bool ProcessorGnssFix::storeCapture(CaptureBasePtr _cap_ptr)
-{
- return false;
-}
void ProcessorGnssFix::configure(SensorBasePtr _sensor)
{
sensor_gnss_ = std::static_pointer_cast<SensorGnss>(_sensor);
diff --git a/test/gtest_factor_gnss_fix_2d.cpp b/test/gtest_factor_gnss_fix_2d.cpp
index a95688fd9c9c73b8456f3d81e2ebc036edf9eee9..a5a439cfc4d7ec232122da70552d75fecfc3a875 100644
--- a/test/gtest_factor_gnss_fix_2d.cpp
+++ b/test/gtest_factor_gnss_fix_2d.cpp
@@ -153,6 +153,8 @@ TEST(FactorGnssFix2dTest, configure_tree)
*/
TEST(FactorGnssFix2dTest, gnss_1_map_base_position)
{
+ problem_ptr->print();
+
// --------------------------- Reset states
resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);