wip

include/core/factor/factor_velocity_direction_3d.h

@@ -16,8 +16,12 @@ WOLF_PTR_TYPEDEFS(FactorVelocityDirection3d);
 //class
 class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d,1,3,4>
 {
+    protected:
+        double min_vel_norm_;
+
     public:
         FactorVelocityDirection3d(FeatureBasePtr _ftr_ptr,
+                                  const double& _min_vel_norm,
                                   const ProcessorBasePtr& _processor_ptr,
                                   bool _apply_loss_function,
                                   FactorStatus _status = FAC_ACTIVE) :
@@ -29,7 +33,8 @@ class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d
                                                                 _apply_loss_function,
                                                                 _status,
                                                                 _ftr_ptr->getFrame()->getV(),
-                                                                _ftr_ptr->getFrame()->getO())
+                                                                _ftr_ptr->getFrame()->getO()),
+                min_vel_norm_(_min_vel_norm)
         {
 //            std::cout << "created FactorVelocityDirection3d " << std::endl;
         }
@@ -46,6 +51,12 @@ inline bool FactorVelocityDirection3d::operator ()(const T* const _v, const T* c
     Eigen::Map<const Eigen::Matrix<T,3,1> > v(_v);
     Eigen::Map<const Eigen::Quaternion<T> > q(_q);
 
+    if (v.norm() < min_vel_norm_)
+    {
+        _residuals[0] = T(0);
+        return true;
+    }
+
     //     std::cout << "v: " << v(0) << " "
     //                        << v(1) << " "
     //                        << v(2) << "\n";

include/core/processor/processor_fix_wing_model.h

@@ -19,6 +19,7 @@ struct ParamsProcessorFixWingModel : public ParamsProcessorBase
 {
         Eigen::Vector3d velocity_local;
         double angle_stdev;
+        double min_vel_norm;
 
         ParamsProcessorFixWingModel() = default;
         ParamsProcessorFixWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
@@ -26,12 +27,14 @@ struct ParamsProcessorFixWingModel : public ParamsProcessorBase
         {
             velocity_local   = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/velocity_local");
             angle_stdev      = _server.getParam<double>          (prefix + _unique_name + "/angle_stdev");
+            min_vel_norm      = _server.getParam<double>         (prefix + _unique_name + "/min_vel_norm");
         }
         std::string print() const override
         {
             return ParamsProcessorBase::print()  + "\n"
                 + "velocity_local: print not implemented\n"
-                + "angle_stdev: " + std::to_string(angle_stdev) + "\n";
+                + "angle_stdev: " + std::to_string(angle_stdev) + "\n"
+                + "min_vel_norm: " + std::to_string(min_vel_norm) + "\n";
         }
 };
 
@@ -46,7 +49,7 @@ class ProcessorFixWingModel : public ProcessorBase
         WOLF_PROCESSOR_CREATE(ProcessorFixWingModel, ParamsProcessorFixWingModel);
 
         virtual ~ProcessorFixWingModel() override;
-        void configure(SensorBasePtr _sensor) override {};
+        void configure(SensorBasePtr _sensor) override;
 
     protected:
 

src/processor/processor_constant_velocity.cpp

@@ -27,8 +27,8 @@ ProcessorConstantVelocity::~ProcessorConstantVelocity()
 
 void ProcessorConstantVelocity::configure(SensorBasePtr _sensor)
 {
-    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");
+    assert(params_processor_->cov_v_rate.rows() == _sensor->getProblem()->getDim() && "cov_v_rate size is wrong");
+    assert(_sensor->getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V");
 }
 
 void ProcessorConstantVelocity::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)

src/processor/processor_fix_wing_model.cpp

@@ -24,8 +24,16 @@ ProcessorFixWingModel::~ProcessorFixWingModel()
 {
 }
 
+void ProcessorFixWingModel::configure(SensorBasePtr _sensor)
+{
+    assert(_sensor->getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V");
+}
+
 void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
 {
+    if (_keyframe_ptr->getV()->isFixed())
+        return;
+
     // emplace capture
     auto cap = CaptureBase::emplace<CaptureBase>(_keyframe_ptr, "CaptureBase",
                                                  _keyframe_ptr->getTimeStamp(), getSensor());
@@ -38,6 +46,7 @@ void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr, const do
     // emplace factor
     auto fac = FactorBase::emplace<FactorVelocityDirection3d>(fea,
                                                               fea,
+                                                              params_processor_->min_vel_norm,
                                                               shared_from_this(),
                                                               false);
 }