Resolve "Processor motion model"

Closes #407 (closed)

include/core/factor/factor_velocity_direction_3d.h

@@ -36,7 +36,7 @@ class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d
                                                                 _ftr_ptr->getFrame()->getO()),
                 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");
+            assert(abs(_ftr_ptr->getMeasurement().norm() - 1.0) < wolf::Constants::EPS && "velocity direction measurement must be normalized");
 //            std::cout << "created FactorVelocityDirection3d " << std::endl;
         }
 
@@ -74,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()));
+    T error = acos( v_local.dot(getMeasurement()) / (v_local.norm()));
 
     // residual
     _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * error;