diff --git a/include/core/factor/factor_velocity_local_direction_3d.h b/include/core/factor/factor_velocity_local_direction_3d.h
deleted file mode 100644
index 31bf83599189cda1033ac38ea43dd36a9cb99500..0000000000000000000000000000000000000000
--- a/include/core/factor/factor_velocity_local_direction_3d.h
+++ /dev/null
@@ -1,142 +0,0 @@
-
-#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"
-
-//#include "ceres/jet.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(FactorVelocityLocalDirection3d);
-
-//class
-class FactorVelocityLocalDirection3d: public FactorAutodiff<FactorVelocityLocalDirection3d,1,3,4>
-{
- 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,
- _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),
- residual_local_(_residual_local),
- residual_angle_(_residual_angle)
- {
- MatrixSizeCheck<3,1>::check(_ftr_ptr->getMeasurement());
- MatrixSizeCheck<1,1>::check(_ftr_ptr->getMeasurementSquareRootInformationUpper());
- assert(std::abs(_ftr_ptr->getMeasurement().norm() - 1.0) < wolf::Constants::EPS && "velocity direction measurement must be normalized");
- }
-
- ~FactorVelocityLocalDirection3d() override = default;
-
- template<typename T>
- bool operator ()(const T* const _v, const T* const _o, T* _residuals) const;
-};
-
-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);
-
- if (v.squaredNorm() < min_vel_sq_norm_)
- {
- _residuals[0] = T(0);
- return true;
- }
-
-// std::cout << "----------\n";
-// std::cout << "v desired (local): \n\t" << getMeasurement()(0) << "\n\t"
-// << getMeasurement()(1) << "\n\t"
-// << getMeasurement()(2) << "\n";
-//
-// 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";
-//
-// 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: \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";
-//
-// 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
-
- // 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.9);
- }
-
- // 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";
- _residuals[0] = T(0);
- return true;
- }
-
- // residual
- _residuals[0] = getMeasurementSquareRootInformationUpper()(0,0) * acos(cos_angle);
-
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index a6305564d0a80a4de10f4eecb74c0ad29ff6249f..8dbbcdb5734782c3853f90158265dfafe07d5bea 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -220,6 +220,8 @@ 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
deleted file mode 100644
index 37fa70557ea733d149f0f356fa145699460fdba4..0000000000000000000000000000000000000000
--- a/test/gtest_factor_velocity_local_direction_3d.cpp
+++ /dev/null
@@ -1,415 +0,0 @@
-#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;
-
-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_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
- 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);
-
- // 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,
- bool residual_local,
- bool residual_angle)
- {
- // 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,
- residual_local,
- residual_angle);
- }
-
- void testLocalVelocity(const Quaterniond& o,
- const Vector3d& v_local,
- bool estimate_o,
- bool estimate_v)
- {
- assert(n_solved <= n_tests);
-
- // 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());
- 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++;
- }
-
- 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);
- }
- }
-};
-
-// 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,true,true);
-
- ASSERT_TRUE(problem->check(0));
- ASSERT_DEATH({emplaceFeatureAndFactor(Vector3d(10.0, 0.0, 0.0), 0.1,true,true);},""); // Not normalized
-}
-
-TEST_F(FactorVelocityLocalDirection3dTest, origin_solveV)
-{
- for (auto i = 0; i < n_tests; i++)
- testLocalVelocity(Quaterniond::Identity(),
- Vector3d::Random().normalized(),
- false, true);
- printAverageResults();
-}
-
-TEST_F(FactorVelocityLocalDirection3dTest, random_solveV)
-{
- for (auto i = 0; i < n_tests; i++)
- testLocalVelocity(Quaterniond(Vector4d::Random().normalized()),
- Vector3d::Random().normalized(),
- false, true);
- printAverageResults();
-}
-
-/*TEST_F(FactorVelocityLocalDirection3dTest, xturned_solveV_x)
-{
- testLocalVelocity(Quaterniond((Vector4d() << 1, 1, 0, 0).finished().normalized()),
- (Vector3d() << 1, 0, 0).finished(),
- false, true);
-}
-
-TEST_F(FactorVelocityLocalDirection3dTest, xturned_solveV_y)
-{
- testLocalVelocity(Quaterniond((Vector4d() << 1, 1, 0, 0).finished().normalized()),
- (Vector3d() << 0, 1, 0).finished(),
- false, true);
-}
-
-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++)
-// {
-// // Random delta
-// Vector3d delta = Vector3d::Random() * 10;
-// pi2pi(delta(2));
-//
-// // Random initial pose
-// Vector3d x0 = Vector3d::Random() * 10;
-// pi2pi(x0(2));
-//
-// // x1 groundtruth
-// Vector3d x1;
-// x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
-// x1(2) = pi2pi(x0(2) + delta(2));
-//
-// // Set states and measurement
-// frm0->setState(x0);
-// frm1->setState(x1);
-// cap1->setData(delta);
-//
-// // feature & factor with delta measurement
-// auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
-// FactorBase::emplace<FactorVelocityLocalDirection3d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
-//
-// // Fix frm0, perturb frm1
-// frm0->fix();
-// frm1->unfix();
-// frm1->perturb(5);
-//
-// // solve for frm1
-// std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-//
-// ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
-//
-// // remove feature (and factor) for the next loop
-// fea1->remove();
-// }
-//}
-//
-//TEST_F(FactorVelocityLocalDirection3dTest, fix_1_solve)
-//{
-// for (int i = 0; i < 1e3; i++)
-// {
-// // Random delta
-// Vector3d delta = Vector3d::Random() * 10;
-// pi2pi(delta(2));
-//
-// // Random initial pose
-// Vector3d x0 = Vector3d::Random() * 10;
-// pi2pi(x0(2));
-//
-// // x1 groundtruth
-// Vector3d x1;
-// x1.head<2>() = x0.head<2>() + Rotation2Dd(x0(2)) * delta.head<2>();
-// x1(2) = pi2pi(x0(2) + delta(2));
-//
-// // Set states and measurement
-// frm0->setState(x0);
-// frm1->setState(x1);
-// cap1->setData(delta);
-//
-// // feature & factor with delta measurement
-// auto fea1 = FeatureBase::emplace<FeatureBase>(cap1, "FeatureOdom2d", delta, data_cov);
-// FactorBase::emplace<FactorVelocityLocalDirection3d>(fea1, fea1, frm0, nullptr, false, TOP_MOTION);
-//
-// // Fix frm1, perturb frm0
-// frm1->fix();
-// frm0->unfix();
-// frm0->perturb(5);
-//
-// // solve for frm0
-// std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-//
-// ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
-//
-// // remove feature (and factor) for the next loop
-// fea1->remove();
-// }
-//}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}