asserts for sizes

include/core/factor/factor_relative_pose_3d.h

@@ -93,6 +93,8 @@ inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_cur
                                                             _frame_past_ptr->getP(), // past frame P
                                                             _frame_past_ptr->getO()) // past frame Q
 {
+    MatrixSizeCheck<7,1>::check(_ftr_current_ptr->getMeasurement());
+    MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
 }
 
 template<typename T>

include/core/factor/factor_velocity_direction_3d.h

@@ -36,8 +36,9 @@ class FactorVelocityDirection3d: public FactorAutodiff<FactorVelocityDirection3d
                                                                 _ftr_ptr->getFrame()->getO()),
                 min_vel_sq_norm_(_min_vel_norm*_min_vel_norm)
         {
+            MatrixSizeCheck<3,1>::check(_ftr_ptr->getMeasurement());
+            MatrixSizeCheck<1,1>::check(_ftr_ptr->getMeasurementSquareRootInformationUpper());
             assert(abs(_ftr_ptr->getMeasurement().norm() - 1.0) < wolf::Constants::EPS && "velocity direction measurement must be normalized");
-//            std::cout << "created FactorVelocityDirection3d " << std::endl;
         }
 
         ~FactorVelocityDirection3d() override = default;
@@ -73,11 +74,16 @@ inline bool FactorVelocityDirection3d::operator ()(const T* const _v, const T* c
     //                              << v_local(1) << " "
     //                              << v_local(2) << "\n";
 
-    // error (angle between measurement and velocity in local coordinates)
-    T error = acos( v_local.dot(getMeasurement()) / (v_local.norm()));
+    // error: angle between specified local velocity direction (measurement) and velocity in local coordinates
+    T cos_error =  v_local.dot(getMeasurement()) / (v_local.norm()); // direction already normalized
+    while (cos_error > T(1))
+        cos_error /= T(0.99);
+
+    //std::cout << "FactorVelocityDirection3d cos_error = " << cos_error << std::endl;
+    //std::cout << "FactorVelocityDirection3d error = " << acos(error) << std::endl;
 
     // residual
-    _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * error;
+    _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * acos(cos_error);
 
     return true;
 }