diff --git a/include/gnss/factor/factor_gnss_fix_2D.h b/include/gnss/factor/factor_gnss_fix_2D.h
index 5b5e6997336abb6ae54f6d81b1fa11399420d395..7e8a2497735d4ae9643a420d3353abd00811eedb 100644
--- a/include/gnss/factor/factor_gnss_fix_2D.h
+++ b/include/gnss/factor/factor_gnss_fix_2D.h
@@ -19,7 +19,7 @@ class FactorGnssFix2D : public FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1,
public:
- FactorGnssFix2D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorGnssFix2D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1, 1, 1>("FactorGnssFix2D",
nullptr,
nullptr,
@@ -30,7 +30,7 @@ class FactorGnssFix2D : public FactorAutodiff<FactorGnssFix2D, 3, 2, 1, 3, 3, 1,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
- _sensor_gnss_ptr->getStateBlock(0),
+ _sensor_gnss_ptr->getP(),
_sensor_gnss_ptr->getEnuMapTranslation(),
_sensor_gnss_ptr->getEnuMapRoll(),
_sensor_gnss_ptr->getEnuMapPitch(),
diff --git a/include/gnss/factor/factor_gnss_fix_3D.h b/include/gnss/factor/factor_gnss_fix_3D.h
index 7182831792ee17803e2be80de487938a6557015f..4041ca790a8b592c854b9e9d93f9007995dbdbd3 100644
--- a/include/gnss/factor/factor_gnss_fix_3D.h
+++ b/include/gnss/factor/factor_gnss_fix_3D.h
@@ -18,7 +18,7 @@ class FactorGnssFix3D : public FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1,
public:
- FactorGnssFix3D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorGnssFix3D(FeatureBasePtr& _ftr_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1, 1, 1>("FactorGnssFix3D",
nullptr,
nullptr,
@@ -29,7 +29,7 @@ class FactorGnssFix3D : public FactorAutodiff<FactorGnssFix3D, 3, 3, 4, 3, 3, 1,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
- _sensor_gnss_ptr->getStateBlock(0),
+ _sensor_gnss_ptr->getP(),
_sensor_gnss_ptr->getEnuMapTranslation(),
_sensor_gnss_ptr->getEnuMapRoll(),
_sensor_gnss_ptr->getEnuMapPitch(),
diff --git a/include/gnss/factor/factor_gnss_single_diff_2D.h b/include/gnss/factor/factor_gnss_single_diff_2D.h
index 8f1ca7865f0c73782f01d9fe34eb9a2f4794be9c..e8f139c430fa689d451bc0189a147d3b83a1c309 100644
--- a/include/gnss/factor/factor_gnss_single_diff_2D.h
+++ b/include/gnss/factor/factor_gnss_single_diff_2D.h
@@ -19,7 +19,7 @@ class FactorGnssSingleDiff2D : public FactorAutodiff<FactorGnssSingleDiff2D, 3,
public:
- FactorGnssSingleDiff2D(FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorGnssSingleDiff2D(FeatureBasePtr& _ftr_ptr, const FrameBasePtr& _frame_other_ptr, const SensorGnssPtr& _sensor_gnss_ptr, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorGnssSingleDiff2D, 3, 2, 1, 2, 1, 3, 1, 1, 1>("GNSS SINGLE DIFFERENCES 2D",
_frame_other_ptr,
nullptr,
@@ -32,7 +32,7 @@ class FactorGnssSingleDiff2D : public FactorAutodiff<FactorGnssSingleDiff2D, 3,
_frame_other_ptr->getO(),
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
- _sensor_gnss_ptr->getStateBlock(0),
+ _sensor_gnss_ptr->getP(),
_sensor_gnss_ptr->getEnuMapRoll(),
_sensor_gnss_ptr->getEnuMapPitch(),
_sensor_gnss_ptr->getEnuMapYaw()),
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 9ca733fd66680e10cdbc1f68af4f639c132f45a7..6ae3ebdebecae4cfdc1ce098c074dae0c102afbb 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -53,17 +53,22 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
// EMPLACE FEATURES
WOLF_DEBUG( "PR ", getName()," - emplacing the feature...");
- auto ftr = FeatureBase::emplace<FeatureGnssFix>(incoming_capture_, incoming_capture_->getData(),incoming_capture_->getDataCovariance());
+ auto ftr = FeatureBase::emplace<FeatureGnssFix>(incoming_capture_,
+ incoming_capture_->getData(),
+ incoming_capture_->getDataCovariance());
// EMPLACE FACTOR
+ WOLF_DEBUG( "PR ", getName()," - emplacing the factor...");
new_fac = emplaceFactor(ftr);
// outlier rejection
+ WOLF_DEBUG( "PR ", getName()," - outlier rejection...");
if (sensor_gnss_->isEnuDefined() && sensor_gnss_->isEnuMapInitialized())
if (rejectOutlier(new_fac))
new_fac = nullptr;
// store last KF
+ WOLF_DEBUG("PR ", getName()," - store KF...");
if (new_fac)
last_KF_= new_frame;
}
@@ -95,6 +100,7 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
sensor_gnss_->initializeEnuMap(first_capture_->getFrame()->getState(), first_capture_->getData(),
incoming_capture_->getFrame()->getState(), incoming_capture_->getData());
}
+ WOLF_DEBUG("process ended");
}
FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr _ftr)
@@ -103,20 +109,21 @@ FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr _ftr)
//WOLF_DEBUG("creating the factor...");
// 2D
if (getProblem()->getDim() == 2)
- return FactorBase::emplace<FactorGnssFix2D>(_ftr, _ftr, sensor_gnss_, shared_from_this(), false, FAC_ACTIVE);
+ return FactorBase::emplace<FactorGnssFix2D>(_ftr, _ftr, sensor_gnss_, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
// 3D
else
- return FactorBase::emplace<FactorGnssFix3D>(_ftr, _ftr, sensor_gnss_, shared_from_this(), false, FAC_ACTIVE);
+ return FactorBase::emplace<FactorGnssFix3D>(_ftr, _ftr, sensor_gnss_, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
}
bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac)
{
+ WOLF_DEBUG("ProcessorGnssFix::rejectOutlier");
// Cast feature
auto gnss_ftr = std::static_pointer_cast<FeatureGnssFix>(fac->getFeature());
// copy states
Eigen::VectorXd x(gnss_ftr->getCapture()->getFrame()->getP()->getState());
Eigen::VectorXd o(gnss_ftr->getCapture()->getFrame()->getP()->getState());
- Eigen::VectorXd x_antena(sensor_gnss_->getStateBlock(0)->getState());
+ Eigen::VectorXd x_antena(sensor_gnss_->getP()->getState());
Eigen::VectorXd t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
Eigen::VectorXd roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
Eigen::VectorXd pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index a4e549352c78f149da0ca8b5ec455d043bc608e9..96fdddde675f6f1685dc7e9b001f5a4a973886fd 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -92,7 +92,7 @@ FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr _ftr)
//WOLF_DEBUG("creating the factor...");
// 2D
if (getProblem()->getDim() == 2)
- return FactorBase::emplace<FactorGnssSingleDiff2D>(_ftr, _ftr, incoming_capture_->getOriginFrame(), sensor_gnss_, shared_from_this());
+ return FactorBase::emplace<FactorGnssSingleDiff2D>(_ftr, _ftr, incoming_capture_->getOriginFrame(), sensor_gnss_, shared_from_this(), params_->apply_loss_function);
// 3D TODO
else
std::runtime_error("Single Differences in 3D not implemented yet.");
@@ -159,5 +159,5 @@ ProcessorBasePtr ProcessorGnssSingleDiff::create(const std::string& _unique_name
// Register in the SensorFactory
#include "core/processor/processor_factory.h"
namespace wolf {
-WOLF_REGISTER_PROCESSOR("GNSS SINGLE DIFFERENCES",ProcessorGnssSingleDiff)
+WOLF_REGISTER_PROCESSOR("ProcessorGnssSingleDiff",ProcessorGnssSingleDiff)
} // namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 726d98fd473241aae6f00b78e2805d252e37a178..4686b740fca68fc811028129737b3640fb9eab6d 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -8,8 +8,8 @@ include_directories(${GTEST_INCLUDE_DIRS})
############# USE THIS TEST AS AN EXAMPLE #################
# #
# Create a specific test executable for gtest_example #
-wolf_add_gtest(gtest_example gtest_example.cpp) #
-target_link_libraries(gtest_example ${PLUGIN_NAME}) #
+#wolf_add_gtest(gtest_example gtest_example.cpp) #
+#target_link_libraries(gtest_example ${PLUGIN_NAME}) #
# #
###########################################################
diff --git a/test/gtest_factor_gnss_fix_2D.cpp b/test/gtest_factor_gnss_fix_2D.cpp
index 5f4e38d6bcc27f640cd223295f97260ac14fd3cb..ade863719d1bf21a816a0e5e4caa9baf95312390 100644
--- a/test/gtest_factor_gnss_fix_2D.cpp
+++ b/test/gtest_factor_gnss_fix_2D.cpp
@@ -5,7 +5,6 @@
* \author: jvallve
*/
-
#include "gnss/factor/factor_gnss_fix_2D.h"
#include <core/utils/utils_gtest.h>
@@ -15,7 +14,6 @@
#include "core/ceres_wrapper/ceres_manager.h"
-
using namespace Eigen;
using namespace wolf;
@@ -109,6 +107,7 @@ TEST(FactorGnssFix2DTest, configure_tree)
ProcessorParamsGnssFixPtr gnss_params_ptr = std::make_shared<ProcessorParamsGnssFix>();
gnss_params_ptr->time_tolerance = 1.0;
gnss_params_ptr->voting_active = true;
+ gnss_params_ptr->apply_loss_function = false;
problem_ptr->installProcessor("ProcessorGnssFix", "gnss fix", gnss_sensor_ptr, gnss_params_ptr);
// Emplace a frame (FIXED)
@@ -121,7 +120,7 @@ TEST(FactorGnssFix2DTest, configure_tree)
gnss_sensor_ptr->process(cap_gnss_ptr);
// Checks
- ASSERT_TRUE(problem_ptr->check(0));
+ ASSERT_TRUE(problem_ptr->check(1));
ASSERT_TRUE(frame_ptr->isKey());
}
diff --git a/test/gtest_factor_gnss_single_diff_2D.cpp b/test/gtest_factor_gnss_single_diff_2D.cpp
index 7fea36864c613bb64a45db1c0f6608b91907ff52..9dadf7e94b0ead2a37260be1df9da87320c31ee3 100644
--- a/test/gtest_factor_gnss_single_diff_2D.cpp
+++ b/test/gtest_factor_gnss_single_diff_2D.cpp
@@ -77,7 +77,7 @@ class FactorGnssSingleDiff2DTest : public testing::Test
gnss_params_ptr->time_th = 0;
gnss_params_ptr->dist_traveled = 0;
gnss_params_ptr->enu_map_init_dist_min = 0;
- problem_ptr->installProcessor("GNSS SINGLE DIFFERENCES", "gnss single difference", gnss_sensor_ptr, gnss_params_ptr);
+ problem_ptr->installProcessor("ProcessorGnssSingleDiff", "gnss single difference", gnss_sensor_ptr, gnss_params_ptr);
// odom sensor & processor
IntrinsicsOdom2DPtr odom_intrinsics_ptr = std::make_shared<IntrinsicsOdom2D>();