Resolve "Processor motion model"

Closes #407 (closed)

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