still not passing gtest!

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;
 }

include/core/processor/processor_fix_wing_model.h

@@ -27,7 +27,9 @@ struct ParamsProcessorFixWingModel : public ParamsProcessorBase
         {
             velocity_local   = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/velocity_local");
             angle_stdev      = _server.getParam<double>          (prefix + _unique_name + "/angle_stdev");
-            min_vel_norm      = _server.getParam<double>         (prefix + _unique_name + "/min_vel_norm");
+            min_vel_norm     = _server.getParam<double>          (prefix + _unique_name + "/min_vel_norm");
+
+            assert(abs(velocity_local.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixWingModel: 'velocity_local' must be normalized");
         }
         std::string print() const override
         {

test/gtest_factor_velocity_local_direction_3d.cpp

@@ -31,6 +31,7 @@ class FactorVelocityLocalDirection3dTest : public testing::Test
             // Frame
             frm = problem->emplaceFrame(0.0, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
             sb_p = frm->getP();
+            sb_p->fix();
             sb_o = frm->getO();
             sb_v = frm->getV();
 
@@ -53,6 +54,60 @@ class FactorVelocityLocalDirection3dTest : public testing::Test
                                                                        nullptr,
                                                                        false);
         }
+
+        void testLocalVelocity(const Quaterniond& o,
+                               const Vector3d& v_local,
+                               bool estimate_o,
+                               bool estimate_v)
+        {
+            // set state
+            sb_o->setState(o.coeffs());
+            sb_v->setState(o * v_local);
+
+            // fix or unfix & perturb
+            if (not estimate_o)
+                sb_o->fix();
+            else
+            {
+                sb_o->unfix();
+                //sb_o->perturb();
+            }
+            if (not estimate_v)
+                sb_v->fix();
+            else
+            {
+                sb_v->unfix();
+                sb_v->perturb();
+            }
+
+            // emplace feature & factor
+            emplaceFeatureAndFactor(v_local, 0.01);
+
+            std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+            WOLF_INFO(report);
+
+            // Check in local coordinates
+            auto v_est_local_normalized = (Quaterniond(Vector4d(sb_o->getState())).conjugate() * sb_v->getState()).normalized();
+            auto cos_angle_local = v_est_local_normalized.dot(v_local);
+            WOLF_INFO("cos(angle local) = ", cos_angle_local);
+            WOLF_INFO_COND(cos_angle_local > 1, "cos(angle local) > 1 in ", cos_angle_local -1.0);
+            WOLF_INFO("angle local = ", acos(cos_angle_local));
+            ASSERT_NEAR(v_est_local_normalized.dot(v_local), 1, 1e-3);
+            if (cos_angle_local > 1)
+                cos_angle_local = 1;
+            ASSERT_NEAR(acos(cos_angle_local), 0, 1e-3);
+
+            // Check in global coordinates
+            auto v_global = Quaterniond(Vector4d(sb_o->getState())) * v_local;
+            auto v_est_normalized = sb_v->getState().normalized();
+            auto cos_angle_global = v_est_normalized.dot(v_global);
+            WOLF_INFO("cos(angle global) = ", cos_angle_global);
+            WOLF_INFO("angle global = ", acos(cos_angle_global));
+            ASSERT_NEAR(cos_angle_global, 1, 1e-3);
+            if (cos_angle_global > 1)
+                cos_angle_global = 1;
+            ASSERT_NEAR(acos(cos_angle_global), 0, 1e-3);
+        }
 };
 
 // Input odometry data and covariance
@@ -69,50 +124,161 @@ TEST_F(FactorVelocityLocalDirection3dTest, check_tree)
     ASSERT_DEATH({emplaceFeatureAndFactor(Vector3d(10.0, 0.0, 0.0), 0.1);},""); // Not normalized
 }
 
-TEST_F(FactorVelocityLocalDirection3dTest, origin_x_fix_PO_solve)
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_x)
 {
-    // set state
-    sb_p->setState(Vector3d::Zero());
-    sb_o->setState(Quaterniond::Identity().coeffs());
-    sb_v->setState(Vector3d::Random());
+    testLocalVelocity(Quaterniond::Identity(),
+                      (Vector3d() << 1, 0, 0).finished(),
+                      false, true);
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_y)
+{
+    testLocalVelocity(Quaterniond::Identity(),
+                      (Vector3d() << 0, 1, 0).finished(),
+                      false, true);
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_z)
+{
+    testLocalVelocity(Quaterniond::Identity(),
+                      (Vector3d() << 0, 0, 1).finished(),
+                      false, true);
+}
 
-    // fix
-    sb_p->fix();
-    sb_o->fix();
-    sb_v->unfix();
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_nx)
+{
+    testLocalVelocity(Quaterniond::Identity(),
+                      (Vector3d() << -1, 0, 0).finished(),
+                      false, true);
+}
 
-    // emplace feature & factor
-    Vector3d v_local(1.0, 0.0, 0.0);
-    emplaceFeatureAndFactor(v_local, 0.1);
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_ny)
+{
+    testLocalVelocity(Quaterniond::Identity(),
+                      (Vector3d() << 0, -1, 0).finished(),
+                      false, true);
+}
 
-    std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_nz)
+{
+    testLocalVelocity(Quaterniond::Identity(),
+                      (Vector3d() << 0, 0, -1).finished(),
+                      false, true);
+}
 
-    ASSERT_LT(abs(sb_v->getState()(1)), 1e-9);
-    ASSERT_LT(abs(sb_v->getState()(2)), 1e-9);
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_xy)
+{
+    testLocalVelocity(Quaterniond::Identity(),
+                      (Vector3d() << 0, 1, 1).finished().normalized(),
+                      false, true);
 }
 
-TEST_F(FactorVelocityLocalDirection3dTest, origin_y_fix_PO_solve)
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_nxny)
 {
-    // set state
-    sb_p->setState(Vector3d::Zero());
-    sb_o->setState(Quaterniond::Identity().coeffs());
-    sb_v->setState(Vector3d::Random());
+    testLocalVelocity(Quaterniond::Identity(),
+                      (Vector3d() << 0, -1, -1).finished().normalized(),
+                      false, true);
+}
 
-    // fix
-    sb_p->fix();
-    sb_o->fix();
-    sb_v->unfix();
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_random)
+{
+    testLocalVelocity(Quaterniond::Identity(),
+                      Vector3d::Random().normalized(),
+                      false, true);
+}
 
-    // emplace feature & factor
-    Vector3d v_local(0.0, 1.0, 0.0);
-    emplaceFeatureAndFactor(v_local, 0.1);
+TEST_F(FactorVelocityLocalDirection3dTest, xturned_solveV_x)
+{
+    testLocalVelocity(Quaterniond((Vector4d() << 1, 1, 0, 0).finished().normalized()),
+                      (Vector3d() << 1, 0, 0).finished(),
+                      false, true);
+}
 
-    std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+TEST_F(FactorVelocityLocalDirection3dTest, xturned_solveV_y)
+{
+    testLocalVelocity(Quaterniond((Vector4d() << 1, 1, 0, 0).finished().normalized()),
+                      (Vector3d() << 0, 1, 0).finished(),
+                      false, true);
+}
 
-    ASSERT_LT(abs(sb_v->getState()(0)), 1e-9);
-    ASSERT_LT(abs(sb_v->getState()(2)), 1e-9);
+TEST_F(FactorVelocityLocalDirection3dTest, xturned_solveV_z)
+{
+    testLocalVelocity(Quaterniond((Vector4d() << 1, 1, 0, 0).finished().normalized()),
+                      (Vector3d() << 0, 0, 1).finished(),
+                      false, true);
 }
 
+TEST_F(FactorVelocityLocalDirection3dTest, yturned_solveV_x)
+{
+    testLocalVelocity(Quaterniond((Vector4d() << 1, 0, 1, 0).finished().normalized()),
+                      (Vector3d() << 1, 0, 0).finished(),
+                      false, true);
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, yturned_solveV_y)
+{
+    testLocalVelocity(Quaterniond((Vector4d() << 1, 0, 1, 0).finished().normalized()),
+                      (Vector3d() << 0, 1, 0).finished(),
+                      false, true);
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, yturned_solveV_z)
+{
+    testLocalVelocity(Quaterniond((Vector4d() << 1, 0, 1, 0).finished().normalized()),
+                      (Vector3d() << 0, 0, 1).finished(),
+                      false, true);
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, zturned_solveV_x)
+{
+    testLocalVelocity(Quaterniond((Vector4d() << 1, 0, 0, 1).finished().normalized()),
+                      (Vector3d() << 1, 0, 0).finished(),
+                      false, true);
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, zturned_solveV_y)
+{
+    testLocalVelocity(Quaterniond((Vector4d() << 1, 0, 0, 1).finished().normalized()),
+                      (Vector3d() << 0, 1, 0).finished(),
+                      false, true);
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, zturned_solveV_z)
+{
+    testLocalVelocity(Quaterniond((Vector4d() << 1, 0, 0, 1).finished().normalized()),
+                      (Vector3d() << 0, 0, 1).finished(),
+                      false, true);
+}
+
+// SOLVE O (not observable but error minimization ok)
+/*TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO_x)
+{
+    testLocalVelocity(Quaterniond::Identity(),
+                      (Vector3d() << 1, 0, 0).finished(),
+                      true, false);
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO_y)
+{
+    testLocalVelocity(Quaterniond::Identity(),
+                      (Vector3d() << 0, 1, 0).finished(),
+                      true, false);
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO_z)
+{
+    testLocalVelocity(Quaterniond::Identity(),
+                      (Vector3d() << 0, 0, 1).finished(),
+                      true, false);
+}
+
+TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO_random)
+{
+    testLocalVelocity(Quaterniond::Identity(),
+                      Vector3d::Random(),
+                      true, false);
+}*/
+
 //TEST_F(FactorVelocityLocalDirection3dTest, fix_PO_solve)
 //{
 //    for (int i = 0; i < 1e3; i++)