Resolve "Processor motion model"

Closes #407 (closed)

include/core/factor/factor_velocity_local_direction_3d.h

@@ -21,19 +21,19 @@ class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalD
 
     public:
         FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr,
-                                  const double& _min_vel_norm,
-                                  const ProcessorBasePtr& _processor_ptr,
-                                  bool _apply_loss_function,
-                                  FactorStatus _status = FAC_ACTIVE) :
+                                       const double& _min_vel_norm,
+                                       const ProcessorBasePtr& _processor_ptr,
+                                       bool _apply_loss_function,
+                                       FactorStatus _status = FAC_ACTIVE) :
                 FactorAutodiff<FactorVelocityLocalDirection3d,1,3,4>("FactorVelocityDirection3d",
-                                                                TOP_ABS,
-                                                                _ftr_ptr,
-                                                                nullptr, nullptr, nullptr, nullptr,
-                                                                _processor_ptr,
-                                                                _apply_loss_function,
-                                                                _status,
-                                                                _ftr_ptr->getFrame()->getV(),
-                                                                _ftr_ptr->getFrame()->getO()),
+                                                                     TOP_ABS,
+                                                                     _ftr_ptr,
+                                                                     nullptr, nullptr, nullptr, nullptr,
+                                                                     _processor_ptr,
+                                                                     _apply_loss_function,
+                                                                     _status,
+                                                                     _ftr_ptr->getFrame()->getV(),
+                                                                     _ftr_ptr->getFrame()->getO()),
                 min_vel_sq_norm_(_min_vel_norm*_min_vel_norm)
         {
             MatrixSizeCheck<3,1>::check(_ftr_ptr->getMeasurement());
@@ -58,32 +58,70 @@ inline bool FactorVelocityLocalDirection3d::operator ()(const T* const _v, const
         _residuals[0] = T(0);
         return true;
     }
-
-    //     std::cout << "v: " << v(0) << " "
-    //                        << v(1) << " "
-    //                        << v(2) << "\n";
-    //     std::cout << "q: " << q.coeffs()(0) << " "
-    //                        << q.coeffs()(1) << " "
-    //                        << q.coeffs()(2) << " "
-    //                        << q.coeffs()(3) << "\n";
-
+    std::cout << "----------\ndesired v_local: " << getMeasurement()(0) << " "
+                                                 << getMeasurement()(1) << " "
+                                                 << getMeasurement()(2) << "\n";
+    //std::cout << "v: " << v(0) << " "
+    //                   << v(1) << " "
+    //                   << v(2) << "\n";
+    //std::cout << "q: " << q.coeffs()(0) << " "
+    //                   << q.coeffs()(1) << " "
+    //                   << q.coeffs()(2) << " "
+    //                   << q.coeffs()(3) << "\n";
+
+//    // desired direction in global coordinates
+//    Eigen::Matrix<T,3,1> v_desired_global = q * getMeasurement().cast<T>();
+//
+//    //std::cout << "v_desired_global: " << v_desired_global(0) << " "
+//    //                                  << v_desired_global(1) << " "
+//    //                                  << v_desired_global(2) << "\n";
+//
+//    std::cout << "v.dot(v_desired_global) - v.norm() = " << v.dot(v_desired_global) - v.norm() << "\n";
+//    std::cout << "angle = " << acos(v.dot(v_desired_global) / v.norm()) << std::endl;
+//
+//    _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * (v.dot(v_desired_global) - v.norm());
+//
+//    return true;
+//
     // velocity in local coordinates
     Eigen::Matrix<T,3,1> v_local = q.conjugate() * v;
 
-    //     std::cout << "v_local: " << v_local(0) << " "
-    //                              << v_local(1) << " "
-    //                              << v_local(2) << "\n";
+    std::cout << "v_local: \n\t" << v_local(0) << "\n\t"
+                                 << v_local(1) << "\n\t"
+                                 << v_local(2) << "\n";
+    std::cout << "v_desired: \n\t" << getMeasurement()(0) << "\n\t"
+                                   << getMeasurement()(1) << "\n\t"
+                                   << getMeasurement()(2) << "\n";
+//
+//    std::cout << "v_local.dot(getMeasurement()) - v.norm() = " << v_local.dot(getMeasurement()) - v.norm() << "\n";
+//
+//    _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * (v_local.dot(getMeasurement()) - v.norm());
+//
+//    return true;
+
 
     // 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);
+    T cos_angle =  v_local.dot(getMeasurement()) / v.norm(); // direction already normalized
+    while (cos_angle > T(1) or cos_angle < T(-1))
+    {
+        WOLF_WARN("cos_angle > 1 or < -1!! ", cos_angle);
+        WOLF_WARN("v.norm() = ", v.norm());
+        cos_angle *= T(0.999);
+    }
+
+    std::cout << "FactorVelocityDirection3d cos_angle = " << cos_angle << std::endl;
+    std::cout << "FactorVelocityDirection3d angle = " << acos(cos_angle) << std::endl;
+
+    if (cos_angle == T(1))
+    {
+        std::cout << "FactorVelocityDirection3d detected cos_error == T(1). Returning residual T(0) to avoid NaN jacobian\n";
+        _residuals[0] = T(0);
+        return true;
+    }
 
-    //std::cout << "FactorVelocityDirection3d cos_error = " << cos_error << std::endl;
-    //std::cout << "FactorVelocityDirection3d error = " << acos(error) << std::endl;
 
     // residual
-    _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * acos(cos_error);
+    _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * acos(cos_angle);
 
     return true;
 }