Merge branch '4-adapt-to-new-emplace-api' into 'devel'

Resolve "Adapt to new emplace() API"

Closes #4

See merge request !4

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

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

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

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

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)

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

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