diff --git a/CMakeLists.txt b/CMakeLists.txt
index e10a0be475926ef484487b5a00a700c1b0cb873f..04dae69fec0938d2cdf6d4eda39b1ae60001f611 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -165,7 +165,7 @@ SET(HDRS_LANDMARK
)
SET(HDRS_PROCESSOR
include/gnss/processor/processor_gnss_fix.h
- include/gnss/processor/processor_gnss_fix.h
+ include/gnss/processor/processor_gnss_single_diff.h
)
SET(HDRS_SENSOR
include/gnss/sensor/sensor_gnss.h
diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
index 467e52a14e36acd80c04b92293a39714ed5672ca..e6b3adfc815439ef9b92c36bc62510f2e54376b1 100644
--- a/include/gnss/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -4,6 +4,7 @@
// Wolf includes
#include "core/processor/processor_base.h"
#include "gnss/capture/capture_gnss_fix.h"
+#include "gnss/sensor/sensor_gnss.h"
// Std includes
@@ -47,7 +48,7 @@ class ProcessorGnssFix : public ProcessorBase
virtual bool voteForKeyFrame();
private:
- void emplaceFactor(FeatureBasePtr& ftr_ptr);
+ FactorBasePtr emplaceFactor(FeatureBasePtr& ftr_ptr);
bool rejectOutlier(FactorBasePtr ctr_ptr);
public:
@@ -57,4 +58,4 @@ class ProcessorGnssFix : public ProcessorBase
} // namespace wolf
-#endif //WOLF_PROCESSOR_GPS_H
+#endif //WOLF_PROCESSOR_GNSS_FIX_H
diff --git a/include/gnss/processor/processor_gnss_single_diff.h b/include/gnss/processor/processor_gnss_single_diff.h
index ef1cdeb507b73700b9dbfa3aa4354898440549b3..405caeb16d5541bfa12468b8ecdeaa2324c99335 100644
--- a/include/gnss/processor/processor_gnss_single_diff.h
+++ b/include/gnss/processor/processor_gnss_single_diff.h
@@ -4,6 +4,7 @@
// Wolf includes
#include "core/processor/processor_base.h"
#include "gnss/capture/capture_gnss_single_diff.h"
+#include "gnss/sensor/sensor_gnss.h"
// Std includes
@@ -27,6 +28,9 @@ class ProcessorGnssSingleDiff : public ProcessorBase
virtual void process(CaptureBasePtr _capture_ptr);
virtual bool voteForKeyFrame();
+ private:
+ FactorBasePtr emplaceFactor(FeatureBasePtr& ftr_ptr);
+
public:
static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr);
@@ -34,4 +38,4 @@ class ProcessorGnssSingleDiff : public ProcessorBase
} // namespace wolf
-#endif //WOLF_PROCESSOR_GPS_H
+#endif //WOLF_PROCESSOR_GNSS_SINGLE_DIFF_H
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 9bdec31711f5cc6231100f8753ed6f1cab2294a0..82a23ec2d4ed380fb095c93e9ad32d3338e3265b 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -54,25 +54,23 @@ void ProcessorGnssFix::process(CaptureBasePtr _capture_ptr)
// ESTABLISH CONSTRAINT
if (new_frame_ptr)
{
- // ADD CAPTURE
- new_frame_ptr->addCapture(_capture_ptr); // Add incoming Capture to the new Frame
+ // LINK CAPTURE
+ _capture_ptr->link(new_frame_ptr); // Add incoming Capture to the new Frame
- // EXTRACT AND ADD FEATURES
- //WOLF_DEBUG( "PR ", getName()," - adding the feature...");
- FeatureBasePtr ftr_ptr = FeatureBase::emplace<FeatureGnssFix>(last_capture_ptr_, last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance());
- // FeatureBasePtr ftr_ptr = last_capture_ptr_->addFeature(std::make_shared<FeatureGnssFix>(last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance()));
+ // EMPLACE FEATURES
+ //WOLF_DEBUG( "PR ", getName()," - emplacing the feature...");
+ auto ftr_ptr = FeatureBase::emplace<FeatureGnssFix>(last_capture_ptr_, last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance());
- // EMPLACE CONSTRAINT
- emplaceFactor(ftr_ptr);
- new_fac_created = true;
+ // EMPLACE FACTOR
+ auto new_fac = emplaceFactor(ftr_ptr);
// outlier rejection
if (sensor_gnss_ptr_->isEnuDefined() && sensor_gnss_ptr_->isEnuMapInitialized())
- if (rejectOutlier(ftr_ptr->getFactorList().front()))
- new_fac_created = false;
+ if (rejectOutlier(new_fac))
+ new_fac = nullptr;
// store last KF
- if (new_fac_created)
+ if (new_fac)
last_gnss_fix_KF_= new_frame_ptr;
//WOLF_DEBUG("Factor established");
@@ -104,21 +102,16 @@ void ProcessorGnssFix::process(CaptureBasePtr _capture_ptr)
first_capture_ptr_ = last_capture_ptr_;
}
-void ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr_ptr)
+FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr_ptr)
{
// CREATE CONSTRAINT --------------------
//WOLF_DEBUG("creating the factor...");
- FactorBasePtr new_fac_ptr = nullptr;
// 2D
if (getProblem()->getDim() == 2)
- new_fac_ptr = 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);
// 3D
else
- new_fac_ptr = FactorBase::emplace<FactorGnssFix3D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false);
-
- // ADD CONSTRAINT --------------------
- //WOLF_DEBUG("adding the factor...");
- new_fac_ptr->link(ftr_ptr);
+ return FactorBase::emplace<FactorGnssFix3D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false);
}
bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac_ptr)
@@ -166,15 +159,12 @@ bool ProcessorGnssFix::voteForKeyFrame()
void ProcessorGnssFix::configure(SensorBasePtr _sensor)
{
sensor_gnss_ptr_ = std::static_pointer_cast<SensorGnss>(_sensor);
- setSensor(_sensor);
};
ProcessorBasePtr ProcessorGnssFix::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
{
- // ProcessorGnssFixPtr prc_ptr = std::make_shared<ProcessorGnssFix>(std::static_pointer_cast<ProcessorParamsGnssFix>(_params), std::static_pointer_cast<SensorGnss>(sensor_ptr));
- ProcessorGnssFixPtr prc_ptr = std::static_pointer_cast<ProcessorGnssFix>(ProcessorBase::emplace<ProcessorGnssFix>(sensor_ptr,
- std::static_pointer_cast<ProcessorParamsGnssFix>(_params),
- std::static_pointer_cast<SensorGnss>(sensor_ptr)));
+ auto prc_ptr = std::make_shared<ProcessorGnssFix>(std::static_pointer_cast<ProcessorParamsGnssFix>(_params),
+ std::static_pointer_cast<SensorGnss>(sensor_ptr));
prc_ptr->setName(_unique_name);
return prc_ptr;
}
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index ebae2b5895cc34ced75ae6ec8b3fbd1df1713eaf..6fdecea33ff4fa873c545866ca9af038d4347210 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -43,34 +43,38 @@ void ProcessorGnssSingleDiff::process(CaptureBasePtr _capture_ptr)
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)
{
- // ADD CAPTURE
- new_frame_ptr->addCapture(_capture_ptr); // Add incoming Capture to the new Frame
+ // LINK CAPTURE
+ _capture_ptr->link(new_frame_ptr); // Add incoming Capture to the new Frame
+
+ // EMPLACE FEATURE
+ //WOLF_DEBUG("emplacing feature...");
+ auto ftr_ptr = FeatureBase::emplace<FeatureGnssSingleDiff>(last_capture_ptr_, last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance());
- // EXTRACT AND ADD FEATURES
- //WOLF_DEBUG("adding the feature...");
- // FeatureBasePtr ftr_ptr = last_capture_ptr_->addFeature(std::make_shared<FeatureGnssSingleDiff>(last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance()));
- FeatureBasePtr ftr_ptr = FeatureBase::emplace<FeatureGnssSingleDiff>(last_capture_ptr_, last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance());
// ADD CONSTRAINT
- FactorBasePtr fac_ptr;
- //WOLF_DEBUG("adding the factor...");
- // 2D
- if (getProblem()->getDim() == 2)
- fac_ptr = ftr_ptr->addFactor(std::make_shared<FactorGnssSingleDiff2D>(ftr_ptr, last_capture_ptr_->getOriginFrame(), sensor_gnss_ptr_, shared_from_this()));
- // 3D TODO
- else
- std::runtime_error("Single Differences in 3D not implemented yet.");
- // fac_ptr = ftr_ptr->addFactor(std::make_shared<FactorGnssSingleDiff3D>(ftr_ptr, sensor_gnss_ptr_, shared_from_this()));
-
- // add constrained-by to the origin
- last_capture_ptr_->getOriginFrame()->addConstrainedBy(fac_ptr);
+ //WOLF_DEBUG("emplacing factor...");
+ emplaceFactor(ftr_ptr);
//WOLF_DEBUG("Factor established");
}
}
+FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr_ptr)
+{
+ // CREATE CONSTRAINT --------------------
+ //WOLF_DEBUG("creating the factor...");
+ // 2D
+ if (getProblem()->getDim() == 2)
+ return FactorBase::emplace<FactorGnssSingleDiff2D>(ftr_ptr, ftr_ptr, last_capture_ptr_->getOriginFrame(), sensor_gnss_ptr_, shared_from_this());
+ // 3D TODO
+ else
+ std::runtime_error("Single Differences in 3D not implemented yet.");
+
+ return nullptr;
+}
+
bool ProcessorGnssSingleDiff::voteForKeyFrame()
{
// TODO
@@ -80,7 +84,6 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame()
void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor)
{
sensor_gnss_ptr_ = std::static_pointer_cast<SensorGnss>(_sensor);
- setSensor(_sensor);
};
ProcessorBasePtr ProcessorGnssSingleDiff::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
diff --git a/test/gtest_factor_gnss_fix_2D.cpp b/test/gtest_factor_gnss_fix_2D.cpp
index 4ed465585867d18f988295865f13dd730f8bc38d..bf783d30e17d1fcf0abab2f470ec87d4749d7640 100644
--- a/test/gtest_factor_gnss_fix_2D.cpp
+++ b/test/gtest_factor_gnss_fix_2D.cpp
@@ -118,7 +118,7 @@ TEST(FactorGnssFix2DTest, configure_tree)
// 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());
- CaptureGnssFixPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssFix>(CaptureBase::emplace<CaptureGnssFix>(frame_ptr, 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()));
gnss_sensor_ptr->process(cap_gnss_ptr);
// Checks
diff --git a/test/gtest_factor_gnss_single_diff_2D.cpp b/test/gtest_factor_gnss_single_diff_2D.cpp
index 901fda3030c1c860afb8f7abaf31fd75f547935c..2c70d8c0cd705aa0a70176e9fa6a2d92aefe4bd0 100644
--- a/test/gtest_factor_gnss_single_diff_2D.cpp
+++ b/test/gtest_factor_gnss_single_diff_2D.cpp
@@ -87,7 +87,7 @@ class FactorGnssSingleDiff2DTest : public testing::Test
odom_params_ptr->max_time_span = 1.0;
odom_params_ptr->time_tolerance = 1.0;
problem_ptr->installProcessor("ODOM 2D", "main odometry", odom_sensor_ptr, odom_params_ptr);
- problem_ptr->setProcessorMotion("main odometry");
+ //problem_ptr->setProcessorMotion("main odometry");
// set prior (FIXED)
Vector3s frame1state = t_map_base1;
@@ -132,8 +132,7 @@ TEST_F(FactorGnssSingleDiff2DTest, check_tree)
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position)
{
// Create GNSS Fix capture
- // CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(1), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssSingleDiff>(CaptureBase::emplace<CaptureGnssSingleDiff>(prior_frame_ptr, TimeStamp(1), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr));
+ CaptureGnssSingleDiffPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssSingleDiff>(CaptureBase::emplace<CaptureGnssSingleDiff>(nullptr, TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr));
gnss_sensor_ptr->process(cap_gnss_ptr);
@@ -168,8 +167,7 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_position)
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_orientation)
{
// Create GNSS Fix capture
- // CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(1), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssSingleDiff>(CaptureBase::emplace<CaptureGnssSingleDiff>(prior_frame_ptr, TimeStamp(1), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr));
+ CaptureGnssSingleDiffPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssSingleDiff>(CaptureBase::emplace<CaptureGnssSingleDiff>(nullptr, TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr));
gnss_sensor_ptr->process(cap_gnss_ptr);
// fixing things
@@ -199,8 +197,7 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_map_base_orientation)
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_enu_map_yaw)
{
// Create GNSS Fix capture
- // CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(1), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssSingleDiff>(CaptureBase::emplace<CaptureGnssSingleDiff>(prior_frame_ptr, TimeStamp(1), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr));
+ CaptureGnssSingleDiffPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssSingleDiff>(CaptureBase::emplace<CaptureGnssSingleDiff>(nullptr, TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr));
gnss_sensor_ptr->process(cap_gnss_ptr);
// unfixing things
@@ -232,8 +229,7 @@ TEST_F(FactorGnssSingleDiff2DTest, gnss_1_enu_map_yaw)
TEST_F(FactorGnssSingleDiff2DTest, gnss_1_base_antena)
{
// Create GNSS Fix capture
- // CaptureGnssSingleDiffPtr cap_gnss_ptr = std::make_shared<CaptureGnssSingleDiff>(TimeStamp(1), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr);
- CaptureGnssSingleDiffPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssSingleDiff>(CaptureBase::emplace<CaptureGnssSingleDiff>(prior_frame_ptr, TimeStamp(1), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr));
+ CaptureGnssSingleDiffPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssSingleDiff>(CaptureBase::emplace<CaptureGnssSingleDiff>(nullptr, TimeStamp(0), gnss_sensor_ptr, t_ecef_antena2-t_ecef_antena1, 1e-6*Matrix3s::Identity(), prior_frame_ptr));
gnss_sensor_ptr->process(cap_gnss_ptr);
// unfixing things