diff --git a/include/core/factor/factor_velocity_local_direction_3d.h b/include/core/factor/factor_velocity_local_direction_3d.h new file mode 100644 index 0000000000000000000000000000000000000000..01e635e660a3cbd40d221a495d244241e2db2461 --- /dev/null +++ b/include/core/factor/factor_velocity_local_direction_3d.h @@ -0,0 +1,148 @@ + +#ifndef FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_ +#define FACTOR_VELOCITY_LOCAL_DIRECTION_3D_H_ + +//Wolf includes +#include "core/factor/factor_autodiff.h" +#include "core/frame/frame_base.h" +#include "core/math/rotations.h" + +namespace wolf { + +WOLF_PTR_TYPEDEFS(FactorVelocityLocalDirection3d); + +//class +class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalDirection3d,2,3,4> +{ + protected: + double min_vel_sq_norm_; // stored in squared norm for efficiency + int orthogonal_axis_; // 0: X, 1: Y, 2: Z + + public: + FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr, + const double& _min_vel_norm, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status = FAC_ACTIVE); + + ~FactorVelocityLocalDirection3d() override = default; + + template<typename T> + bool operator ()(const T* const _v, const T* const _o, T* _residuals) const; + + template<typename T> + Eigen::Matrix<T,2,1> computeElevationAzimuth(const Eigen::Matrix<T,3,1> vector) const + { + Eigen::Matrix<T,2,1> elev_azim; + + // plane YZ + if (orthogonal_axis_ == 0) + { + elev_azim(0) = asin(vector(0) / vector.norm()); + elev_azim(1) = atan2(vector(2), vector(1)); + } + // plane XZ + if (orthogonal_axis_ == 1) + { + elev_azim(0) = asin(vector(1) / vector.norm()); + elev_azim(1) = atan2(vector(0), vector(2)); + } + // plane XY + if (orthogonal_axis_ == 2) + { + elev_azim(0) = asin(vector(2) / vector.norm()); + elev_azim(1) = atan2(vector(1), vector(0)); + } + + return elev_azim; + } +}; + +FactorVelocityLocalDirection3d::FactorVelocityLocalDirection3d(FeatureBasePtr _ftr_ptr, + const double& _min_vel_norm, + const ProcessorBasePtr& _processor_ptr, + bool _apply_loss_function, + FactorStatus _status) : + FactorAutodiff<FactorVelocityLocalDirection3d,2,3,4>("FactorVelocityLocalDirection3d", + 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()); + MatrixSizeCheck<1,1>::check(_ftr_ptr->getMeasurementSquareRootInformationUpper()); + + /* Decide the plane (two axis A1, A2 from X, Y, Z in local coordinates) in which compute elevation and azimuth + * - elevation w.r.t. the plane + * - azimuth computed with atan2(A2, A1) + * + * This is done to avoid the degenerated case: elevation = +/-PI/2 + * We select the orthogonal axis as the farthest to the desired velocity in local coordinates, + * so the component one with lower value. + */ + + measurement_.minCoeff(&orthogonal_axis_); + + // measurement: elevation & azimuth (w.r.t. selected plane) + measurement_ = computeElevationAzimuth(Eigen::Vector3d(measurement_)); + measurement_sqrt_information_upper_ = Matrix2d::Identity() * measurement_sqrt_information_upper_(0,0); +} + +template<typename T> +inline bool FactorVelocityLocalDirection3d::operator ()(const T* const _v, const T* const _q, T* _residuals) const +{ + Eigen::Map<const Eigen::Matrix<T,3,1> > v(_v); + Eigen::Map<const Eigen::Quaternion<T> > q(_q); + Eigen::Map<Eigen::Matrix<T,2,1> > residuals(_residuals); + + if (v.squaredNorm() < min_vel_sq_norm_) + { + _residuals[0] = T(0); + return true; + } + +// std::cout << "----------\n"; +// std::cout << "desired elevation: " << getMeasurement()(0) << "\n"; +// std::cout << "desired azimuth: " << getMeasurement()(1) << "\n"; + +// std::cout << "v: \n\t" << v(0) << "\n\t" +// << v(1) << "\n\t" +// << v(2) << "\n"; +// +// std::cout << "q: \n\t" << q.coeffs()(0) << "\n\t" +// << q.coeffs()(1) << "\n\t" +// << q.coeffs()(2) << "\n\t" +// << q.coeffs()(3) << "\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"; + + // expectation + Eigen::Matrix<T,2,1> exp_elev_azim = computeElevationAzimuth(v_local); +// std::cout << "expected elevation: " << exp_elev_azim(0) << "\n"; +// std::cout << "expected azimuth: " << exp_elev_azim(1) << "\n"; + + // error + Eigen::Matrix<T,2,1> error = getMeasurement() - exp_elev_azim; + pi2pi(error(1)); +// std::cout << "error (corrected): \n\t" << error(0) << "\n\t" +// << error(1) << "\n; + + // residuals + residuals = getMeasurementSquareRootInformationUpper() * error; +// std::cout << "residuals: \n\t" << residuals(0) << "\n\t" +// << residuals(1) << "\n; + + return true; +} + +} // namespace wolf + +#endif diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 8dbbcdb5734782c3853f90158265dfafe07d5bea..a6305564d0a80a4de10f4eecb74c0ad29ff6249f 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -220,8 +220,6 @@ target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME}) # FactorVelocityLocalDirection3d class test wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp) target_link_libraries(gtest_factor_velocity_local_direction_3d ${PLUGIN_NAME}) -wolf_add_gtest(gtest_factor_velocity_local_direction_3d_b gtest_factor_velocity_local_direction_3d_b.cpp) -target_link_libraries(gtest_factor_velocity_local_direction_3d_b ${PLUGIN_NAME}) # MakePosDef function test wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp) diff --git a/test/gtest_factor_velocity_local_direction_3d.cpp b/test/gtest_factor_velocity_local_direction_3d.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0834d95071973788df1aa053baba80adf3e95bf4 --- /dev/null +++ b/test/gtest_factor_velocity_local_direction_3d.cpp @@ -0,0 +1,287 @@ +#include "core/utils/utils_gtest.h" + +#include "core/ceres_wrapper/solver_ceres.h" +#include "core/factor/factor_velocity_local_direction_3d.h" +#include "core/capture/capture_odom_2d.h" +#include "core/math/rotations.h" + + +using namespace Eigen; +using namespace wolf; + +int N_TESTS = 100; + +class FactorVelocityLocalDirection3dTest : public testing::Test +{ + protected: + ProblemPtr problem; + SolverManagerPtr solver; + FrameBasePtr frm; + StateBlockPtr sb_p; + StateBlockPtr sb_o; + StateBlockPtr sb_v; + CaptureBasePtr cap; + + int n_solved; + std::vector<double> convergence, iterations, times, error; + + virtual void SetUp() + { + // create problem + problem = Problem::create("POV", 3); + + // create solver + 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()); + sb_p = frm->getP(); + sb_p->fix(); + sb_o = frm->getO(); + sb_v = frm->getV(); + + // Capture + cap = CaptureBase::emplace<CaptureBase>(frm, "CaptureBase", + frm->getTimeStamp(), nullptr); + } + + FactorBasePtr emplaceFeatureAndFactor(const Vector3d& v_local, + const double& angle_stdev) + { + // emplace feature + FeatureBasePtr fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase", + v_local, + Matrix1d(angle_stdev * angle_stdev)); + + // emplace factor + return FactorBase::emplace<FactorVelocityLocalDirection3d>(fea, + fea, + 0, + nullptr, + false); + } + + bool testLocalVelocity(const Quaterniond& o, + const Vector3d& v_local, + bool estimate_o, + bool estimate_v) + { + assert(cap->getFeatureList().empty()); + + // 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->setState(Vector3d::Random()); + } + + // emplace feature & factor + auto fac = emplaceFeatureAndFactor(v_local, 0.01); + + // solve + std::string report = solver->solve(SolverManager::ReportVerbosity::FULL); + + // 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); + if (cos_angle_local > 1) + cos_angle_local = 1; + if (cos_angle_local < -1) + cos_angle_local = -1; + + // 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); + if (cos_angle_global > 1) + cos_angle_global = 1; + if (cos_angle_global < -1) + cos_angle_global = -1; + + // Check angle error + if (std::abs(acos(cos_angle_local)) > M_PI/180 or + std::abs(acos(cos_angle_global)) > M_PI/180) + { + WOLF_ERROR("\n\n!!!!!!!!!!!! ERROR TOO BIG iteration: ", iterations.size()); + WOLF_INFO("cos(angle local) = ", cos_angle_local); + WOLF_INFO("angle local = ", acos(cos_angle_local)); + WOLF_INFO("cos(angle global) = ", cos_angle_global); + WOLF_INFO("angle global = ", acos(cos_angle_global)); + + problem->print(4,1,1,1); + WOLF_INFO(report); + + // Reset + fac->getFeature()->remove(); + + return false; + } + + // Reset + fac->getFeature()->remove(); + + // Update performaces + convergence.push_back(solver->hasConverged() ? 1 : 0); + iterations.push_back(solver->iterations()); + times.push_back(solver->totalTime()); + error.push_back(acos(cos_angle_local)); + + return true; + } + + void printAverageResults() + { + WOLF_INFO("/////////////////// Average results: n_solved = ", iterations.size()); + double conv_avg = Eigen::Map<Eigen::VectorXd>(convergence.data(), convergence.size()).mean(); + double iter_avg = Eigen::Map<Eigen::VectorXd>(iterations.data(), iterations.size()).mean(); + double time_avg = Eigen::Map<Eigen::VectorXd>(times.data(), times.size()).mean(); + double err_avg = Eigen::Map<Eigen::VectorXd>(error.data(), error.size()).mean(); + + double iter_stdev = (Eigen::Map<Eigen::ArrayXd>(iterations.data(), iterations.size()) - iter_avg).matrix().norm(); + double time_stdev = (Eigen::Map<Eigen::ArrayXd>(times.data(), times.size()) - time_avg).matrix().norm(); + double err_stdev = (Eigen::Map<Eigen::ArrayXd>(error.data(), error.size()) - err_avg).matrix().norm(); + + 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); + } +}; + +// Input odometry data and covariance +Vector3d v_local(1.0, 0.0, 0.0); +double angle_stdev = 0.1; + +TEST_F(FactorVelocityLocalDirection3dTest, check_tree) +{ + ASSERT_TRUE(problem->check(0)); + + emplaceFeatureAndFactor(Vector3d(1.0, 0.0, 0.0), 0.1); + + ASSERT_TRUE(problem->check(0)); +} + +TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV) +{ + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), + Vector3d::Random().normalized(), + false, true)); + printAverageResults(); +} + +TEST_F(FactorVelocityLocalDirection3dTest, random_solveV) +{ + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), + Vector3d::Random().normalized(), + false, true)); + printAverageResults(); +} + +TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV_degenerated) +{ + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), + (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4, + false, true)); + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), + (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4, + false, true)); + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), + (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, + false, true)); + printAverageResults(); +} + +TEST_F(FactorVelocityLocalDirection3dTest, random_solveV_degenerated) +{ + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), + (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, + false, true)); + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), + (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4, + false, true)); + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), + (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, + false, true)); + printAverageResults(); +} + +TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO) +{ + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), + Vector3d::Random().normalized(), + true, false)); + printAverageResults(); +} + +TEST_F(FactorVelocityLocalDirection3dTest, random_solveO) +{ + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), + Vector3d::Random().normalized(), + true, false)); + printAverageResults(); +} + +TEST_F(FactorVelocityLocalDirection3dTest, origin_solveO_degenerated) +{ + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), + (Vector3d() << 1, 0, 0).finished() + Vector3d::Random() * 1e-4, + true, false)); + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), + (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4, + true, false)); + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond::Identity(), + (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, + true, false)); + printAverageResults(); +} + +TEST_F(FactorVelocityLocalDirection3dTest, random_solveO_degenerated) +{ + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), + (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, + true, false)); + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), + (Vector3d() << 0, 1, 0).finished() + Vector3d::Random() * 1e-4, + true, false)); + for (auto i = 0; i < N_TESTS; i++) + ASSERT_TRUE(testLocalVelocity(Quaterniond(Vector4d::Random().normalized()), + (Vector3d() << 0, 0, 1).finished() + Vector3d::Random() * 1e-4, + true, false)); + printAverageResults(); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}