diff --git a/include/core/factor/factor_velocity_local_direction_3d.h b/include/core/factor/factor_velocity_local_direction_3d.h index 13ff8451ccb352b905fb7427e001a40eccc4b79a..31bf83599189cda1033ac38ea43dd36a9cb99500 100644 --- a/include/core/factor/factor_velocity_local_direction_3d.h +++ b/include/core/factor/factor_velocity_local_direction_3d.h @@ -18,12 +18,15 @@ class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalD { protected: double min_vel_sq_norm_; // stored in squared norm for efficiency + bool residual_local_, residual_angle_; public: FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr, const double& _min_vel_norm, const ProcessorBasePtr& _processor_ptr, bool _apply_loss_function, + bool _residual_local = true, + bool _residual_angle = true, FactorStatus _status = FAC_ACTIVE) : FactorAutodiff<FactorVelocityLocalDirection3d,1,3,4>("FactorVelocityDirection3d", TOP_ABS, @@ -34,11 +37,13 @@ class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalD _status, _ftr_ptr->getFrame()->getV(), _ftr_ptr->getFrame()->getO()), - min_vel_sq_norm_(_min_vel_norm*_min_vel_norm) + min_vel_sq_norm_(_min_vel_norm*_min_vel_norm), + residual_local_(_residual_local), + residual_angle_(_residual_angle) { 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"); + assert(std::abs(_ftr_ptr->getMeasurement().norm() - 1.0) < wolf::Constants::EPS && "velocity direction measurement must be normalized"); } ~FactorVelocityLocalDirection3d() override = default; @@ -58,68 +63,74 @@ inline bool FactorVelocityLocalDirection3d::operator ()(const T* const _v, const _residuals[0] = T(0); return true; } - 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()); + +// std::cout << "----------\n"; +// std::cout << "v desired (local): \n\t" << getMeasurement()(0) << "\n\t" +// << getMeasurement()(1) << "\n\t" +// << getMeasurement()(2) << "\n"; // -// return true; +// Eigen::Matrix<T,3,1> v_desired_global = q * getMeasurement().cast<T>(); +// std::cout << "v desired (global): \n\t" << v_desired_global(0) << "\n\t" +// << v_desired_global(1) << "\n\t" +// << v_desired_global(2) << "\n"; // - // velocity in local coordinates - Eigen::Matrix<T,3,1> v_local = q.conjugate() * v; - - 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"; +// Eigen::Matrix<T,3,1> v_local = q.conjugate() * v; +// std::cout << "v (local): \n\t" << v_local(0) << "\n\t" +// << v_local(1) << "\n\t" +// << v_local(2) << "\n"; // -// std::cout << "v_local.dot(getMeasurement()) - v.norm() = " << v_local.dot(getMeasurement()) - v.norm() << "\n"; +// std::cout << "v: \n\t" << v(0) << "\n\t" +// << v(1) << "\n\t" +// << v(2) << "\n"; // -// _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * (v_local.dot(getMeasurement()) - v.norm()); +// std::cout << "q: \n\t" << q.coeffs()(0) << "\n\t" +// << q.coeffs()(1) << "\n\t" +// << q.coeffs()(2) << "\n\t" +// << q.coeffs()(3) << "\n"; // -// return true; +// 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; + // DOT PRODUCT NORM RESIDUAL ------------------------------------------------- + // global coordinates + if (not residual_angle_ and not residual_local_) + { + _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * (v.dot(q * getMeasurement().cast<T>()) - v.norm()); + return true; + } + // local coordinates + if (not residual_angle_ and residual_local_) + { + _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * ((q.conjugate() * v).dot(getMeasurement().cast<T>()) / v.norm() - T(1.0)); + return true; + } + + // ANGLE RESIDUAL ------------------------------------------------------------ + T cos_angle; + // global coordinates + if (not residual_local_) + cos_angle = v.dot(q * getMeasurement().cast<T>()) / v.norm(); // direction already normalized + + // local coordinates + else + cos_angle = (q.conjugate() * v).dot(getMeasurement().cast<T>()) / v.norm(); // direction already normalized - // error: angle between specified local velocity direction (measurement) and velocity in local coordinates - T cos_angle = v_local.dot(getMeasurement()) / v.norm(); // direction already normalized + // numeric issues 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); + //WOLF_WARN("cos_angle > 1 or < -1!! ", cos_angle); + //WOLF_WARN("v.norm() = ", v.norm()); + cos_angle *= T(0.9); } - std::cout << "FactorVelocityDirection3d cos_angle = " << cos_angle << std::endl; - std::cout << "FactorVelocityDirection3d angle = " << acos(cos_angle) << std::endl; - + // in perfect fitting Jacobians are ill defined if (cos_angle == T(1)) { - std::cout << "FactorVelocityDirection3d detected cos_error == T(1). Returning residual T(0) to avoid NaN jacobian\n"; + //std::cout << "FactorVelocityDirection3d detected cos_error == T(1). Returning residual T(0) to avoid NaN jacobian\n"; _residuals[0] = T(0); return true; } - // residual _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * acos(cos_angle); diff --git a/include/core/processor/processor_fix_wing_model.h b/include/core/processor/processor_fix_wing_model.h index 81adaf0344883e4fd472597b9d5ea28a3ea78710..8c23917354ca1d0e0c3723045ded8d118e3c3abc 100644 --- a/include/core/processor/processor_fix_wing_model.h +++ b/include/core/processor/processor_fix_wing_model.h @@ -29,7 +29,7 @@ struct ParamsProcessorFixWingModel : public ParamsProcessorBase angle_stdev = _server.getParam<double> (prefix + _unique_name + "/angle_stdev"); 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"); + assert(std::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 d0eeb3b850559e502b2c3a76ebe8953ae6799bc5..37fa70557ea733d149f0f356fa145699460fdba4 100644 --- a/test/gtest_factor_velocity_local_direction_3d.cpp +++ b/test/gtest_factor_velocity_local_direction_3d.cpp @@ -20,13 +20,21 @@ class FactorVelocityLocalDirection3dTest : public testing::Test StateBlockPtr sb_v; CaptureBasePtr cap; + int n_tests, n_solved; + std::vector<VectorXd> convergence, iterations, time, error; + virtual void SetUp() { + n_tests = 1e2; + n_solved = 0; + // create problem problem = Problem::create("POV", 3); // create solver - solver = std::make_shared<SolverCeres>(problem); + auto params_solver = std::make_shared<ParamsCeres>(); + params_solver->solver_options.max_num_iterations = 1e3; + solver = std::make_shared<SolverCeres>(problem, params_solver); // Frame frm = problem->emplaceFrame(0.0, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished()); @@ -38,9 +46,30 @@ class FactorVelocityLocalDirection3dTest : public testing::Test // Capture cap = CaptureBase::emplace<CaptureBase>(frm, "CaptureBase", frm->getTimeStamp(), nullptr); + + // init + convergence = std::vector<VectorXd>({VectorXd(n_tests), + VectorXd(n_tests), + VectorXd(n_tests), + VectorXd(n_tests)}); + iterations = std::vector<VectorXd>({VectorXd(n_tests), + VectorXd(n_tests), + VectorXd(n_tests), + VectorXd(n_tests)}); + time = std::vector<VectorXd>({VectorXd(n_tests), + VectorXd(n_tests), + VectorXd(n_tests), + VectorXd(n_tests)}); + error = std::vector<VectorXd>({VectorXd(n_tests), + VectorXd(n_tests), + VectorXd(n_tests), + VectorXd(n_tests)}); } - FactorBasePtr emplaceFeatureAndFactor(const Vector3d& v_local, const double& angle_stdev ) + FactorBasePtr emplaceFeatureAndFactor(const Vector3d& v_local, + const double& angle_stdev, + bool residual_local, + bool residual_angle) { // emplace feature FeatureBasePtr fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase", @@ -52,7 +81,9 @@ class FactorVelocityLocalDirection3dTest : public testing::Test fea, 0, nullptr, - false); + false, + residual_local, + residual_angle); } void testLocalVelocity(const Quaterniond& o, @@ -60,6 +91,8 @@ class FactorVelocityLocalDirection3dTest : public testing::Test bool estimate_o, bool estimate_v) { + assert(n_solved <= n_tests); + // set state sb_o->setState(o.coeffs()); sb_v->setState(o * v_local); @@ -77,36 +110,95 @@ class FactorVelocityLocalDirection3dTest : public testing::Test else { sb_v->unfix(); + //sb_v->setState(Vector3d::Random()); sb_v->perturb(); } + auto init_o = sb_o->getState(); + auto init_v = sb_v->getState(); + + // COMPARE residuals + for (auto res = 0; res < 4; res++) + { + bool residual_local = (res == 0 or res == 1); + bool residual_angle = (res == 0 or res == 2); + //WOLF_INFO("============= Residual ", + // (residual_local ? "local " : "global "), + // (residual_angle ? "angle" : "dot product norm error")); + + // emplace feature & factor + auto fac = emplaceFeatureAndFactor(v_local, 0.001, residual_local, residual_angle); + + 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)); + //EXPECT_NEAR(v_est_local_normalized.dot(v_local), 1, 1e-2); + if (cos_angle_local > 1) + cos_angle_local = 1; + //EXPECT_NEAR(acos(cos_angle_local), 0, 5*M_PI/180); + + // 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)); + //EXPECT_NEAR(cos_angle_global, 1, 1e-2); + if (cos_angle_global > 1) + cos_angle_global = 1; + //EXPECT_NEAR(acos(cos_angle_global), 0, 5*M_PI/180); + + // PRINT + /*WOLF_INFO_COND(solver->hasConverged(), "\tSolver CONVERGED"); + WOLF_INFO_COND(not solver->hasConverged(), "\tSolver NOT CONVERGED"); + WOLF_INFO("\tSolver iterations: ", solver->iterations()); + WOLF_INFO("\tSolver total time: ", solver->totalTime()); + WOLF_INFO("\tangle error: ", acos(cos_angle_local));*/ + + // Update performaces + convergence.at(res)(n_solved) = (solver->hasConverged() ? 1 : 0); + iterations.at(res)(n_solved) = solver->iterations(); + time.at(res)(n_solved) = solver->totalTime(); + error.at(res)(n_solved) = acos(cos_angle_local); + + // Reset + fac->getFeature()->remove(); + sb_o->setState(init_o); + sb_v->setState(init_v); + ASSERT_EQ(cap->getFeatureList().size(),0); + } + n_solved++; + } - // 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); + void printAverageResults() + { + WOLF_INFO("/////////////////// Average results: "); + for (auto res = 0; res < 4; res++) + { + double conv_avg = convergence.at(res).head(n_solved).mean(); + double iter_avg = iterations.at(res).head(n_solved).mean(); + double time_avg = time.at(res).head(n_solved).mean(); + double err_avg = error.at(res).head(n_solved).mean(); + + double iter_stdev = (iterations.at(res).head(n_solved).array() - conv_avg).matrix().norm(); + double time_stdev = (time.at(res).head(n_solved).array() - conv_avg).matrix().norm(); + double err_stdev = (error.at(res).head(n_solved).array() - conv_avg).matrix().norm(); + + bool residual_local = (res == 0 or res == 1); + bool residual_angle = (res == 0 or res == 2); + WOLF_INFO("======= Residual ", + (residual_local ? "local " : "global "), + (residual_angle ? "angle" : "dot product norm error")); + WOLF_INFO("\tconvergence: ", 100 * conv_avg, "%"); + WOLF_INFO("\titerations: ", iter_avg, " - stdev: ", iter_stdev); + WOLF_INFO("\ttime: ", 1000 * time_avg, " ms", " - stdev: ", time_stdev); + WOLF_INFO("\tangle error: ", err_avg, " - stdev: ", err_stdev); + } } }; @@ -118,76 +210,31 @@ TEST_F(FactorVelocityLocalDirection3dTest, check_tree) { ASSERT_TRUE(problem->check(0)); - emplaceFeatureAndFactor(Vector3d(1.0, 0.0, 0.0), 0.1); + emplaceFeatureAndFactor(Vector3d(1.0, 0.0, 0.0), 0.1,true,true); ASSERT_TRUE(problem->check(0)); - ASSERT_DEATH({emplaceFeatureAndFactor(Vector3d(10.0, 0.0, 0.0), 0.1);},""); // Not normalized -} - -TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_x) -{ - 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); -} - -TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_nx) -{ - testLocalVelocity(Quaterniond::Identity(), - (Vector3d() << -1, 0, 0).finished(), - false, true); + ASSERT_DEATH({emplaceFeatureAndFactor(Vector3d(10.0, 0.0, 0.0), 0.1,true,true);},""); // Not normalized } -TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_ny) +TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV) { - testLocalVelocity(Quaterniond::Identity(), - (Vector3d() << 0, -1, 0).finished(), - false, true); + for (auto i = 0; i < n_tests; i++) + testLocalVelocity(Quaterniond::Identity(), + Vector3d::Random().normalized(), + false, true); + printAverageResults(); } -TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_nz) +TEST_F(FactorVelocityLocalDirection3dTest, random_solveV) { - testLocalVelocity(Quaterniond::Identity(), - (Vector3d() << 0, 0, -1).finished(), - false, true); + for (auto i = 0; i < n_tests; i++) + testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), + Vector3d::Random().normalized(), + false, true); + printAverageResults(); } -TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_xy) -{ - testLocalVelocity(Quaterniond::Identity(), - (Vector3d() << 0, 1, 1).finished().normalized(), - false, true); -} - -TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_nxny) -{ - testLocalVelocity(Quaterniond::Identity(), - (Vector3d() << 0, -1, -1).finished().normalized(), - false, true); -} - -TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_random) -{ - testLocalVelocity(Quaterniond::Identity(), - Vector3d::Random().normalized(), - false, true); -} - -TEST_F(FactorVelocityLocalDirection3dTest, xturned_solveV_x) +/*TEST_F(FactorVelocityLocalDirection3dTest, xturned_solveV_x) { testLocalVelocity(Quaterniond((Vector4d() << 1, 1, 0, 0).finished().normalized()), (Vector3d() << 1, 0, 0).finished(), @@ -248,7 +295,7 @@ 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)