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

new implementation computing azimuth and elevation

parent 64bee3fe
No related branches found
No related tags found
1 merge request!419Resolve "Processor motion model"
Pipeline #6749 passed
#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
...@@ -220,8 +220,6 @@ target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME}) ...@@ -220,8 +220,6 @@ 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;
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();
}
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