Skip to content
Snippets Groups Projects
Commit a20db862 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

still not passing gtest!

parent c5bafa69
No related branches found
No related tags found
1 merge request!419Resolve "Processor motion model"
Pipeline #6746 failed
...@@ -21,19 +21,19 @@ class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalD ...@@ -21,19 +21,19 @@ class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalD
public: public:
FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr, FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr,
const double& _min_vel_norm, const double& _min_vel_norm,
const ProcessorBasePtr& _processor_ptr, const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function, bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE) : FactorStatus _status = FAC_ACTIVE) :
FactorAutodiff<FactorVelocityLocalDirection3d,1,3,4>("FactorVelocityDirection3d", FactorAutodiff<FactorVelocityLocalDirection3d,1,3,4>("FactorVelocityDirection3d",
TOP_ABS, TOP_ABS,
_ftr_ptr, _ftr_ptr,
nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr,
_processor_ptr, _processor_ptr,
_apply_loss_function, _apply_loss_function,
_status, _status,
_ftr_ptr->getFrame()->getV(), _ftr_ptr->getFrame()->getV(),
_ftr_ptr->getFrame()->getO()), _ftr_ptr->getFrame()->getO()),
min_vel_sq_norm_(_min_vel_norm*_min_vel_norm) min_vel_sq_norm_(_min_vel_norm*_min_vel_norm)
{ {
MatrixSizeCheck<3,1>::check(_ftr_ptr->getMeasurement()); MatrixSizeCheck<3,1>::check(_ftr_ptr->getMeasurement());
...@@ -58,32 +58,70 @@ inline bool FactorVelocityLocalDirection3d::operator ()(const T* const _v, const ...@@ -58,32 +58,70 @@ inline bool FactorVelocityLocalDirection3d::operator ()(const T* const _v, const
_residuals[0] = T(0); _residuals[0] = T(0);
return true; return true;
} }
std::cout << "----------\ndesired v_local: " << getMeasurement()(0) << " "
// std::cout << "v: " << v(0) << " " << getMeasurement()(1) << " "
// << v(1) << " " << getMeasurement()(2) << "\n";
// << v(2) << "\n"; //std::cout << "v: " << v(0) << " "
// std::cout << "q: " << q.coeffs()(0) << " " // << v(1) << " "
// << q.coeffs()(1) << " " // << v(2) << "\n";
// << q.coeffs()(2) << " " //std::cout << "q: " << q.coeffs()(0) << " "
// << q.coeffs()(3) << "\n"; // << 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 // velocity in local coordinates
Eigen::Matrix<T,3,1> v_local = q.conjugate() * v; Eigen::Matrix<T,3,1> v_local = q.conjugate() * v;
// std::cout << "v_local: " << v_local(0) << " " std::cout << "v_local: \n\t" << v_local(0) << "\n\t"
// << v_local(1) << " " << v_local(1) << "\n\t"
// << v_local(2) << "\n"; << 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 // 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 T cos_angle = v_local.dot(getMeasurement()) / v.norm(); // direction already normalized
while (cos_error > T(1)) while (cos_angle > T(1) or cos_angle < T(-1))
cos_error /= T(0.99); {
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 // residual
_residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * acos(cos_error); _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * acos(cos_angle);
return true; return true;
} }
......
...@@ -27,7 +27,9 @@ struct ParamsProcessorFixWingModel : public ParamsProcessorBase ...@@ -27,7 +27,9 @@ struct ParamsProcessorFixWingModel : public ParamsProcessorBase
{ {
velocity_local = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/velocity_local"); velocity_local = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/velocity_local");
angle_stdev = _server.getParam<double> (prefix + _unique_name + "/angle_stdev"); 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 std::string print() const override
{ {
......
...@@ -31,6 +31,7 @@ class FactorVelocityLocalDirection3dTest : public testing::Test ...@@ -31,6 +31,7 @@ class FactorVelocityLocalDirection3dTest : public testing::Test
// Frame // Frame
frm = problem->emplaceFrame(0.0, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); frm = problem->emplaceFrame(0.0, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
sb_p = frm->getP(); sb_p = frm->getP();
sb_p->fix();
sb_o = frm->getO(); sb_o = frm->getO();
sb_v = frm->getV(); sb_v = frm->getV();
...@@ -53,6 +54,60 @@ class FactorVelocityLocalDirection3dTest : public testing::Test ...@@ -53,6 +54,60 @@ class FactorVelocityLocalDirection3dTest : public testing::Test
nullptr, nullptr,
false); 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 // Input odometry data and covariance
...@@ -69,50 +124,161 @@ TEST_F(FactorVelocityLocalDirection3dTest, check_tree) ...@@ -69,50 +124,161 @@ TEST_F(FactorVelocityLocalDirection3dTest, check_tree)
ASSERT_DEATH({emplaceFeatureAndFactor(Vector3d(10.0, 0.0, 0.0), 0.1);},""); // Not normalized 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 testLocalVelocity(Quaterniond::Identity(),
sb_p->setState(Vector3d::Zero()); (Vector3d() << 1, 0, 0).finished(),
sb_o->setState(Quaterniond::Identity().coeffs()); false, true);
sb_v->setState(Vector3d::Random()); }
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 TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_nx)
sb_p->fix(); {
sb_o->fix(); testLocalVelocity(Quaterniond::Identity(),
sb_v->unfix(); (Vector3d() << -1, 0, 0).finished(),
false, true);
}
// emplace feature & factor TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_ny)
Vector3d v_local(1.0, 0.0, 0.0); {
emplaceFeatureAndFactor(v_local, 0.1); 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); TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_xy)
ASSERT_LT(abs(sb_v->getState()(2)), 1e-9); {
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 testLocalVelocity(Quaterniond::Identity(),
sb_p->setState(Vector3d::Zero()); (Vector3d() << 0, -1, -1).finished().normalized(),
sb_o->setState(Quaterniond::Identity().coeffs()); false, true);
sb_v->setState(Vector3d::Random()); }
// fix TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_random)
sb_p->fix(); {
sb_o->fix(); testLocalVelocity(Quaterniond::Identity(),
sb_v->unfix(); Vector3d::Random().normalized(),
false, true);
}
// emplace feature & factor TEST_F(FactorVelocityLocalDirection3dTest, xturned_solveV_x)
Vector3d v_local(0.0, 1.0, 0.0); {
emplaceFeatureAndFactor(v_local, 0.1); 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); TEST_F(FactorVelocityLocalDirection3dTest, xturned_solveV_z)
ASSERT_LT(abs(sb_v->getState()(2)), 1e-9); {
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) //TEST_F(FactorVelocityLocalDirection3dTest, fix_PO_solve)
//{ //{
// for (int i = 0; i < 1e3; i++) // for (int i = 0; i < 1e3; i++)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment