Merge branch 'devel' into feature/plugins

CMakeLists.txt

@@ -98,7 +98,7 @@ ENDIF()
 
 option(_WOLF_TRACE "Enable wolf tracing macro" ON)
 
-option(BUILD_EXAMPLES "Build examples" OFF)
+# option(BUILD_EXAMPLES "Build examples" OFF)
 set(BUILD_TESTS true)
 
 # Does this has any other interest
@@ -761,6 +761,13 @@ IF (GLOG_FOUND)
     ENDIF(BUILD_TESTS)
 ENDIF (GLOG_FOUND)
 
+IF(BUILD_EXAMPLES)
+  #Build examples
+  MESSAGE("Building examples.")
+  ADD_SUBDIRECTORY(src/examples)
+ENDIF(BUILD_EXAMPLES)
+
+
 #install library
 
 #=============================================================

include/base/constraint/constraint_GPS_pseudorange_2D.h

@@ -27,11 +27,15 @@ WOLF_PTR_TYPEDEFS(ConstraintGPSPseudorange2D);
 class ConstraintGPSPseudorange2D : public ConstraintAutodiff<ConstraintGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>
 {
     public:
-        ConstraintGPSPseudorange2D(const FeatureBasePtr& _ftr_ptr,
+        ConstraintGPSPseudorange2D(const FeatureBasePtr& _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
                                    bool _apply_loss_function = false, 
                                    ConstraintStatus _status = CTR_ACTIVE) :
            ConstraintAutodiff<ConstraintGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>("GPS PR 2D",
                                                                                nullptr,
+                                                                               nullptr,
+                                                                               nullptr,
+                                                                               nullptr,
+																			   _pr_ptr,
                                                                                _apply_loss_function,
                                                                                _status,
                                                                                _ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to the initial pos frame

include/base/constraint/constraint_GPS_pseudorange_3D.h

@@ -26,11 +26,11 @@ class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudor
 
     public:
 
-        ConstraintGPSPseudorange3D(FeatureBasePtr _ftr_ptr, 
+        ConstraintGPSPseudorange3D(FeatureBasePtr _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
                                    bool _apply_loss_function = false, 
                                    ConstraintStatus _status = CTR_ACTIVE) :
              ConstraintAutodiff<ConstraintGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>("GPS PR 3D",
-                                                                                 nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status,
+                                                                                 nullptr, nullptr, nullptr, nullptr, _pr_ptr, _apply_loss_function, _status,
                             _ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to map frame
                             _ftr_ptr->getFramePtr()->getOPtr(), // orientation of the vehicle's frame wrt map frame
                             _ftr_ptr->getCapturePtr()->getSensorPPtr(), // position of the sensor (gps antenna) with respect to base frame

src/examples/test_diff_drive.cpp

@@ -10,7 +10,7 @@
 #include "base/problem.h"
 #include "base/sensor/sensor_diff_drive.h"
 #include "base/capture/capture_wheel_joint_position.h"
-#include "processor/processor_diff_drive.h"
+#include "base/processor/processor_diff_drive.h"
 
 //std
 #include <iostream>

src/processor/processor_GPS.cpp

@@ -44,7 +44,7 @@ void ProcessorGPS::process(CaptureBasePtr _capture_ptr)
     for (auto i_it = capture_gps_ptr_->getFeatureList().begin();
             i_it != capture_gps_ptr_->getFeatureList().end(); i_it++)
     {
-        capture_gps_ptr_->getFeatureList().front()->addConstraint(std::make_shared<ConstraintGPSPseudorange2D>((*i_it)));
+        capture_gps_ptr_->getFeatureList().front()->addConstraint(std::make_shared<ConstraintGPSPseudorange2D>((*i_it), shared_from_this()));
     }
     //std::cout << "Constraints established" << std::endl;
 }
@@ -69,7 +69,7 @@ ProcessorBasePtr ProcessorGPS::create(const std::string& _unique_name, const Pro
 } // namespace wolf
 
 // Register in the SensorFactory
-#include "processor_factory.h"
+#include "base/processor/processor_factory.h"
 namespace wolf {
 WOLF_REGISTER_PROCESSOR("GPS",ProcessorGPS)
 } // namespace wolf