implementing processors and factor

CMakeLists.txt

@@ -229,6 +229,7 @@ SET(HDRS_FACTOR
   include/core/factor/factor_relative_pose_2d_with_extrinsics.h
   include/core/factor/factor_relative_pose_3d.h
   include/core/factor/factor_pose_3d_with_extrinsics.h
+  include/core/factor/factor_velocity_direction.h
   )
 SET(HDRS_FEATURE
   include/core/feature/feature_base.h
@@ -245,7 +246,9 @@ SET(HDRS_PROCESSOR
   include/core/processor/is_motion.h
   include/core/processor/motion_buffer.h
   include/core/processor/processor_base.h
+  include/core/processor/processor_constant_velocity.h
   include/core/processor/processor_diff_drive.h
+  include/core/processor/processor_fix_wing_model.h
   include/core/processor/factory_processor.h
   include/core/processor/processor_logging.h
   include/core/processor/processor_loop_closure.h
@@ -344,7 +347,9 @@ SET(SRCS_PROCESSOR
   src/processor/is_motion.cpp
   src/processor/motion_buffer.cpp
   src/processor/processor_base.cpp
+  src/processor/processor_constant_velocity.cpp
   src/processor/processor_diff_drive.cpp
+  src/processor/processor_fix_wing_model.cpp
   src/processor/processor_loop_closure.cpp
   src/processor/processor_motion.cpp
   src/processor/processor_odom_2d.cpp

include/core/factor/factor_velocity_direction_3d.h

+
+#ifndef FACTOR_VELOCITY_DIRECTION_3D_H_
+#define FACTOR_VELOCITY_DIRECTION_3D_H_
+
+//Wolf includes
+#include "core/factor/factor_autodiff.h"
+#include "core/frame/frame_base.h"
+#include "core/math/rotations.h"
+
+//#include "ceres/jet.h"
+
+namespace wolf {
+    
+WOLF_PTR_TYPEDEFS(FactorVelocityDirection3d);
+
+//class
+class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d,1,3,4>
+{
+    public:
+        FactorVelocityDirection3d(FeatureBasePtr _ftr_ptr,
+                                  const ProcessorBasePtr& _processor_ptr,
+                                  bool _apply_loss_function,
+                                  FactorStatus _status = FAC_ACTIVE) :
+                FactorAutodiff<FactorVelocityDirection3d,1,3,4>("FactorVelocityDirection3d",
+                                                                TOP_ABS,
+                                                                _ftr_ptr,
+                                                                nullptr, nullptr, nullptr, nullptr,
+                                                                _processor_ptr,
+                                                                _apply_loss_function,
+                                                                _status,
+                                                                _ftr_ptr->getFrame()->getV(),
+                                                                _ftr_ptr->getFrame()->getO())
+        {
+//            std::cout << "created FactorVelocityDirection3d " << std::endl;
+        }
+
+        ~FactorVelocityDirection3d() override = default;
+
+        template<typename T>
+        bool operator ()(const T* const _v, const T* const _o, T* _residuals) const;
+};
+
+template<typename T>
+inline bool FactorVelocityDirection3d::operator ()(const T* const _v, const T* const _q, T* _residuals) const
+{
+    Eigen::Map<const Eigen::Matrix<T,3,1> > v(_v);
+    Eigen::Map<const Eigen::Quaternion<T> > q(_q);
+
+    //     std::cout << "v: " << v(0) << " "
+    //                        << v(1) << " "
+    //                        << v(2) << "\n";
+    //     std::cout << "q: " << q.coeffs()(0) << " "
+    //                        << q.coeffs()(1) << " "
+    //                        << q.coeffs()(2) << " "
+    //                        << q.coeffs()(3) << "\n";
+
+    // velocity in local coordinates
+    Eigen::Matrix<T,3,1> v_local = q.conjugate() * v;
+
+    //     std::cout << "v_local: " << v_local(0) << " "
+    //                              << v_local(1) << " "
+    //                              << v_local(2) << "\n";
+
+    // error (angle between measurement and velocity in local coordinates)
+    double error = acos( v_local.dot(getMeasurement().cast<T>()) / (v_local.norm() * getMeasurement().cast<T>().norm()));
+
+    // residual
+    _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * error;
+
+    return true;
+}
+
+} // namespace wolf
+
+#endif

include/core/processor/processor_constant_velocity.h

+/*
+ * processor_constant_velocity.h
+ *
+ *  Created on: Sep 6, 2021
+ *      Author: joanvallve
+ */
+
+#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_CONSTANT_VELOCITY_H_
+#define INCLUDE_CORE_PROCESSOR_PROCESSOR_CONSTANT_VELOCITY_H_
+
+#include "core/processor/processor_base.h"
+
+namespace wolf
+{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorConstantVelocity);
+
+struct ParamsProcessorConstantVelocity : public ParamsProcessorBase
+{
+        ParamsProcessorConstantVelocity() = default;
+        ParamsProcessorConstantVelocity(std::string _unique_name, const wolf::ParamsServer & _server) :
+            ParamsProcessorBase(_unique_name, _server)
+        {
+        }
+        std::string print() const override
+        {
+            return ParamsProcessorBase::print();
+        }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorConstantVelocity);
+
+class ProcessorConstantVelocity : public ProcessorBase
+{
+    protected:
+        ParamsProcessorConstantVelocityPtr params_processor_;
+        FrameBasePtr last_keyframe_;
+
+    public:
+        ProcessorConstantVelocity(ParamsProcessorConstantVelocityPtr _params);
+
+        // Factory method for high level API
+        WOLF_PROCESSOR_CREATE(ProcessorConstantVelocity, ParamsProcessorConstantVelocity);
+
+        virtual ~ProcessorConstantVelocity() override;
+        void configure(SensorBasePtr _sensor) override;
+
+    protected:
+
+        /** \brief process an incoming capture NEVER CALLED
+         */
+        virtual void processCapture(CaptureBasePtr) override {};
+
+        /** \brief process an incoming key-frame: applies the motion model between consecutive keyframes
+         */
+        virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
+
+        /** \brief trigger in capture
+         */
+        virtual bool triggerInCapture(CaptureBasePtr) const override {return false;};
+
+        /** \brief trigger in key-frame
+         */
+        virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override {return true;};
+
+        /** \brief store key frame
+        */
+        virtual bool storeKeyFrame(FrameBasePtr) override {return false;};
+
+        /** \brief store capture
+        */
+        virtual bool storeCapture(CaptureBasePtr) override {return false;};
+
+        /** \brief Vote for KeyFrame generation
+         */
+        virtual bool voteForKeyFrame() const override {return false;};
+};
+
+} /* namespace wolf */
+
+#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_CONSTANT_VELOCITY_H_ */

include/core/processor/processor_frame_factor.h → include/core/processor/processor_fix_wing_model.h

 /*
- * processor_frame_factor.h
+ * processor_fix_wing_model.h
  *
  *  Created on: Sep 6, 2021
  *      Author: joanvallve
  */
 
-#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_FRAME_FACTOR_H_
-#define INCLUDE_CORE_PROCESSOR_PROCESSOR_FRAME_FACTOR_H_
+#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
+#define INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
 
 #include "core/processor/processor_base.h"
 
 namespace wolf
 {
 
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFrameFactor);
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixWingModel);
 
-struct ParamsProcessorFrameFactor : public ParamsProcessorBase
+struct ParamsProcessorFixWingModel : public ParamsProcessorBase
 {
-        ParamsProcessorFrameFactor() = default;
-        ParamsProcessorFrameFactor(std::string _unique_name, const wolf::ParamsServer & _server) :
+        ParamsProcessorFixWingModel() = default;
+        ParamsProcessorFixWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
             ParamsProcessorBase(_unique_name, _server)
         {
         }
@@ -28,17 +28,17 @@ struct ParamsProcessorFrameFactor : public ParamsProcessorBase
         }
 };
 
-WOLF_PTR_TYPEDEFS(ProcessorFrameFactor);
+WOLF_PTR_TYPEDEFS(ProcessorFixWingModel);
 
-class ProcessorFrameFactor : public ProcessorBase
+class ProcessorFixWingModel : public ProcessorBase
 {
     public:
-        ProcessorFrameFactor(ParamsProcessorMotionModelPtr);
+        ProcessorFixWingModel(ParamsProcessorMotionModelPtr);
 
         // Factory method for high level API
-        WOLF_PROCESSOR_CREATE(ProcessorFrameFactor, ParamsProcessorFrameFactor);
+        WOLF_PROCESSOR_CREATE(ProcessorFixWingModel, ParamsProcessorFixWingModel);
 
-        virtual ~ProcessorFrameFactor() override;
+        virtual ~ProcessorFixWingModel() override;
         void configure(SensorBasePtr _sensor) override {};
 
     protected:
@@ -72,9 +72,9 @@ class ProcessorFrameFactor : public ProcessorBase
         virtual bool voteForKeyFrame() const override {return false;};
 
         // ATTRIBUTES
-        ParamsProcessorFrameFactorPtr params_processor_;
+        ParamsProcessorFixWingModelPtr params_processor_;
 };
 
 } /* namespace wolf */
 
-#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_FRAME_FACTOR_H_ */
+#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_ */

src/processor/processor_constant_velocity.cpp

+/*
+ * processor_constant_velocity.cpp
+ *
+ *  Created on: Sep 6, 2021
+ *      Author: joanvallve
+ */
+
+#include "core/processor/processor_constant_velocity.h"
+
+#include "core/capture/capture_void.h"
+#include "core/feature/feature_base.h"
+#include "core/factor/factor_block_difference.h"
+
+namespace wolf
+{
+
+ProcessorConstantVelocity::ProcessorConstantVelocity(ParamsProcessorConstantVelocityPtr _params) :
+        ProcessorBase("ProcessorConstantVelocity", 0, _params),
+        params_processor_(_params),
+        last_keyframe_(nullptr)
+{
+    assert(params_processor_->cov_rate.rows() == getProblem()->getDim() && "covariance size is wrong");
+}
+
+ProcessorConstantVelocity::~ProcessorConstantVelocity()
+{
+}
+
+void ProcessorConstantVelocity::configure(SensorBasePtr _sensor)
+{
+    assert(getProblem()->getFrameStructure().find('V') != std::string::npos) && "Processor only works with problems with V");
+}
+
+void ProcessorConstantVelocity::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+{
+    // TODO: handle if _keyframe_ptr is not the newest one
+
+    // only if not null previous keyframe
+    if (last_keyframe_ and
+        _keyframe_ptr->getTimeStamp() > last_keyframe_->getTimeStamp())
+    {
+        // emplace capture
+        auto cap = CaptureBase::emplace<CapureVoid>(_keyframe_ptr,
+                                                    _keyframe_ptr->getTimeStamp(),
+                                                    getSensor());
+
+        double dt = _keyframe_ptr->getTimeStamp() - last_keyframe_->getTimeStamp();
+
+        // STATE BLOCK V
+        // emplace feature
+        auto fea_v = FeatureBase::emplace<FeatureBase>(cap,
+                                                       "FeatureBase",
+                                                       Eigen::VectorXd::Zero(getProblem()->getDim()),
+                                                       params_processor_->cov_v_rate * dt);
+
+        // emplace factor
+        auto fac_v = FactorBase::emplace<FactorBlockDifference>(fea_v,
+                                                                fea_v,
+                                                                last_keyframe_->getV(),
+                                                                _keyframe_ptr->getV(),
+                                                                last_keyframe_, nullptr, nullptr, nullptr,
+                                                                0, -1, 0, -1,
+                                                                shared_from_this());
+    
+        /*// STATE BLOCK P
+        // emplace feature
+        auto fea_p = FeatureBase::emplace<FeatureBase>(cap,
+                                                     "FeatureBase",
+                                                     Eigen::VectorXd::Zero(getProblem()->getDim()),
+                                                     params_processor_->cov_p_rate * dt);
+
+        // emplace factor ct vel TODO*/
+    }
+    
+    // store keyframes
+    last_keyframe_ = _keyframe_ptr;
+}
+
+} /* namespace wolf */
+
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorConstantVelocity);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorConstantVelocity);
+} // namespace wolf
+

src/processor/processor_fix_wing_model.cpp

+/*
+ * processor_fix_wing_model.cpp
+ *
+ *  Created on: Sep 6, 2021
+ *      Author: joanvallve
+ */
+
+#include "core/processor/processor_fix_wing_model.h"
+
+#include "core/capture/capture_void.h"
+#include "core/feature/feature_base.h"
+
+namespace wolf
+{
+
+ProcessorFixWingModel::ProcessorFixWingModel(ParamsProcessorMotionModelPtr _params) :
+        params_motion_model_(_params)
+{
+}
+
+ProcessorFixWingModel::~ProcessorFixWingModel()
+{
+}
+
+void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
+{
+    // emplace capture
+    auto cap = CaptureBase::emplace<CapureVoid>(_keyframe_ptr, _keyframe_ptr->getTimeStamp(), getSensor());
+    
+    // emplace feature
+    auto fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase", 
+                                                 measurement, 
+                                                 cov);
+    
+    // emplace factor
+    auto fac = 
+}
+
+} /* namespace wolf */
+
+
+// Register in the FactoryProcessor
+#include "core/processor/factory_processor.h"
+namespace wolf {
+WOLF_REGISTER_PROCESSOR(ProcessorFixWingModel);
+WOLF_REGISTER_PROCESSOR_AUTO(ProcessorFixWingModel);
+} // namespace wolf
+

src/processor/processor_frame_factor.cpp

-/*
- * processor_frame_factor.cpp
- *
- *  Created on: Sep 6, 2021
- *      Author: joanvallve
- */
-
-#include "../../include/core/processor/processor_frame_factor.h"
-
-namespace wolf
-{
-
-ProcessorFrameFactor::ProcessorFrameFactor(ParamsProcessorMotionModelPtr _params) :
-        params_motion_model_(_params)
-{
-}
-
-ProcessorFrameFactor::~ProcessorFrameFactor()
-{
-}
-
-void ProcessorFrameFactor::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
-{
-    // TODO
-}
-
-} /* namespace wolf */
-
-
-// Register in the FactoryProcessor
-#include "core/processor/factory_processor.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR(ProcessorFrameFactor);
-WOLF_REGISTER_PROCESSOR_AUTO(ProcessorFrameFactor);
-} // namespace wolf
-