From a20db862024c538364554eb09c634db311ecfac7 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 1 Oct 2021 16:01:27 +0200
Subject: [PATCH] still not passing gtest!
---
.../factor_velocity_local_direction_3d.h | 98 +++++---
.../core/processor/processor_fix_wing_model.h | 4 +-
...est_factor_velocity_local_direction_3d.cpp | 226 +++++++++++++++---
3 files changed, 267 insertions(+), 61 deletions(-)
diff --git a/include/core/factor/factor_velocity_local_direction_3d.h b/include/core/factor/factor_velocity_local_direction_3d.h
index b66f57af4..13ff8451c 100644
--- a/include/core/factor/factor_velocity_local_direction_3d.h
+++ b/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;
}
diff --git a/include/core/processor/processor_fix_wing_model.h b/include/core/processor/processor_fix_wing_model.h
index 73e7f3cc2..81adaf034 100644
--- a/include/core/processor/processor_fix_wing_model.h
+++ b/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
{
diff --git a/test/gtest_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp
index caf49d273..d0eeb3b85 100644
--- a/test/gtest_factor_velocity_local_direction_3d.cpp
+++ b/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++)
--
GitLab