diff --git a/CMakeLists.txt b/CMakeLists.txt
index c6c2db62aa3d682dd1c78bdfbb696b837fa07120..599d7d8773e383fe7855a47e026d73743cf0e1fe 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -229,6 +229,7 @@ SET(HDRS_FACTOR
include/core/factor/factor_relative_pose_2d_with_extrinsics.h
include/core/factor/factor_relative_pose_3d.h
include/core/factor/factor_pose_3d_with_extrinsics.h
+ include/core/factor/factor_velocity_local_direction_3d.h
)
SET(HDRS_FEATURE
include/core/feature/feature_base.h
@@ -246,6 +247,7 @@ SET(HDRS_PROCESSOR
include/core/processor/motion_buffer.h
include/core/processor/processor_base.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/processor_logging.h
include/core/processor/processor_loop_closure.h
@@ -345,6 +347,7 @@ SET(SRCS_PROCESSOR
src/processor/motion_buffer.cpp
src/processor/processor_base.cpp
src/processor/processor_diff_drive.cpp
+ src/processor/processor_fix_wing_model.cpp
src/processor/processor_loop_closure.cpp
src/processor/processor_motion.cpp
src/processor/processor_odom_2d.cpp
diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index 0673207bc47e1873084e3f4d711628a67b322f3c..4bf6f7de661059e7c9f96e7f12bb444f18bed013 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -119,6 +119,8 @@ class SolverCeres : public SolverManager
double finalCost() override;
+ double totalTime() override;
+
ceres::Solver::Options& getSolverOptions();
const Eigen::SparseMatrixd computeHessian() const;
diff --git a/include/core/factor/factor_relative_pose_3d.h b/include/core/factor/factor_relative_pose_3d.h
index 67d98f7e26d03e360c0a5d846b7d84b54012047b..afcf8daf6884c9c08eb800433fa1f2392a8ecd0c 100644
--- a/include/core/factor/factor_relative_pose_3d.h
+++ b/include/core/factor/factor_relative_pose_3d.h
@@ -93,6 +93,8 @@ inline FactorRelativePose3d::FactorRelativePose3d(const FeatureBasePtr& _ftr_cur
_frame_past_ptr->getP(), // past frame P
_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>
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/include/core/processor/processor_fix_wing_model.h b/include/core/processor/processor_fix_wing_model.h
new file mode 100644
index 0000000000000000000000000000000000000000..8c23917354ca1d0e0c3723045ded8d118e3c3abc
--- /dev/null
+++ b/include/core/processor/processor_fix_wing_model.h
@@ -0,0 +1,92 @@
+/*
+ * 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_ */
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 58d21a5cf834cc18372a197b3bbbead9894e46f6..f3fbeefb3dce65e58ad82d3c604ba3e554169624 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -131,6 +131,8 @@ class SolverManager
virtual double finalCost() = 0;
+ virtual double totalTime() = 0;
+
/**
* \brief Updates solver's problem according to the wolf_problem
*/
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index a2d7616c58181be35b9aae642e0e8e9f956b4879..5914b40a8bc67e44bbc392b146766331e344ced7 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -612,6 +612,11 @@ double SolverCeres::finalCost()
return double(summary_.final_cost);
}
+double SolverCeres::totalTime()
+{
+ return double(summary_.total_time_in_seconds);
+}
+
ceres::CostFunctionPtr SolverCeres::createCostFunction(const FactorBasePtr& _fac_ptr)
{
assert(_fac_ptr != nullptr);
diff --git a/src/processor/processor_fix_wing_model.cpp b/src/processor/processor_fix_wing_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7c12e90f8f08005d2f0086d57ad1633534dbe8ea
--- /dev/null
+++ b/src/processor/processor_fix_wing_model.cpp
@@ -0,0 +1,66 @@
+/*
+ * 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
+
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 4e03c1a2a284f02b2fe26869ad656e5c959a450a..46964ac57b3430b8113edbb7ee23d4923fc0d50b 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -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)
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
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
target_link_libraries(gtest_make_posdef ${PLUGIN_NAME})
@@ -230,6 +234,10 @@ wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
target_link_libraries(gtest_param_prior ${PLUGIN_NAME})
# 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)
target_link_libraries(gtest_processor_diff_drive ${PLUGIN_NAME})
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index aab56cd1ab532a6a7d3e4b876f2de6dea0ea1b7b..cf45c71c94167f934504e5f97f91080d4d011ce8 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_manager_dummy.h
@@ -60,6 +60,7 @@ class SolverManagerDummy : public SolverManager
SizeStd iterations() override { return 1; }
double initialCost() override { return double(1); }
double finalCost() override { return double(0); }
+ double totalTime() override { return double(0); }
protected:
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();
+}
diff --git a/test/gtest_processor_fix_wing_model.cpp b/test/gtest_processor_fix_wing_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e817a9c400c53cdd569c72de90c672ac703fdaf4
--- /dev/null
+++ b/test/gtest_processor_fix_wing_model.cpp
@@ -0,0 +1,136 @@
+
+#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();
+}