Resolve "Processor motion model"

Closes #407 (closed)

include/core/factor/factor_velocity_direction_3d.h

@@ -17,7 +17,7 @@ WOLF_PTR_TYPEDEFS(FactorVelocityDirection3d);
 class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d,1,3,4>
 {
     protected:
-        double min_vel_norm_;
+        double min_vel_sq_norm_; // stored in squared norm for efficiency
 
     public:
         FactorVelocityDirection3d(FeatureBasePtr _ftr_ptr,
@@ -34,8 +34,9 @@ class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d
                                                                 _status,
                                                                 _ftr_ptr->getFrame()->getV(),
                                                                 _ftr_ptr->getFrame()->getO()),
-                min_vel_norm_(_min_vel_norm)
+                min_vel_sq_norm_(_min_vel_norm*_min_vel_norm)
         {
+            assert(abs(_ftr_ptr->getMeasurement().norm() - 1.0) < wolf::EPS && "velocity direction measurement must be normalized");
 //            std::cout << "created FactorVelocityDirection3d " << std::endl;
         }
 
@@ -51,7 +52,7 @@ 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_)
+    if (v.squaredNorm() < min_vel_sq_norm_)
     {
         _residuals[0] = T(0);
         return true;
@@ -73,7 +74,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)
-    T 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()));
 
     // residual
     _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * error;