Resolve "Processor motion model"

Closes #407 (closed)

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;