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

removed old implementation using dot product

parent 1dd7523f
No related branches found
No related tags found
1 merge request!419Resolve "Processor motion model"
#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
...@@ -220,6 +220,8 @@ target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME}) ...@@ -220,6 +220,8 @@ target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME})
# FactorVelocityLocalDirection3d class test # FactorVelocityLocalDirection3d class test
wolf_add_gtest(gtest_factor_velocity_local_direction_3d gtest_factor_velocity_local_direction_3d.cpp) 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}) 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 # MakePosDef function test
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp) wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
......
#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();
}
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