compiling

include/core/factor/factor_velocity_direction_3d.h

@@ -62,7 +62,7 @@ inline bool FactorVelocityDirection3d::operator ()(const T* const _v, const T* c
     //                              << 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()));
+    T error = acos( v_local.dot(getMeasurement().cast<T>()) / (v_local.norm() * getMeasurement().cast<T>().norm()));
 
     // residual
     _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * error;

include/core/processor/processor_constant_velocity.h

@@ -17,14 +17,18 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorConstantVelocity);
 
 struct ParamsProcessorConstantVelocity : public ParamsProcessorBase
 {
+        Eigen::MatrixXd cov_v_rate;
+
         ParamsProcessorConstantVelocity() = default;
         ParamsProcessorConstantVelocity(std::string _unique_name, const wolf::ParamsServer & _server) :
             ParamsProcessorBase(_unique_name, _server)
         {
+            cov_v_rate = _server.getParam<Eigen::VectorXd> (prefix + _unique_name + "/cov_rate_diagonal").asDiagonal();
         }
         std::string print() const override
         {
-            return ParamsProcessorBase::print();
+            return ParamsProcessorBase::print()  + "\n"
+                + "cov_v_rate: print not implemented\n";
         }
 };
 

include/core/processor/processor_fix_wing_model.h

@@ -17,14 +17,21 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixWingModel);
 
 struct ParamsProcessorFixWingModel : public ParamsProcessorBase
 {
+        Eigen::Vector3d velocity_local;
+        double angle_stdev;
+
         ParamsProcessorFixWingModel() = default;
         ParamsProcessorFixWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
             ParamsProcessorBase(_unique_name, _server)
         {
+            velocity_local   = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/velocity_local");
+            angle_stdev      = _server.getParam<double>          (prefix + _unique_name + "/angle_stdev");
         }
         std::string print() const override
         {
-            return ParamsProcessorBase::print();
+            return ParamsProcessorBase::print()  + "\n"
+                + "velocity_local: print not implemented\n"
+                + "angle_stdev: " + std::to_string(angle_stdev) + "\n";
         }
 };
 
@@ -33,7 +40,7 @@ WOLF_PTR_TYPEDEFS(ProcessorFixWingModel);
 class ProcessorFixWingModel : public ProcessorBase
 {
     public:
-        ProcessorFixWingModel(ParamsProcessorMotionModelPtr);
+        ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params);
 
         // Factory method for high level API
         WOLF_PROCESSOR_CREATE(ProcessorFixWingModel, ParamsProcessorFixWingModel);

src/processor/processor_constant_velocity.cpp

@@ -7,7 +7,7 @@
 
 #include "core/processor/processor_constant_velocity.h"
 
-#include "core/capture/capture_void.h"
+#include "core/capture/capture_base.h"
 #include "core/feature/feature_base.h"
 #include "core/factor/factor_block_difference.h"
 
@@ -19,7 +19,6 @@ ProcessorConstantVelocity::ProcessorConstantVelocity(ParamsProcessorConstantVelo
         params_processor_(_params),
         last_keyframe_(nullptr)
 {
-    assert(params_processor_->cov_rate.rows() == getProblem()->getDim() && "covariance size is wrong");
 }
 
 ProcessorConstantVelocity::~ProcessorConstantVelocity()
@@ -28,7 +27,8 @@ ProcessorConstantVelocity::~ProcessorConstantVelocity()
 
 void ProcessorConstantVelocity::configure(SensorBasePtr _sensor)
 {
-    assert(getProblem()->getFrameStructure().find('V') != std::string::npos) && "Processor only works with problems with V");
+    assert(params_processor_->cov_v_rate.rows() == getProblem()->getDim() && "cov_v_rate size is wrong");
+    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)
@@ -40,9 +40,8 @@ void ProcessorConstantVelocity::processKeyFrame(FrameBasePtr _keyframe_ptr, cons
         _keyframe_ptr->getTimeStamp() > last_keyframe_->getTimeStamp())
     {
         // emplace capture
-        auto cap = CaptureBase::emplace<CapureVoid>(_keyframe_ptr,
-                                                    _keyframe_ptr->getTimeStamp(),
-                                                    getSensor());
+        auto cap = CaptureBase::emplace<CaptureBase>(_keyframe_ptr, "CaptureBase",
+                                                     _keyframe_ptr->getTimeStamp(), getSensor());
 
         double dt = _keyframe_ptr->getTimeStamp() - last_keyframe_->getTimeStamp();
 

src/processor/processor_fix_wing_model.cpp

@@ -7,14 +7,16 @@
 
 #include "core/processor/processor_fix_wing_model.h"
 
-#include "core/capture/capture_void.h"
+#include "core/capture/capture_base.h"
 #include "core/feature/feature_base.h"
+#include "core/factor/factor_velocity_direction_3d.h"
 
 namespace wolf
 {
 
-ProcessorFixWingModel::ProcessorFixWingModel(ParamsProcessorMotionModelPtr _params) :
-        params_motion_model_(_params)
+ProcessorFixWingModel::ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params) :
+        ProcessorBase("ProcessorFixWingModel", 3, _params),
+        params_processor_(_params)
 {
 }
 
@@ -25,15 +27,19 @@ 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());
+    auto cap = CaptureBase::emplace<CaptureBase>(_keyframe_ptr, "CaptureBase",
+                                                 _keyframe_ptr->getTimeStamp(), getSensor());
     
     // emplace feature
     auto fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase", 
-                                                 measurement, 
-                                                 cov);
+                                                 params_processor_->velocity_local,
+                                                 Eigen::Matrix1d(params_processor_->angle_stdev * params_processor_->angle_stdev));
     
     // emplace factor
-    auto fac = 
+    auto fac = FactorBase::emplace<FactorVelocityDirection3d>(fea,
+                                                              fea,
+                                                              shared_from_this(),
+                                                              false);
 }
 
 } /* namespace wolf */