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

Resolve "Processor motion model"

parent c268927d
No related branches found
No related tags found
1 merge request!419Resolve "Processor motion model"
...@@ -229,6 +229,7 @@ SET(HDRS_FACTOR ...@@ -229,6 +229,7 @@ SET(HDRS_FACTOR
include/core/factor/factor_relative_pose_2d_with_extrinsics.h include/core/factor/factor_relative_pose_2d_with_extrinsics.h
include/core/factor/factor_relative_pose_3d.h include/core/factor/factor_relative_pose_3d.h
include/core/factor/factor_pose_3d_with_extrinsics.h include/core/factor/factor_pose_3d_with_extrinsics.h
include/core/factor/factor_velocity_local_direction_3d.h
) )
SET(HDRS_FEATURE SET(HDRS_FEATURE
include/core/feature/feature_base.h include/core/feature/feature_base.h
...@@ -246,6 +247,7 @@ SET(HDRS_PROCESSOR ...@@ -246,6 +247,7 @@ SET(HDRS_PROCESSOR
include/core/processor/motion_buffer.h include/core/processor/motion_buffer.h
include/core/processor/processor_base.h include/core/processor/processor_base.h
include/core/processor/processor_diff_drive.h include/core/processor/processor_diff_drive.h
include/core/processor/processor_fix_wing_model.h
include/core/processor/factory_processor.h include/core/processor/factory_processor.h
include/core/processor/processor_logging.h include/core/processor/processor_logging.h
include/core/processor/processor_loop_closure.h include/core/processor/processor_loop_closure.h
...@@ -345,6 +347,7 @@ SET(SRCS_PROCESSOR ...@@ -345,6 +347,7 @@ SET(SRCS_PROCESSOR
src/processor/motion_buffer.cpp src/processor/motion_buffer.cpp
src/processor/processor_base.cpp src/processor/processor_base.cpp
src/processor/processor_diff_drive.cpp src/processor/processor_diff_drive.cpp
src/processor/processor_fix_wing_model.cpp
src/processor/processor_loop_closure.cpp src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp src/processor/processor_odom_2d.cpp
......
...@@ -119,6 +119,8 @@ class SolverCeres : public SolverManager ...@@ -119,6 +119,8 @@ class SolverCeres : public SolverManager
double finalCost() override; double finalCost() override;
double totalTime() override;
ceres::Solver::Options& getSolverOptions(); ceres::Solver::Options& getSolverOptions();
const Eigen::SparseMatrixd computeHessian() const; const Eigen::SparseMatrixd computeHessian() const;
......
...@@ -93,6 +93,8 @@ inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_cur ...@@ -93,6 +93,8 @@ inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_cur
_frame_past_ptr->getP(), // past frame P _frame_past_ptr->getP(), // past frame P
_frame_past_ptr->getO()) // past frame Q _frame_past_ptr->getO()) // past frame Q
{ {
MatrixSizeCheck<7,1>::check(_ftr_current_ptr->getMeasurement());
MatrixSizeCheck<6,6>::check(_ftr_current_ptr->getMeasurementSquareRootInformationUpper());
} }
template<typename T> template<typename T>
......
#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
/*
* processor_fix_wing_model.h
*
* Created on: Sep 6, 2021
* Author: joanvallve
*/
#ifndef INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
#define INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_
#include "core/processor/processor_base.h"
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorFixWingModel);
struct ParamsProcessorFixWingModel : public ParamsProcessorBase
{
Eigen::Vector3d velocity_local;
double angle_stdev;
double min_vel_norm;
ParamsProcessorFixWingModel() = default;
ParamsProcessorFixWingModel(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsProcessorBase(_unique_name, _server)
{
velocity_local = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/velocity_local");
angle_stdev = _server.getParam<double> (prefix + _unique_name + "/angle_stdev");
min_vel_norm = _server.getParam<double> (prefix + _unique_name + "/min_vel_norm");
assert(std::abs(velocity_local.norm() - 1.0) < wolf::Constants::EPS && "ParamsProcessorFixWingModel: 'velocity_local' must be normalized");
}
std::string print() const override
{
return ParamsProcessorBase::print() + "\n"
+ "velocity_local: print not implemented\n"
+ "angle_stdev: " + std::to_string(angle_stdev) + "\n"
+ "min_vel_norm: " + std::to_string(min_vel_norm) + "\n";
}
};
WOLF_PTR_TYPEDEFS(ProcessorFixWingModel);
class ProcessorFixWingModel : public ProcessorBase
{
public:
ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params);
// Factory method for high level API
WOLF_PROCESSOR_CREATE(ProcessorFixWingModel, ParamsProcessorFixWingModel);
virtual ~ProcessorFixWingModel() override;
void configure(SensorBasePtr _sensor) override;
protected:
/** \brief process an incoming capture NEVER CALLED
*/
virtual void processCapture(CaptureBasePtr) override {};
/** \brief process an incoming key-frame: applies the motion model between consecutive keyframes
*/
virtual void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
/** \brief trigger in capture
*/
virtual bool triggerInCapture(CaptureBasePtr) const override {return false;};
/** \brief trigger in key-frame
*/
virtual bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) const override {return true;};
/** \brief store key frame
*/
virtual bool storeKeyFrame(FrameBasePtr) override {return false;};
/** \brief store capture
*/
virtual bool storeCapture(CaptureBasePtr) override {return false;};
/** \brief Vote for KeyFrame generation
*/
virtual bool voteForKeyFrame() const override {return false;};
// ATTRIBUTES
ParamsProcessorFixWingModelPtr params_processor_;
};
} /* namespace wolf */
#endif /* INCLUDE_CORE_PROCESSOR_PROCESSOR_FIX_WING_MODEL_H_ */
...@@ -131,6 +131,8 @@ class SolverManager ...@@ -131,6 +131,8 @@ class SolverManager
virtual double finalCost() = 0; virtual double finalCost() = 0;
virtual double totalTime() = 0;
/** /**
* \brief Updates solver's problem according to the wolf_problem * \brief Updates solver's problem according to the wolf_problem
*/ */
......
...@@ -612,6 +612,11 @@ double SolverCeres::finalCost() ...@@ -612,6 +612,11 @@ double SolverCeres::finalCost()
return double(summary_.final_cost); return double(summary_.final_cost);
} }
double SolverCeres::totalTime()
{
return double(summary_.total_time_in_seconds);
}
ceres::CostFunctionPtr SolverCeres::createCostFunction(const FactorBasePtr& _fac_ptr) ceres::CostFunctionPtr SolverCeres::createCostFunction(const FactorBasePtr& _fac_ptr)
{ {
assert(_fac_ptr != nullptr); assert(_fac_ptr != nullptr);
......
/*
* processor_fix_wing_model.cpp
*
* Created on: Sep 6, 2021
* Author: joanvallve
*/
#include "core/processor/processor_fix_wing_model.h"
#include "core/capture/capture_base.h"
#include "core/feature/feature_base.h"
#include "core/factor/factor_velocity_local_direction_3d.h"
namespace wolf
{
ProcessorFixWingModel::ProcessorFixWingModel(ParamsProcessorFixWingModelPtr _params) :
ProcessorBase("ProcessorFixWingModel", 3, _params),
params_processor_(_params)
{
}
ProcessorFixWingModel::~ProcessorFixWingModel()
{
}
void ProcessorFixWingModel::configure(SensorBasePtr _sensor)
{
assert(_sensor->getProblem()->getFrameStructure().find('V') != std::string::npos && "Processor only works with problems with V");
}
void ProcessorFixWingModel::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
{
if (_keyframe_ptr->getV()->isFixed())
return;
if (_keyframe_ptr->getFactorOf(shared_from_this()) != nullptr)
return;
// emplace capture
auto cap = CaptureBase::emplace<CaptureBase>(_keyframe_ptr, "CaptureBase",
_keyframe_ptr->getTimeStamp(), getSensor());
// emplace feature
auto fea = FeatureBase::emplace<FeatureBase>(cap, "FeatureBase",
params_processor_->velocity_local,
Eigen::Matrix1d(params_processor_->angle_stdev * params_processor_->angle_stdev));
// emplace factor
auto fac = FactorBase::emplace<FactorVelocityLocalDirection3d>(fea,
fea,
params_processor_->min_vel_norm,
shared_from_this(),
false);
}
} /* namespace wolf */
// Register in the FactoryProcessor
#include "core/processor/factory_processor.h"
namespace wolf {
WOLF_REGISTER_PROCESSOR(ProcessorFixWingModel);
WOLF_REGISTER_PROCESSOR_AUTO(ProcessorFixWingModel);
} // namespace wolf
...@@ -217,6 +217,10 @@ target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAM ...@@ -217,6 +217,10 @@ target_link_libraries(gtest_factor_relative_pose_2d_with_extrinsics ${PLUGIN_NAM
wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp) wolf_add_gtest(gtest_factor_relative_pose_3d gtest_factor_relative_pose_3d.cpp)
target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME}) 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})
# 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)
target_link_libraries(gtest_make_posdef ${PLUGIN_NAME}) target_link_libraries(gtest_make_posdef ${PLUGIN_NAME})
...@@ -230,6 +234,10 @@ wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp) ...@@ -230,6 +234,10 @@ wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
target_link_libraries(gtest_param_prior ${PLUGIN_NAME}) target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
# ProcessorDiffDriveSelfcalib class test # ProcessorDiffDriveSelfcalib class test
wolf_add_gtest(gtest_processor_fix_wing_model gtest_processor_fix_wing_model.cpp)
target_link_libraries(gtest_processor_fix_wing_model ${PLUGIN_NAME})
# ProcessorFixWingModel class test
wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp) wolf_add_gtest(gtest_processor_diff_drive gtest_processor_diff_drive.cpp)
target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME}) target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
......
...@@ -60,6 +60,7 @@ class SolverManagerDummy : public SolverManager ...@@ -60,6 +60,7 @@ class SolverManagerDummy : public SolverManager
SizeStd iterations() override { return 1; } SizeStd iterations() override { return 1; }
double initialCost() override { return double(1); } double initialCost() override { return double(1); }
double finalCost() override { return double(0); } double finalCost() override { return double(0); }
double totalTime() override { return double(0); }
protected: protected:
......
#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();
}
#include "core/utils/utils_gtest.h"
#include "core/problem/problem.h"
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/processor/processor_fix_wing_model.h"
#include "core/state_block/state_quaternion.h"
// STL
#include <iterator>
#include <iostream>
using namespace wolf;
using namespace Eigen;
class ProcessorFixWingModelTest : public testing::Test
{
protected:
ProblemPtr problem;
SolverManagerPtr solver;
SensorBasePtr sensor;
ProcessorBasePtr processor;
virtual void SetUp()
{
// create problem
problem = Problem::create("POV", 3);
// create solver
solver = std::make_shared<SolverCeres>(problem);
// Emplace sensor
sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SensorBase",
std::make_shared<StateBlock>(Vector3d::Zero()),
std::make_shared<StateQuaternion>((Vector4d() << 0,0,0,1).finished()),
nullptr,
2);
// Emplace processor
ParamsProcessorFixWingModelPtr params = std::make_shared<ParamsProcessorFixWingModel>();
params->velocity_local = (Vector3d() << 1, 0, 0).finished();
params->angle_stdev = 0.1;
params->min_vel_norm = 0;
processor = ProcessorBase::emplace<ProcessorFixWingModel>(sensor, params);
}
FrameBasePtr emplaceFrame(TimeStamp ts, const Vector10d& x)
{
// new frame
return problem->emplaceFrame(ts, x);
}
};
TEST_F(ProcessorFixWingModelTest, setup)
{
EXPECT_TRUE(problem->check());
}
TEST_F(ProcessorFixWingModelTest, keyFrameCallback)
{
// new frame
auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
// keyframecallback
problem->keyFrameCallback(frm1, nullptr, 0.5);
// check one capture
ASSERT_EQ(frm1->getCapturesOf(sensor).size(), 1);
auto cap = frm1->getCaptureOf(sensor);
ASSERT_TRUE(cap != nullptr);
// check one feature
ASSERT_EQ(cap->getFeatureList().size(), 1);
// check one factor
auto fac = frm1->getFactorOf(processor, "FactorVelocityLocalDirection3d");
ASSERT_TRUE(fac != nullptr);
ASSERT_TRUE(fac->getFeature() != nullptr);
ASSERT_TRUE(fac->getCapture() == cap);
}
TEST_F(ProcessorFixWingModelTest, keyFrameCallbackRepeated)
{
// new frame
auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
// keyframecallback
problem->keyFrameCallback(frm1, nullptr, 0.5);
// repeated keyframecallback
problem->keyFrameCallback(frm1, nullptr, 0.5);
// check one capture
ASSERT_EQ(frm1->getCapturesOf(sensor).size(), 1);
auto cap = frm1->getCaptureOf(sensor);
ASSERT_TRUE(cap != nullptr);
// check one feature
ASSERT_EQ(cap->getFeatureList().size(), 1);
// check one factor
auto fac = frm1->getFactorOf(processor, "FactorVelocityLocalDirection3d");
ASSERT_TRUE(fac != nullptr);
ASSERT_TRUE(fac->getFeature() != nullptr);
ASSERT_TRUE(fac->getCapture() == cap);
}
TEST_F(ProcessorFixWingModelTest, solve_origin)
{
// new frame
auto frm1 = emplaceFrame(1, (Vector10d() << 0,0,0,0,0,0,1,1,0,0).finished());
// keyframecallback
problem->keyFrameCallback(frm1, nullptr, 0.5);
// perturb
frm1->getP()->fix();
frm1->getO()->fix();
frm1->getV()->unfix();
frm1->getV()->setState(Vector3d::Random());
// solve
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
WOLF_INFO(report);
ASSERT_GT(frm1->getV()->getState()(0), 0);
ASSERT_NEAR(frm1->getV()->getState()(1), 0, Constants::EPS);
ASSERT_NEAR(frm1->getV()->getState()(2), 0, Constants::EPS);
}
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