Merge branch '11-loss-function-api' into 'devel'

Resolve "Loss function API"

Closes #11

See merge request !14

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(),

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(),

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()),

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());

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

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})      #
 #                                                         #
 ###########################################################
 

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());
 }
 

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>();